def build_waypoints(path):
assert len(path) > 0, 'The path is empty !'
waypoints = []
for lat, lon, alt in path:
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL_REL_ALT
wp.command = CommandCode.NAV_WAYPOINT
wp.is_current = False
wp.autocontinue = True
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = alt
waypoints.append(wp)
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL_REL_ALT
wp.command = CommandCode.NAV_LAND
wp.is_current = False
wp.autocontinue = True
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = alt
waypoints.append(wp)
waypoints[0].command = CommandCode.NAV_TAKEOFF
waypoints[0].is_current = True
return waypoints
def test_wp():
wp1 = Waypoint()
wp2 = Waypoint()
wp3 = Waypoint()
wp4 = Waypoint()
wp5 = Waypoint()
mission = []
wp1.command = 16
wp2.command = 16
wp3.command = 16
wp4.command = 16
wp5.command = 16
wp4.x_lat = 42.168019
wp4.y_long = -88.54291
wp4.z_alt = 4
mission.append(wp4)
wp1.x_lat = 42.168019
wp1.y_long = -88.537836
wp1.z_alt = 4
mission.append(wp1)
wp2.x_lat = 42.161040
wp2.y_long = -88.537836
wp2.z_alt = 4
mission.append(wp2)
wp3.x_lat = 42.161040
wp3.y_long = -88.54291
wp3.z_alt = 4
mission.append(wp3)
wp5.x_lat = 42.168019
wp5.y_long = -88.54291
wp5.z_alt = 4
mission.append(wp5)
self.send_mission(mission)
def read_json(self, filename):
with open(filename) as json_data:
data_dict = json.load(json_data)
waypoint_list = WaypointList()
wp = Waypoint()
wp.command = CommandCode.NAV_TAKEOFF
waypoint_list.waypoints.append(wp)
wp.is_current = True
c = 0
for phase in (data_dict["phases"]):
wp = Waypoint()
wp.is_current = 0
wp.frame = 3
if phase['type'] == 'Waypoint':
wp.param1 = 0
wp.command = CommandCode.NAV_WAYPOINT
wp.autocontinue = True
wp.x_lat = phase['point']['latitude']
wp.y_long = phase['point']['longitude']
wp.z_alt = -phase['depth']
waypoint_list.waypoints.append(wp)
if phase['type'] == 'Path':
for track in phase['tracks']:
wp.param1 = 0
wp.command = CommandCode.NAV_WAYPOINT
wp.autocontinue = True
wp.x_lat = track['start_point']['latitude']
wp.y_long = track['start_point']['longitude']
wp.z_alt = track['altitude']
waypoint_list.waypoints.append(wp)
wp.command = CommandCode.NAV_SPLINE_WAYPOINT
wp.autocontinue = True
wp.x_lat = track['end_point']['latitude']
wp.y_long = track['end_point']['longitude']
wp.z_alt = track['altitude']
waypoint_list.waypoints.append(wp)
if phase['type'] == 'Dive':
wp.command = CommandCode.NAV_CONTINUE_AND_CHANGE_ALT
wp.autocontinue = True
wp.param1 = 2 # to dive and 1 to climb
wp.z_alt = -phase['depth']
waypoint_list.waypoints.append(wp)
if phase['type'] == 'Ascent':
wp.command = CommandCode.NAV_CONTINUE_AND_CHANGE_ALT
wp.autocontinue = True
wp.param1 = 1
wp.z_alt = 0
waypoint_list.waypoints.append(wp)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.waypoint_pub.publish(waypoint_list)
rate.sleep()
def mission_create (self):
param = rospy.get_param('~waypoints')
rospy.loginfo('Waypoints from parameter server: %s', param)
# rospy.logdebug(param)
wl = WaypointList()
wp = Waypoint()
wp.frame = self.MAV_GLOBAL_FRAME
wp.command = self.MAV_CMD_TAKEOFF # takeoff
wp.is_current = True
wp.autocontinue = True
wp.param1 = param[0]['alt'] # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = 0
wp.y_long = 0
wp.z_alt = param[0]['alt']
wl.waypoints.append(wp)
for index in range(len(param)):
wp = Waypoint()
wp.frame = self.MAV_GLOBAL_FRAME
wp.command = self.MAV_CMD_WAYPOINT # simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = param[index]['lat']
wp.y_long = param[index]['lon']
wp.z_alt = param[index]['alt']
wl.waypoints.append(wp)
wp = Waypoint()
wp.frame = 2
wp.command = MAV_CMD_RTL
wp.is_current = False
wp.autocontinue = True
wl.waypoints.append(wp)
rospy.loginfo('Waypoints %s', wl)
self.push_mission(wl.waypoints)
self.arming()
rospy.sleep(2)
self.set_current(0)
# def gps_cb(data):
# lat = data.latitude
# lon = data.longitude
# rospy.Subscriber("mavros/global_position/global", NavSatFix, gps_cb)
rospy.spin()
def __init__(self):
self.ready = False
self.got_position = False
self.airborne= False
rospy.init_node('pixhawk_takeoff', anonymous=True)
self.takeoff_height = rospy.get_param('~takeoff_height')
self.subscriber_pos = rospy.Subscriber('~global_pos_in', NavSatFix, self.callback_global_pos)
self.service_arming = rospy.ServiceProxy('~arming_out', CommandBool)
self.service_set_mode = rospy.ServiceProxy('~mode_out', SetMode)
self.service_push_wp = rospy.ServiceProxy('~waypoint_push', WaypointPush)
self.ready = True
while not self.got_position and not rospy.is_shutdown():
rospy.sleep(0.1)
rospy.loginfo_once('Waiting for vehicle position...')
# #{ Prepare mission
wp0 = Waypoint()
wp0.frame = 3 # GLOBAL_REL_ALT
wp0.command = 22 # TAKEOFF
wp0.is_current = True
wp0.x_lat = self.latitude
wp0.y_long = self.longitude
wp0.z_alt = self.takeoff_height
wp0.autocontinue = True
wp1 = Waypoint()
wp1.frame = 3 # GLOBAL_REL_ALT
wp1.command = 17 # NAV_LOITER_UNLIM
wp1.is_current = False
wp1.x_lat = self.latitude
wp1.y_long = self.longitude
wp1.z_alt = self.takeoff_height
wp1.autocontinue = True
# #}
resp = self.service_push_wp.call(0, [wp0, wp1])
print(resp.success)
print(resp.wp_transfered)
self.switch_to_mode('AUTO.MISSION')
while not self.call_arming() and not rospy.is_shutdown():
rospy.logerr_once('Arming failed. Retrying...')
rospy.sleep(0.01)
rospy.loginfo('Vehicle armed')
def get_mission(mission_num=0):
rospack = rospkg.RosPack()
package_path = rospack.get_path('snotbot')
tree = ET.parse(package_path + '/scripts/missions.xml')
for mission in tree.getroot():
if mission.attrib['id'] == mission_num:
break
waypoint_list = []
for item in mission:
waypoint = Waypoint()
waypoint.frame = int(item.find('frame').text)
waypoint.command = int(item.find('command').text)
waypoint.is_current = bool(item.find('is_current').text)
waypoint.autocontinue = bool(item.find('autocontinue').text)
waypoint.param1 = float(item.find('param1').text)
waypoint.param2 = float(item.find('param2').text)
waypoint.param3 = float(item.find('param3').text)
waypoint.param4 = float(item.find('param4').text)
waypoint.x_lat = float(item.find('latitude').text)
waypoint.y_long = float(item.find('longitude').text)
waypoint.z_alt = float(item.find('altitude').text)
waypoint_list.append(waypoint)
return waypoint_list
def update_waypoints(self):
'''
Creates and publishes waypoint list using mavros/waypoints, also update position of model (marker) in Gazebo
:return:
'''
self.waypoint_msg = WaypointList()
# Publish mavros/waypoints
way_tmp = Waypoint()
way_tmp.frame = 0
way_tmp.command = 16
way_tmp.is_current = True
self.waypoint_msg.waypoints.append(way_tmp)
way_tmp = Waypoint()
way_tmp.frame = 0
way_tmp.command = 22
way_tmp.is_current = False
way_tmp.autocontinue = True
way_tmp.param2 = 1
way_tmp.param3 = 0
way_tmp.param4 = 0
way_tmp.x_lat = self.current_waypoint_position_x
way_tmp.y_long = self.current_waypoint_position_y
way_tmp.z_alt = 0
self.waypoint_msg.waypoints.append(way_tmp)
self.mavros_waypoint_pub.publish(self.waypoint_msg)
self.update_waypoint_model()
def modify_waypoint_list(self):
"""
This method is used to create a modified waypoint list that
is uploaded to the copter so it can resume it's mission
after refuelling.
"""
# Create a new waypoint
backup_point = Waypoint()
backup_point.frame = Waypoint.FRAME_GLOBAL_REL_ALT
backup_point.command = CommandCode.NAV_WAYPOINT
backup_point.is_current = True
backup_point.autocontinue = True
backup_point.x_lat = self.vehicle.global_position.latitude
backup_point.y_long = self.vehicle.global_position.longitude
new_waypoint_list = self.vehicle.mission_list
count = 0
for waypoint in new_waypoint_list:
if waypoint.is_current:
waypoint.is_current = False
# set backup waypoint at same height as next waypoint
backup_point.z_alt = waypoint.z_alt
new_waypoint_list.insert(count, backup_point)
self.new_waypoint_number = count
break
count += 1
return new_waypoint_list
def parse_file(self):
file = open("/home/clement/catkin_ws/src/ros_ulysse/src/test.txt", "r")
L = file.readlines()
nb_wp = len(L)
print("Nombre de waypoints :", nb_wp)
self.req = WaypointPushRequest()
for i in range(len(L) - 1):
ligne = L[i + 1].split("\t")
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL
wp.command = CommandCode.NAV_WAYPOINT #Navigate to Waypoint
wp.is_current = bool(ligne[1])
wp.autocontinue = bool(ligne[2])
wp.param1 = float(ligne[4])
wp.param2 = float(ligne[5])
wp.param3 = float(ligne[6])
wp.param4 = float(ligne[7])
wp.x_lat = float(ligne[8])
wp.y_long = float(ligne[9])
wp.z_alt = float(ligne[10])
self.waypoints_list.append(wp)
self.req.waypoints = self.waypoints_list
print(type(self.req.waypoints))
def addWaypoint(lat, lon, obstacles=False, detour=False, override=False):
"""Send waypoint from Odroid to Pixhawk.
Keyword arguments:
detour -- insert waypoint at beginning of array
override -- clear previous waypoints)
"""
waypoint = Waypoint()
waypoint.frame = 3
waypoint.command = 16
waypoint.is_current = 0
waypoint.autocontinue = True
waypoint.param1 = 0 #hold time
waypoint.param2 = 2 #acceptance radius (m)
waypoint.param3 = 0
waypoint.param4 = 0
waypoint.x_lat = lat
waypoint.y_long = lon
waypoint.z_alt = altitude
publish("Adding waypoint: ("+str(waypoint.x_lat)+", "+str(waypoint.y_long)+")")
oldWaypoints = waypoints[:]
publish("Old waypoints: " + str(len(oldWaypoints)))
if override: result = push([waypoint])
elif detour: result = push([waypoint]+oldWaypoints)
else: result = push(oldWaypoints + [waypoint])
publish("Result: " + str(result))
def takeoff_waypoint(curr_count, new_latitude, new_longitude):
waypoint_clear_client(curr_count)
wl = WaypointList()
with open(path_to_waypoint_file) as waypoints:
lines = waypoints.readlines()
for line in lines[1:]:
values = list(line.split('\t'))
wp = Waypoint()
wp.is_current = bool(values[1])
wp.frame = int(values[2])
wp.command = int(values[3])
wp.param1 = float(values[4])
wp.param2 = float(values[5])
wp.param3 = float(values[6])
wp.param4 = float(values[7])
# print(latitude, longitude, "xxxxxx", new_latitude, new_longitude)
wp.x_lat = float(new_latitude)
wp.y_long = float(new_longitude)
wp.z_alt = float(values[10])
wp.autocontinue = bool(values[11])
wl.waypoints.append(wp)
# print(wl.waypoints)
try:
rospy.wait_for_service('vehicle' + str(curr_count) +
'/mavros/mission/push')
service = rospy.ServiceProxy(
'vehicle' + str(curr_count) + '/mavros/mission/push', WaypointPush)
if service.call(0, wl.waypoints).success:
print('write mission success')
else:
print('write mission error')
except:
print("Service call failed: %s" % e)
def main_control():
#pdb.set_trace()
rospy.init_node('offb_node', anonymous=True)
pub = rospy.Publisher("contoller", control, queue_size=10)
while True:
n, lat, lon, alt = raw_input().split()
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL_REL_ALT
wp.command = CommandCode.NAV_WAYPOINT
wp.is_current = True
wp.autocontinue = True
wp.param1 = 0
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = alt
cont = control()
cont.waypoint = wp
cont.uav_no = n
pub.publish(cont)
def upload_missions(self):
actualMissionFile = '/home/anne/catkin_ws/src/agrodrone/agrodrone/tests/integration/' + self.filename
wpl = []
if self.vehicle.fcu_type == "PX4":
with open(actualMissionFile) as mission_file:
data = json.load(mission_file)
for item in data["items"]:
waypoint = Waypoint()
waypoint.command = item["command"]
waypoint.autocontinue = item["autoContinue"]
waypoint.frame = item["frame"]
waypoint.x_lat = item["coordinate"][0]
waypoint.y_long = item["coordinate"][1]
waypoint.z_alt = item["coordinate"][2]
wpl.append(waypoint)
elif self.vehicle.fcu_type == "Ardupilot":
mission = QGroundControlWP()
for waypoint in mission.read(open(actualMissionFile, 'r')):
wpl.append(waypoint)
else:
rospy.logerr("Unknown fcu type set")
res = self._srv_wp_push(wpl)
return res.success
def parse_file(self):
# file_name=input("Entrer le nom du fichier: ")
file = open("../sources/txt_mission_planner/ulysse_promenade.txt", "r")
L = file.readlines()
nb_wp = len(L)
print("Nombre de waypoints :", nb_wp - 1)
self.req = WaypointPushRequest()
for i in range(1, len(L)):
ligne = L[i].split("\t")
print(ligne)
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL
wp.command = CommandCode.NAV_WAYPOINT #Navigate to Waypoint
wp.is_current = True
wp.autocontinue = False
wp.param1 = float(ligne[4])
wp.param2 = float(ligne[5])
wp.param3 = float(ligne[6])
wp.param4 = float(ligne[7])
wp.x_lat = float(ligne[8])
wp.y_long = float(ligne[9])
wp.z_alt = float(ligne[10])
self.waypoints_list.append(wp)
self.req.waypoints = self.waypoints_list
print(self.req.waypoints)
def createWaypointPush(gps_list):
clearCurrentMission()
waypointList = WaypointList()
# waypointList.start_index =
for coord in gps_list:
wp = Waypoint()
wp.frame = 3
wp.command = 16 #simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 #takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = coord.latitude
wp.y_long = coord.longitude
wp.z_alt = coord.altitude
waypointList.waypoints.append(wp)
for wp in waypointList.waypoints:
print(wp.x_lat, wp.y_long, wp.z_alt)
try:
pushMissionSrv = rospy.ServiceProxy('/mavros/mission/push',
WaypointPush)
pushResponse = pushMissionSrv(0, waypointList.waypoints)
rospy.loginfo(pushResponse)
except rospy.ServiceException, e:
rospy.loginfo('Service call failed: %s' % e)
def setMission(positions, timeout=WAIT_FOR_MAVLINK_TIMEOUT):
"""Upload mission of passed positions.
"""
wl = WaypointList()
pos = positions[0]
wp = Waypoint()
wp.command = MAV_CMD_NAV_TAKEOFF
wp.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT
wp.is_current = True
wp.autocontinue = True
wp.x_lat = pos.lat
wp.y_long = pos.lon
wp.z_alt = pos.alt
wl.waypoints.append(wp)
for pos in positions[1:]:
wp = Waypoint()
wp.command = MAV_CMD_NAV_WAYPOINT
wp.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT
wp.is_current = False
wp.autocontinue = True
wp.x_lat = pos.lat
wp.y_long = pos.lon
wp.z_alt = pos.alt
wl.waypoints.append(wp)
wp = Waypoint()
wp.command = MAV_CMD_NAV_LAND
wp.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT
wp.is_current = False
wp.autocontinue = True
wp.x_lat = pos.lat
wp.y_long = pos.lon
wp.z_alt = 0
wl.waypoints.append(wp)
try:
wpPushService.wait_for_service(timeout=timeout)
ret = wpPushService(0, wl.waypoints)
except rospy.ROSException, err:
Info("Waypoint push call failed with error '%s'", err)
return None
def waypoint_push_client():
waypoint_clear_client()
wl = WaypointList()
path_to_way = dir_path = os.path.dirname(os.path.realpath(__file__))
with open(path_to_way + '/way.txt', 'r') as wps:
lines = wps.readlines()
for line in lines[1:]:
values = list(line.split('\t'))
wp = Waypoint()
wp.is_current = bool(values[1])
wp.frame = int(values[2])
wp.command = int(values[3])
wp.param1 = float(values[4])
wp.param2 = float(values[5])
wp.param3 = float(values[6])
wp.param4 = float(values[7])
wp.x_lat = float(values[8])
wp.y_long = float(values[9])
wp.z_alt = float(values[10])
wp.autocontinue = bool(values[11])
wl.waypoints.append(wp)
# wp.frame = 3
# wp.command = 22 # takeoff
# wp.is_current = True
# wp.autocontinue = True
# wp.param1 = data[0]['altitude'] # takeoff altitude
# wp.param2 = 0
# wp.param3 = 0
# wp.param4 = 0
# wp.x_lat = data[0]['latitude']
# wp.y_long = data[0]['longitude']
# wp.z_alt = data[0]['altitude']
#wl.waypoints.append(wp)
# for point in data:
# wp = Waypoint()
# wp.frame = 3
# wp.command = 16 # simple point
# wp.is_current = False
# wp.autocontinue = True
# wp.param1 = 0 # takeoff altitude
# wp.param2 = 0
# wp.param3 = 0
# wp.param4 = 0
# wp.x_lat = point['latitude']
# wp.y_long = point['longitude']
# wp.z_alt = point['altitude']
# wl.waypoints.append(wp)
try:
service = rospy.ServiceProxy('mavros/mission/push', WaypointPush)
if service.call(0, wl.waypoints).success:
print 'write mission success'
else:
print 'write mission error'
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def waypoint(lat, lon, alt):
w = Waypoint()
w.frame = rhc.MAV_FRAME_GLOBAL_RELATIVE_ALT
w.is_current = False
w.autocontinue = True
w.x_lat = lat
w.y_long = lon
w.z_alt = alt
return w
def mission_create():
param = rospy.get_param('waypoints')
print 'Waypoints from parameter server: '
print param
wl = WaypointList()
wp = Waypoint()
wp.frame = MAV_GLOBAL_FRAME
wp.command = MAV_CMD_TAKEOFF # takeoff
wp.is_current = True
wp.autocontinue = True
wp.param1 = param[0]['alt'] # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = param[0]['alt']
wl.waypoints.append(wp)
for index in range(len(param)):
wp = Waypoint()
wp.frame = MAV_GLOBAL_FRAME
wp.command = MAV_CMD_WAYPOINT # simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = param[index]['lat']
wp.y_long = param[index]['lon']
wp.z_alt = param[index]['alt']
wl.waypoints.append(wp)
wp = Waypoint()
wp.frame = 2
wp.command = MAV_CMD_RTL
wp.is_current = False
wp.autocontinue = True
wl.waypoints.append(wp)
push_mission(wl.waypoints)
def mission_create(mission_msg):
rospy.loginfo('Waypoints from message server: ')
rospy.loginfo(mission_msg)
wl = WaypointList()
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL
wp.command = CommandCode.NAV_TAKEOFF # takeoff
wp.is_current = True
wp.autocontinue = True
wp.param1 = mission_msg.waypoints[0].altitude # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = self.lat
wp.y_long = self.lon
wp.z_alt = mission_msg.waypoints[0].altitude
wl.waypoints.append(wp)
for item in mission_msg.waypoints:
wp = Waypoint()
wp.frame = Waypoint.FRAME_GLOBAL
wp.command = CommandCode.NAV_WAYPOINT # simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = item.latitude
wp.y_long = item.longitude
wp.z_alt = item.altitude
wl.waypoints.append(wp)
# wp = Waypoint()
# wp.frame = 2
# wp.command = CommandCode.NAV_RETURN_TO_LAUNCH
# wp.is_current = False
# wp.autocontinue = True
# wl.waypoints.append(wp)
push_mission(wl.waypoints)
def mission_create(mission_msg):
rospy.loginfo('Waypoints from message server: ')
rospy.loginfo(mission_msg)
wl = WaypointList()
wp = Waypoint()
wp.frame = self.MAV_GLOBAL_FRAME
wp.command = self.MAV_CMD_TAKEOFF # takeoff
wp.is_current = True
wp.autocontinue = True
wp.param1 = mission_msg.waypoints[0].altitude # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = mission_msg.waypoints[0].altitude
wl.waypoints.append(wp)
for item in mission_msg.waypoints:
wp = Waypoint()
wp.frame = self.MAV_GLOBAL_FRAME
wp.command = self.MAV_CMD_WAYPOINT # simple point
wp.is_current = False
wp.autocontinue = True
wp.param1 = 0 # takeoff altitude
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = item.latitude
wp.y_long = item.longitude
wp.z_alt = item.altitude
wl.waypoints.append(wp)
wp = Waypoint()
wp.frame = 2
wp.command = self.MAV_CMD_RTL
wp.is_current = False
wp.autocontinue = True
wl.waypoints.append(wp)
push_mission(wl.waypoints)
def takeOff(self):
'Automatic TAKEOFF'
global waypointFlag
# Reset waypointFlag
waypointFlag = False
counter = 0
wpPushFlag = False
# Deliver a new mission plan to the autopilot
# waypoint
wp = Waypoint()
wp.frame = 3 # Global frame:0 with relative alt: 3
wp.command = 22 # Navigate to waypoint
wp.is_current = True # TBC
wp.autocontinue = True # TBC
wp.param1 = 5 # Pitch
wp.param2 = 0 # Empty
wp.param3 = 0 # Empty
wp.param4 = float(
'nan') # Desired yaw angle at waypoint (NaN for unchanged)
wp.x_lat = 0
wp.y_long = 0
wp.z_alt = 10
# Push new waypoint
while (wpPushFlag == False) and (counter <= 3):
rospy.loginfo("Request new mission plan")
rospy.wait_for_service('/mavros/mission/push')
try:
pushWaypointsRequest = rospy.ServiceProxy(
'/mavros/mission/push', WaypointPush)
pushWaypointsResponse = pushWaypointsRequest(start_index=0,
waypoints=[wp])
rospy.loginfo(pushWaypointsResponse)
wpPushFlag = pushWaypointsResponse.success
print("Waypoint Pushed")
except rospy.ServiceException as e:
rospy.loginfo("Request new mission plan failed: %s" % e)
counter += 1
# Message?
# if not in AUTO, set it into this mode
if self.state[3] != 'AUTO':
self.setFlightMode(mode='AUTO')
print("Okay it should be AUTO now")
while waypointFlag == False: # Is True when the WP is reached
# print("This is the waypoint %s" %waypointFlag)
pass
'''
def waypoint(self, coordinates): #coordinates = [lat, long, alt]
####################################### IMPLEMENT SECURITY FOR ALTITUDE (CHECK WITH THE GROUND ALT) ####################################
global waypointFlag
# Reset waypointFlag
waypointFlag = False
counter = 0
wpPushFlag = False
# Deliver a new mission plan to the autopilot
# waypoint
wp = Waypoint()
wp.frame = 3 # Global frame:0 with relative alt: 3
wp.command = 16 # Navigate to waypoint
wp.is_current = True # TBC
wp.autocontinue = True # TBC
wp.param1 = 5 # Hold time is sec
wp.param2 = 0 # Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)
wp.param3 = 0 # 0 to pass through WP, else radius in meters to pass by WP
wp.param4 = float(
'nan') # Desired yaw angle at waypoint (NaN for unchanged)
wp.x_lat = coordinates[0]
wp.y_long = coordinates[1]
wp.z_alt = coordinates[2]
# Push new waypoint
while (wpPushFlag == False) and (counter <= 3):
rospy.loginfo("Request new mission plan")
rospy.wait_for_service('/mavros/mission/push')
try:
pushWaypointsRequest = rospy.ServiceProxy(
'/mavros/mission/push', WaypointPush)
pushWaypointsResponse = pushWaypointsRequest(start_index=0,
waypoints=[wp])
rospy.loginfo(pushWaypointsResponse)
wpPushFlag = pushWaypointsResponse.success
print("TakeOff Pushed")
except rospy.ServiceException as e:
rospy.loginfo("Request new mission plan failed: %s" % e)
counter += 1
# Message?
# if not in AUTO, set it into this mode
if self.state[3] != 'AUTO':
self.setFlightMode(mode='AUTO')
print("Okay it should be AUTO now")
while waypointFlag == False: # Is True when the WP is reached
# print("This is the waypoint %s" %waypointFlag)
pass
def MR_Nav_Waypoint(mcWaypoint):
#NAV-Waypoint
mrWaypoint = Waypoint()
mrWaypoint.command = 16 #NAV_WAYPOINT
mrWaypoint.frame = 0 #GLOBAL
mrWaypoint.autocontinue = True
mrWaypoint.param1 = mcWaypoint.time_to_wait
mrWaypoint.x_lat = mcWaypoint.point.lat
mrWaypoint.y_long = mcWaypoint.point.lng
mrWaypoint.z_alt = mcWaypoint.rel_height
return(mrWaypoint)
def createWaypoint(lat, lon, altitude, type):
waypoint = Waypoint()
waypoint.frame = Waypoint.FRAME_GLOBAL_REL_ALT
waypoint.command = type
waypoint.is_current = 0
waypoint.autocontinue = 0
waypoint.x_lat = lat
waypoint.y_long = lon
waypoint.z_alt = altitude
waypoint.param1 = 1
return waypoint
def waypoint(lat, lon, alt, delay):
w = Waypoint()
w.frame = 3 # Currently is relative to HOME frame TODO: set global frame
w.command = CommandCode.NAV_WAYPOINT
w.is_current = False
w.autocontinue = True
w.param1 = delay # Hold time in mession
w.param2 = 2 # Position trashold in meters
w.x_lat = lat
w.y_long = lon
w.z_alt = alt
return w
def waypoint(lat, lon, alt, delay):
w = Waypoint()
w.frame = Waypoint.FRAME_GLOBAL
w.command = CommandCode.NAV_WAYPOINT
w.is_current = False
w.autocontinue = True
w.param1 = delay # Hold time in mession
w.param2 = 2 # Position trashold in meters
w.x_lat = lat
w.y_long = lon
w.z_alt = alt
return w
def waypoint(lat, lon, alt, delay):
w = Waypoint()
w.frame = Waypoint.FRAME_GLOBAL
w.command = CommandCode.NAV_WAYPOINT
w.is_current = False
w.autocontinue = True
w.param1 = delay # Hold time in mession
w.param2 = 2 # Position trashold in meters
w.x_lat = lat
w.y_long = lon
w.z_alt = alt
return w
def mission_waypoint_push(self, request, response):
rospy.loginfo(request)
mission_push_service = rospy.ServiceProxy(
self.namespace + '/mavros/mission/push', WaypointPush)
waypoints = []
for i in range(2):
takeoff_waypoint = Waypoint()
takeoff_waypoint.command = 22
takeoff_waypoint.param1 = 0
takeoff_waypoint.param2 = 0
takeoff_waypoint.param3 = 0
takeoff_waypoint.param4 = 0
if self.location:
takeoff_waypoint.x_lat = self.location.latitude
takeoff_waypoint.y_long = self.location.longitude
takeoff_waypoint.z_alt = 10
takeoff_waypoint.is_current = True
takeoff_waypoint.autocontinue = True
takeoff_waypoint.frame = 3
waypoints.append(takeoff_waypoint)
for waypoint in request.get('waypoints'):
new_waypoint = Waypoint()
new_waypoint.command = waypoint.get('command')
new_waypoint.is_current = waypoint.get('is_current')
new_waypoint.autocontinue = waypoint.get('autocontinue')
new_waypoint.param1 = waypoint.get('param1')
new_waypoint.param2 = waypoint.get('param2')
new_waypoint.param3 = waypoint.get('param3')
new_waypoint.param4 = waypoint.get('param4')
new_waypoint.x_lat = waypoint.get('x_lat')
new_waypoint.y_long = waypoint.get('y_long')
new_waypoint.z_alt = waypoint.get('z_alt')
new_waypoint.frame = waypoint.get('frame')
waypoints.append(new_waypoint)
rospy.loginfo(waypoints)
local_response = mission_push_service(request.get('start_index'),
waypoints)
response['success'] = local_response.success
response['wp_transfered'] = local_response.wp_transfered
return local_response.success
def createLand():
wp = Waypoint()
wp.frame = 3
wp.command = 21
wp.autocontinue = True
wp.param1 = 25.0
wp.param2 = 3.0
wp.param3 = -0.0
wp.param4 = 0.0
wp.x_lat = 47.3667049
wp.y_long = 8.5499866
wp.z_alt = 10.0
return wp
def createLand():
wp = Waypoint()
wp.frame = 3
wp.command = 21
wp.autocontinue = True
wp.param1 = 25.0
wp.param2 = 3.0
wp.param3 = -0.0
wp.param4 = 0.0
wp.x_lat = 47.3667049
wp.y_long = 8.5499866
wp.z_alt = 10.0
return wp
def createWaypoint(_visitationRadius, _lat, _lon, _alt):
wp = Waypoint()
wp.frame = 3
wp.command = 16
wp.autocontinue = True
wp.param1 = 0
wp.param2 = _visitationRadius
wp.param3 = 0
wp.param4 = 0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp
def createLand(_lat, _lon, _alt):
wp = Waypoint()
wp.frame = 3
wp.command = 21
wp.autocontinue = True
wp.param1 = 25.0
wp.param2 = 3.0
wp.param3 = -0.0
wp.param4 = 0.0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp
def createWaypoint(_visitationRadius, _lat, _lon, _alt):
wp = Waypoint()
wp.frame = 3
wp.command = 16
wp.autocontinue = True
wp.param1 = 0
wp.param2 = _visitationRadius
wp.param3 = 0
wp.param4 = 0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp
def MR_Gimbal_Pitch_Waypoint(pitch):
#set camera gimbal pitch to param
#mavLink sets this as separated Message of type "Waypoint" but with command "205" - "DO_MOUNT_CONTROL"
mrWaypoint = Waypoint()
mrWaypoint.command = 205 #DO_MOUNT_CONTROL
mrWaypoint.frame = 0 #GLOBAL
mrWaypoint.param1 = pitch #pitch
mrWaypoint.param2 = 0 #roll
mrWaypoint.param3 = 0 #yaw
mrWaypoint.param4 = 0 #altitude (not used in this mode)
mrWaypoint.x_lat = 0 #latitude (not used in this mode)
mrWaypoint.y_long = 0 #longitude (not used in this mode)
mrWaypoint.z_alt = 2 #MAV_MOUNT_MODE 2 => MAVLINK_TARGETING => "Start Mavlink Roll,Pitch,Yaw control with stabilization"
return(mrWaypoint)
def wp_copy(wp):
w = Waypoint()
w.command = wp.command
w.frame = wp.frame
w.is_current = wp.is_current
w.autocontinue = wp.autocontinue
w.x_lat = wp.x_lat
w.y_long = wp.y_long
w.z_alt = wp.z_alt
w.param1 = wp.param1
w.param2 = wp.param2
w.param3 = wp.param3
w.param4 = wp.param4
return w
def make_waypoint(frame, command, is_current, lat, lon, alt, hold):
waypoint = Waypoint()
waypoint.frame = frame
waypoint.command = command
waypoint.is_current = is_current
waypoint.autocontinue = True
waypoint.param1 = hold #hold time
waypoint.param2 = 0
waypoint.param3 = 0
waypoint.param4 = 0
waypoint.x_lat = lat
waypoint.y_long = lon
waypoint.z_alt = alt
return waypoint
def make_global_waypoint(self, lat, lon, alt=DEFAULT_ALT, hold=5):
waypoint = Waypoint()
waypoint.frame = 3
waypoint.command = 16
waypoint.is_current = 0
waypoint.autocontinue = False
waypoint.param1 = hold #hold time
waypoint.param2 = 2
waypoint.param3 = 0
waypoint.param4 = 0
waypoint.x_lat = lat
waypoint.y_long = lon
waypoint.z_alt = alt
return waypoint
def createTakeoffCurr(_lat, _lon, _alt):
# 22 MAV_CMD_NAV_TAKEOFF Takeoff from ground / hand
# Mission Param #1 Minimum pitch (if airspeed sensor present), desired pitch without sensor
# Mission Param #2 Empty
# Mission Param #3 Empty
# Mission Param #4 Yaw angle (if magnetometer present), ignored without magnetometer
# Mission Param #5 Latitude
# Mission Param #6 Longitude
# Mission Param #7 Altitude
wp = Waypoint()
wp.frame = 3
wp.command = 22
wp.autocontinue = True
wp.param1 = 0.261799395084
wp.param2 = 0.0
wp.param3 = 0.0
wp.param4 = 0.0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp