def start_with_takeoff(self):
start = self.make_global_waypoint(0, 0)
takeoff = Waypoint()
takeoff.command = 22
takeoff.z_alt = DEFAULT_ALT
self.waypoints.insert(0, takeoff)
self.waypoints.insert(0, start)
def start_with_takeoff(self):
start = self.make_global_waypoint(0, 0)
takeoff = Waypoint()
takeoff.command = 22
takeoff.z_alt = DEFAULT_ALT
self.waypoints.insert(0, takeoff)
self.waypoints.insert(0, start)
def read(self, file_):
got_header = False
for data in csv.reader(file_, self.CSVDialect):
if data[0].startswith('#'):
continue
# skip comments (i think in next format version they add this)
if not got_header:
qgc, wpl, ver = data[0].split(' ', 3)
ver = int(ver)
if qgc == 'QGC' and wpl == 'WPL' and ver in self.known_versions:
got_header = True
else:
yield Waypoint(is_current=bool(int(data[1])),
frame=int(data[2]),
command=int(data[3]),
param1=float(data[4]),
param2=float(data[5]),
param3=float(data[6]),
param4=float(data[7]),
x_lat=float(data[8]),
y_long=float(data[9]),
z_alt=float(data[10]),
autocontinue=bool(int(data[11])))
def make_global_waypoint(self, lat, lon, alt=DEFAULT_ALT, hold=5):
rospack = rospkg.RosPack()
package_path = rospack.get_path('drone_control')
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 end_with_rtl(self):
rtl = Waypoint()
rtl.command = 20
self.waypoints.append(rtl)
def goto_waypoint(req):
lon,lat = transform_coordinate(21781, 4326, req.x, req.y)
print "goto lon ",lon, " lat ",lat
# rospy.wait_for_service('/mavros/mission/goto')
try:
goto = rospy.ServiceProxy('/mavros/mission/goto',WaypointGOTO )
wp = Waypoint()
"""
uint8 FRAME_GLOBAL = 0
uint8 FRAME_LOCAL_NED = 1
uint8 FRAME_MISSION = 2
uint8 FRAME_GLOBAL_REL_ALT = 3
uint8 FRAME_LOCAL_ENU = 4
"""
wp.frame = 3
"""
uint16 NAV_WAYPOINT = 16
uint16 NAV_LOITER_UNLIM = 17
uint16 NAV_LOITER_TURNS = 18
uint16 NAV_LOITER_TIME = 19
uint16 NAV_RETURN_TO_LAUNCH = 20
uint16 NAV_LAND = 21
uint16 NAV_TAKEOFF = 22
"""
wp.command = 16
wp.is_current = True
wp.autocontinue = False
wp.param1 = 0
wp.param2 = 0
wp.param3 = 0
wp.param4 = 0
wp.x_lat = lat
wp.y_long = lon
wp.z_alt = 5
print "sending goto waypoint ",wp
ret = goto(wp)
print "ret_value ",ret
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def create_waypoint(x, y, z = 5, wp_type = Waypoint.NAV_WAYPOINT):
point = Waypoint()
point.frame = Waypoint.FRAME_GLOBAL_REL_ALT
point.command = wp_type
point.is_current = False
point.autocontinue = True
point.param1 = 0
point.param2 = 0
point.param3 = 0
point.param4 = 0
point.x_lat = x
point.y_long = y
point.z_alt = z
return point
def get_mission(mission_num=0):
rospack = rospkg.RosPack()
package_path = rospack.get_path('drone_control')
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 make_global_waypoint(lat, lon):
waypoint = Waypoint()
waypoint.frame = 3
waypoint.command = 16
waypoint.is_current = 0
waypoint.autocontinue = False
waypoint.param1 = 5 #hold time
waypoint.param2 = 2
waypoint.param3 = 0
waypoint.param4 = 0
waypoint.x_lat = lat
waypoint.y_long = lon
waypoint.z_alt = 5
return waypoint
def make_global_waypoint(self, lat, lon, alt=DEFAULT_ALT, hold=5):
rospack = rospkg.RosPack()
package_path = rospack.get_path('drone_control')
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 end_with_rtl(self):
rtl = Waypoint()
rtl.command = 20
self.waypoints.append(rtl)
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
