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 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))