def takeoff(client, altitude = 3):
global current_pose
req = CommandTOLRequest()
req.latitude = current_pose.latitude
req.longitude = current_pose.longitude
req.altitude = altitude
client(req)
def land(self):
self.setpoint_raw_timer.shutdown()
self.stay_armed_stay_offboard_timer.shutdown()
req = CommandTOLRequest()
req.latitude = self.global_position.latitude
req.longitude = self.global_position.longitude
self.land_client(req)
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
def land(self):
if not self.current_state.armed:
print "not armed yet"
else:
# wait for service
rospy.wait_for_service("mavros/cmd/land")
# service client
set_rq = rospy.ServiceProxy("mavros/cmd/land", CommandTOL)
# set request object
req = CommandTOLRequest()
req.yaw = 0.0
req.latitude = 0.0
req.longitude = 0.0
req.altitude = 0.0
req.min_pitch = 0.0
#zero time
t0 = rospy.get_time()
# check response
while self.current_state.armed:
if rospy.get_time() - t0 > 5.0: # check every 5 seconds
try:
# request
set_rq.call(req)
except rospy.ServiceException, e:
print "Service did not process request: %s" % str(e)
t0 = rospy.get_time()
print "landed savely"
def run(self):
parser = argparse.ArgumentParser(
description='Swarm control',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
commands = ['arm', 'disarm', 'land', 'offboard', 'takeoff']
parser.add_argument('command', choices=commands, help='Command')
parser.add_argument('agent',
type=int,
nargs='?',
default=None,
help='Agent ID')
args = parser.parse_args()
master = masterapi.Master('/')
system_state = master.getSystemState()
self.services = [service[0] for service in system_state[2]]
self.topics = [t for t, _ in rospy.get_published_topics()]
if args.command == 'arm':
self.call_service('mavros/cmd/arming',
service_class=CommandBool,
request=CommandBoolRequest(True),
agent=args.agent,
function=self._are_agents_on_ground)
elif args.command == 'disarm':
self.call_service('mavros/cmd/arming',
service_class=CommandBool,
request=CommandBoolRequest(False),
agent=args.agent,
function=self._are_agents_on_ground)
elif args.command == 'land':
self.call_service('mavros/cmd/land',
service_class=CommandTOL,
request=CommandTOLRequest(),
agent=args.agent)
elif args.command == 'offboard':
self.call_service('mavros/set_mode',
service_class=SetMode,
request=SetModeRequest(custom_mode='OFFBOARD'),
agent=args.agent)
elif args.command == 'takeoff':
# Takeoff is a combination of takeoff and arming!
n = rospy.get_param('/gcs/n')
altitude = rospy.get_param('/gcs/origin_altitude')
agents = [args.agent] if args.agent is not None else list(
range(1, n + 1))
for agent in agents:
template = '/drone_{}/mavros/home_position/home'
home_position = rospy.wait_for_message(template.format(agent),
HomePosition)
request = CommandTOLRequest()
request.latitude = home_position.geo.latitude
request.longitude = home_position.geo.longitude
request.altitude = altitude
request.yaw = 0.5 * np.pi
self.call_service('mavros/cmd/takeoff',
service_class=CommandTOL,
request=request,
agent=agent)
self.call_service('mavros/cmd/arming',
service_class=CommandBool,
request=CommandBoolRequest(True),
agent=args.agent,
function=self._are_agents_on_ground)