control.py 文件源码

def _return_home(self):
        print(self.namespace, 'land')
        req = CommandTOLRequest()
        req.min_pitch = 0.0
        req.yaw = -math.pi if self.color == 'blue' else 0.0
        req.latitude = self.start_position.latitude
        req.longitude = self.start_position.longitude
        req.altitude = self.start_position.altitude

        service_name = '%s/mavros/cmd/land' % self.namespace
        rospy.wait_for_service(service_name)
        try:
            service = rospy.ServiceProxy(service_name, CommandTOL)
            resp = service.call(req)
        except rospy.ServiceException as e:
            print(self.namespace, 'service call to land failed:', str(e),
                  file=sys.stderr)
            return False
        if not resp.success:
            print(self.namespace, 'failed to land', file=sys.stderr)
            return False
        print(self.namespace, 'landing')
        self._set_state(STATE_LANDING)
        return True