def add_waypoints(self, index=0):
"""
Setting up waypoints for the UAV
"""
while self.home.header == Header() and (not rospy.is_shutdown()):
rospy.logwarn(
'Home has not been set! Setting current location as home.')
self.set_current_location_as_home()
# Clear current wps available
self._clear_wp_proxy()
# First wp is home with landing action
home_wp = Waypoint(Waypoint.FRAME_GLOBAL_REL_ALT, 21, False, False, .6,
1., 0, 0, self.home.geo.latitude,
self.home.geo.longitude, 0.)
wps = [home_wp]
# Iterating other wps given by planner
for waypoint in self._waypoints:
wp = Waypoint(Waypoint.FRAME_GLOBAL_REL_ALT, 16, False, False,
waypoint['hold_time'], waypoint['radius'], 0,
waypoint['yaw'], waypoint['lat'], waypoint['long'],
waypoint['rel_alt'])
wps.append(wp)
# Push waypoints to mavros service
response = self._add_wp_proxy(index, wps)
if response.success:
rospy.loginfo('%d waypoints are added' % response.wp_transfered)
else:
rospy.logwarn('No waypoint is added!')
self.waypoints = wps
return response.success
def add_waypoints(self, index=0):
"""
Setting up waypoints for the UAV
"""
if self.home.header == Header():
rospy.logwarn(
'Home has not been set! Setting current location as home.')
self.set_current_location_as_home()
# Clear current wps available
self._clear_wp_proxy()
# First wp is home with landing action
home_wp = Waypoint(Waypoint.FRAME_GLOBAL_REL_ALT, 21, False, False, .6,
1., 0, np.float('nan'), self.home.geo.latitude,
self.home.geo.longitude, 0.)
wps = [home_wp]
# Load waypoints and start adding them to the list
waypoints = self.load_wp_config_from_file()
for waypoint in waypoints['waypoints']:
wp = Waypoint(Waypoint.FRAME_GLOBAL_REL_ALT, 16, False, False,
waypoint['hold_time'], waypoint['radius'], 0.,
waypoint['yaw'], waypoint['lat'], waypoint['long'],
waypoint['rel_alt'])
wps.append(wp)
# Push waypoints to mavros service
response = self._add_wp_proxy(index, wps)
if response.success:
rospy.loginfo('%d waypoints are added' % response.wp_transfered)
else:
rospy.logwarn('No waypoint is added!')
self.waypoints = wps
return response.success
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 emergency_landing(self, duration=rospy.Duration(600, 0)):
"""
Emergency land to home when home wobbles
"""
start = rospy.Time.now()
landed = 0
while (rospy.Time.now() - start <
duration) and not (rospy.is_shutdown()) and (
not self.external_intervened) and (not self.landed):
if landed == self.ACTION_SUCCESS:
self._rate.sleep()
continue
cur_pos = np.array([
self.global_pose.latitude,
self.global_pose.longitude,
])
home_pos = np.array(
[self.home.geo.latitude, self.home.geo.longitude])
altitude = 1.0
if np.linalg.norm(cur_pos - home_pos) < 3e-6 and (self.rel_alt <=
altitude):
if self._min_range > -1 and np.abs(
np.max(np.diff(self._rangefinder[-5:]))) > 1.:
continue
landed = self.guided_mode(50 * self._rate.sleep_dur,
mode='land')
else:
if not self._clear_wp_proxy().success:
if self.current_mode != 'GUIDED':
self.guided_mode(10 * self._rate.sleep_dur)
continue
# wps[0] must be home waypoint
home_wp = Waypoint(Waypoint.FRAME_GLOBAL_REL_ALT, 21, False,
False, 1.0, 1., 0, 0,
self.home.geo.latitude,
self.home.geo.longitude, 0.)
wp = Waypoint(Waypoint.FRAME_GLOBAL_REL_ALT, 16, False, False,
0.1, 0.3, 0, 0., self.home.geo.latitude,
self.home.geo.longitude, altitude / 2.)
wps = [home_wp, wp]
self.waypoints = wps
# Push waypoints to mavros service
if self._add_wp_proxy(0, wps).success:
self.goto(1, 50 * self._rate.sleep_dur, True)
self._rate.sleep()
landed = int(self.landed)
if bool(landed):
self.add_waypoints()
self.guided_mode(10 * self._rate.sleep_dur)
if (rospy.Time.now() - start) > duration:
landed = self.OUT_OF_DURATION
if self.external_intervened:
landed = self.EXTERNAL_INTERVENTION
return landed
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 = 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 push_mission(mission):
set_speed = Waypoint(
frame=3,
command=178,
is_current=True,
autocontinue=True,
param1=1,
param2=1,
param3=-1,
param4=0,
) # Setting ground speed to 1 m/s
takeoff = Waypoint(
frame=3, command=22, is_current=True, autocontinue=True, z_alt=10
) # Takeoff, 10 meters, make current wp
waypoints = [set_speed, takeoff]
for wp in mission:
waypoints.append(
Waypoint(
frame=3,
command=16,
is_current=False,
autocontinue=True,
x_lat=wp["latitude"],
y_long=wp["longitude"],
z_alt=0,
)
)
return_to_launch = Waypoint(
frame=3, command=20, is_current=False, autocontinue=True
) # Return to launch point
land = Waypoint(
frame=3,
command=21,
is_current=False,
autocontinue=True,
x_lat=mission[0]["latitude"],
y_long=mission[0]["longitude"],
z_alt=0,
) # Land at same coordinates as wp_1
waypoints.append(return_to_launch)
waypoints.append(land)
mission_service_object = mission_push_client(
0, waypoints
) # Full waypoint update with waypoints in mission_wp
print("Sent mission to vehicle: ", mission_service_object)
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 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 MR_Home_Waypoint(mcWaypoint):
#Home-Waypoint
mrWaypoint = Waypoint()
mrWaypoint.command = 179 #DO_SET_HOME
mrWaypoint.frame = 0 #GLOBAL
mrWaypoint.autocontinue = True
if (mcWaypoint.point.lat == 0): #no Home is set in MissionControl
mrWaypoint.param1 = 1 #use current location
else:
mrWaypoint.param1 = 0
mrWaypoint.x_lat = mcWaypoint.point.lat
mrWaypoint.y_long = mcWaypoint.point.lng
mrWaypoint.z_alt = mcWaypoint.rel_height
return(mrWaypoint)
def read(self, wpfile):
waypoints = []
f = open(wpfile, "r")
# Header lines look like this "QGC WPL 110"
pastheaderline = False
for data in csv.reader(f, self.CSVDialect):
if not pastheaderline:
qgc, wpl, ver = data[0].split(' ', 3)
ver = int(ver)
if qgc == 'QGC' and wpl == 'WPL' and (ver == 110 or ver ==120):
pastheaderline = True
elif data[0].startswith("Last update"):
break
else:
# Convert waypoints into Waypoint format
waypoints.append(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]))
))
return waypoints
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 read_plan_file(f):
d = json.load(f)
if 'mission' in d:
d = d['mission']
if 'items' in d:
for wp in d['items']:
yield Waypoint(
is_current=False,
frame=int(wp['frame']),
command=int(wp['command']),
param1=float('nan'
if wp['params'][0] is None else wp['params'][0]),
param2=float('nan'
if wp['params'][1] is None else wp['params'][1]),
param3=float('nan'
if wp['params'][2] is None else wp['params'][2]),
param4=float('nan'
if wp['params'][3] is None else wp['params'][3]),
x_lat=float(wp['params'][4]),
y_long=float(wp['params'][5]),
z_alt=float(wp['params'][6]),
autocontinue=bool(wp['autoContinue']))
else:
raise IOError("no mission items")
def latlngs_to_wps(points, altitude):
wps = []
for point in points:
wp = Waypoint(0, 16, False, True, 0, 0, 0, None, point['latitude'],
point['longitude'], altitude)
wps.append(wp)
return wps
def __init__(self):
self.home_waypoint = Waypoint()
self.home_waypoint.frame = 0
self.home_waypoint.command = 22
self.home_waypoint.is_current = 0
self.home_waypoint.autocontinue = 1
self.home_waypoint.param1 = 25
self.home_waypoint.param2 = 0
self.home_waypoint.param3 = 0
self.home_waypoint.param4 = 0
self.home_waypoint.x_lat = 0
self.home_waypoint.y_long = 0
self.home_waypoint.z_alt = 0
self.home = calculation_waypoint()
self.wp_plan = []
self.initial_run = True
self.state = "STABILIZED"
self.has_wp_plan = False
self.mission_started = False
self.waypoint_iter = 0
self.integration = False
self.integrated_area = 0
self.vasi_mikri = 0
self.pr_closest_point = calculation_waypoint(0, 0)
def read_plan_file(f):
d = json.load(f)
if 'mission' in d:
d = d['mission']
# For each item in the item array return a new Waypoints object.
# See documentation for what command 16 (MAV_CMD_NAV_WAYPOINT) does.
# https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_WAYPOINT
# Command 22 is takeoff. MAV_CMD_NAV_TAKEOFF
# Command 21 is land. MAV_CMD_NAV_LAND
# Command 16 is waypoint. MAV_CMD_NAV_WAYPOINT
if 'items' in d:
for wp in d['items']:
# Mission item.
# Documentation: https://mavlink.io/en/messages/common.html#MISSION_ITEM_INT
yield Waypoint(
is_current=False,
frame=int(wp['frame']),
command=int(wp['command']),
param1=float(
'nan' if wp['params'][0] is None else wp['params'][0]),
param2=float(
'nan' if wp['params'][1] is None else wp['params'][1]),
param3=float(
'nan' if wp['params'][2] is None else wp['params'][2]),
param4=float('nan control'
if wp['params'][3] is None else wp['params'][3]),
x_lat=float(wp['params'][4]),
y_long=float(wp['params'][5]),
z_alt=float(wp['params'][6]),
autocontinue=bool(wp['autoContinue']))
else:
raise IOError("no mission items")
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 send_mission_from_file(self):
mission = []
user_dir = os.path.expanduser('~')
# replace with your workspace path
ws_path = '/ardu_ws/src'
filename = user_dir + ws_path + '/bebop2_mavros/scripts/path.txt'
with open(filename, 'r') as file:
for idx, row in enumerate(file):
wp = Waypoint()
x, y = row.split(',')
wp.command = 16
# wp.z_alt = 4
wp.x_lat, wp.y_long = map(float, row.split(','))
mission.append(wp)
self.num_wp = idx
rospy.loginfo('Sending mission, {} waypoints'.format(self.num_wp))
resp = self.send_wp(waypoints=mission)
rospy.loginfo(resp)
def MR_Speed_Waypoint(speed):
#set Speed used for Path to this new Waypoint
#mavLink sets this as separated Message of type "Waypoint" but with command "178" - "DO_CHANGE_SPEED"
mrWaypoint = Waypoint()
mrWaypoint.command = 178 #DO_CHANGE_SPEED
mrWaypoint.frame = 0 #GLOBAL
mrWaypoint.param1 = 1 #Ground Speed, DJI drones are capable of this
mrWaypoint.param2 = speed
mrWaypoint.param3 = -1 #unchanged (no) throttle-Value
mrWaypoint.param4 = 0 #absolute value
return(mrWaypoint)
def _home_cb(self, msg):
"""
Home position callback
"""
self.home = msg
if len(self.waypoints):
home_wp = Waypoint(Waypoint.FRAME_GLOBAL_REL_ALT, 21, False, False,
.6, 1., 0, 0, self.home.geo.latitude,
self.home.geo.longitude, 0.)
self.waypoints[0] = home_wp
def rtl():
w = Waypoint()
w.frame = 2
w.command = MAV_CMD_RTL
w.is_current = False
w.autocontinue = True
return w
def MR_Photo_Distance_Waypoint(distance):
#set photo-distance (not defined in MissionControl-Node)
#mavLink sets this as separated Message of type "Waypoint" but with command "206" - "DO_SET_CAM_TRIGG_DIST"
mrWaypoint = Waypoint()
mrWaypoint.command = 206 #DO_SET_CAM_TRIGG_DIST
mrWaypoint.frame = 0 #GLOBAL
mrWaypoint.param1 = distance #Distance, 0 to stop taking Photos
mrWaypoint.param2 = 0 #"1" => triger camera once
return(mrWaypoint)
def read_mission_file(f):
d = json.load(f)
if 'items' in d:
for wp in d['items']:
yield Waypoint(is_current=False,
frame=int(wp['frame']),
command=int(wp['command']),
param1=float(wp['param1']),
param2=float(wp['param2']),
param3=float(wp['param3']),
param4=float(wp['param4']),
x_lat=float(wp['coordinate'][0]),
y_long=float(wp['coordinate'][1]),
z_alt=float(wp['coordinate'][2]),
autocontinue=bool(wp['autoContinue']))
else:
raise IOError("no mission items")
def read_new_mission(f):
d = json.load(f)
current = True
for wp in d['items']:
yield Waypoint(is_current=current,
frame=int(wp['frame']),
command=int(wp['command']),
param1=float(wp['param1']),
param2=float(wp['param2']),
param3=float(wp['param3']),
param4=float(wp['param4']),
x_lat=float(wp['coordinate'][0]),
y_long=float(wp['coordinate'][1]),
z_alt=float(wp['coordinate'][2]),
autocontinue=bool(wp['autoContinue']))
if current:
current = False
def emergency_landing(self):
"""
Emergency land to home when home wobbles
"""
# Clear current wps available
self._clear_wp_proxy()
# wps[0] must be home waypoint
home_wp = Waypoint(Waypoint.FRAME_GLOBAL_REL_ALT, 21, False, False,
1.0, 1., 0, np.float('nan'), self.home.geo.latitude,
self.home.geo.longitude, 0.)
wps = [home_wp, home_wp]
self.waypoints = wps
# Push waypoints to mavros service
if self._add_wp_proxy(0, wps).success:
self.home_moved = False
# assume that set_current_wp_proxy sets wp to 1
self.full_mission_auto(1, rospy.Duration(1, 0))
return True
def create_waypoint_list(name):
known_versions = (110, 120)
csv.register_dialect('mission_planer',
delimiter='\t',
doublequote=False,
skipinitialspace=True,
lineterminator='\r\n',
quoting=csv.QUOTE_NONE)
rospy.logwarn('WARNING')
with open(name, 'r') as file_wp:
rospy.logwarn('@@@WARNING@@@')
waypoint_list = []
got_header = False
for data in csv.reader(file_wp, dialect='mission_planer'):
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 known_versions:
got_header = True
else:
waypoint_list.append(
Waypoint(frame=int(data[2]),
is_current=bool(int(data[1])),
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]))))
#print waypoint_list
return waypoint_list
def mission_object_constructor(mission_json):
if 'mission' in mission_json:
mission_json = mission_json['mission']
if 'items' in mission_json:
for wp in mission_json['items']:
yield Waypoint(
is_current=False,
frame=int(wp['frame']),
command=int(wp['command']),
param1=float(
'nan' if wp['params'][0] is None else wp['params'][0]),
param2=float(
'nan' if wp['params'][1] is None else wp['params'][1]),
param3=float(
'nan' if wp['params'][2] is None else wp['params'][2]),
param4=float(
'nan' if wp['params'][3] is None else wp['params'][3]),
x_lat=float(wp['params'][4]),
y_long=float(wp['params'][5]),
z_alt=float(wp['params'][6]),
autocontinue=bool(wp['autoContinue']))
else:
raise IOError("no mission items")
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 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 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 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 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
def rtl():
w = Waypoint()
w.command = CommandCode.NAV_RETURN_TO_LAUNCH
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 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 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 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 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 rtl():
w = Waypoint()
w.command = MAV_CMD_RTL
return w
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))
