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 check_local_position(self, current):
if self.compare_points(current, self.desired_point):
try:
self.land_point = CommandTOLRequest()
self.land_point.yaw = 0
self.land_point.latitude = self.desired_point.pose.position.x
self.land_point.longitude = self.desired_point.pose.position.y
self.land_point.altitude = self.desired_point.pose.position.z
self.desired_point = next(self.points_iter)
except StopIteration:
self.finish_mission = True
def land(self):
"""The function changes the mode of the drone to LAND.
Returns:
0 (int): LAND successful
-1 (int): LAND unsuccessful.
"""
srv_land = CommandTOLRequest(0, 0, 0, 0, 0)
response = self.land_client(srv_land)
if response.success:
rospy.loginfo(CGREEN2 +
"Land Sent {}".format(str(response.success)) + CEND)
return 0
else:
rospy.logerr(CRED2 + "Landing failed" + CEND)
return -1
def takeoff(self, takeoff_alt):
"""The takeoff function will arm the drone and put the drone in a hover above the initial position.
Args:
takeoff_alt (Float): The altitude at which the drone should hover.
Returns:
0 (int): Takeoff successful.
-1 (int): Takeoff unsuccessful.
"""
self.arm()
takeoff_srv = CommandTOLRequest(0, 0, 0, 0, takeoff_alt)
response = self.takeoff_client(takeoff_srv)
rospy.sleep(3)
if response.success:
rospy.loginfo(CGREEN2 + "Takeoff successful" + CEND)
return 0
else:
rospy.logerr(CRED2 + "Takeoff failed" + CEND)
return -1
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)
