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 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 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 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 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 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 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 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 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 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 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 = 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 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 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 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 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 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 = 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 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 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 wpts(x, y, z): d = Waypoint() d.frame = 3 d.command = 16 d.is_current = False d.autocontinue = True d.param1 = 0.000000 d.param2 = 0.000000 d.param3 = 0.000000 d.param4 = 0.000000 d.x_lat = x #-35.362521 d.y_long = y #149.170465 d.z_alt = z #70.000000 return [d]
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 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 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 construct_waypoint(c_wp): """ Construct and return a proper mavros Waypoint structure """ the_wp = Waypoint() the_wp.frame = 3 the_wp.command = 16 the_wp.is_current = 1 the_wp.autocontinue = 1 the_wp.param1 = 0 the_wp.param2 = 10 the_wp.param3 = 0 the_wp.param4 = 0 the_wp.x_lat = c_wp.latitude the_wp.y_long = c_wp.longitude the_wp.z_alt = ALTITUDE return the_wp
def get_mission(mission_hash): mission_url = 'http://{0}/ipfs/{1}'.format(os.environ['IPFS_HOST'], mission_hash) mission_data = urllib2.urlopen(mission_url).read() mission_items = json.loads(mission_data)['items'] for item in mission_items: w = Waypoint() w.frame = item['frame'] w.command = item['command'] w.autocontinue = item['autoContinue'] w.param1 = item['param1'] w.param2 = item['param2'] w.param3 = item['param3'] w.param4 = item['param4'] w.x_lat = item['coordinate'][0] w.y_long = item['coordinate'][1] w.z_alt = item['coordinate'][2] yield w
def createTakeoffCurr(): # 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 = 47.3669242859 wp.y_long = 8.54999923706 wp.z_alt = 10.0 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 waypointh(): #for home. d = Waypoint() d.frame = 0 d.command = 16 d.is_current = False d.autocontinue = True d.param1 = 0.000000 d.param2 = 0.000000 d.param3 = 0.000000 d.param4 = 0.000000 d.x_lat = -35.363262 d.y_long = 149.165238 d.z_alt = 584.090027 rospy.wait_for_service('/mavros/mission/push') try: wp = rospy.ServiceProxy('/mavros/mission/push', WaypointPush) response = wp(0, [d]) response.success except rospy.ServiceException, e: print "Service call failed: %s" % e
def update_waypoints(self): """ Creates and publishes waypoint list using mavros/waypoints, also update position of model (marker) in Gazebo :return: """ # Publish mavros/waypoints way_tmp = Waypoint() way_tmp.frame = 0 way_tmp.command = 22 way_tmp.is_current = True 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 = self.current_waypoint_position_z self.mavros_waypoint_pub.publish(way_tmp) self.update_waypoint_model()
def callback(data): global wp_plan global waypoint_iter global distance global integrated_area global integration_started global vasi_mikri global pr_closest_point actual_position = waypoint(data.latitude, data.longitude) previous_wp = wp_plan[waypoint_iter] next_wp = wp_plan[waypoint_iter + 1] closest_point = get_closest_point_on_segment(previous_wp.latitude, previous_wp.longitude, next_wp.latitude, next_wp.longitude, data.latitude, data.longitude) # if waypoint_iter > 0: # if integration_started == 0: # rospy.loginfo("========= INTEGRATION STARTED =========") # vasi_mikri = get_distance(data.latitude, data.longitude, closest_point.latitude, closest_point.longitude) # pr_closest_point = waypoint(latitude=closest_point.latitude, longitude=closest_point.longitude) # integration_started = 1 # else: # vasi_megali = get_distance(data.latitude, data.longitude, closest_point.latitude, closest_point.longitude) # integrated_area += ((vasi_mikri + vasi_megali) * get_distance(pr_closest_point.latitude, pr_closest_point.longitude, # closest_point.latitude, closest_point.longitude)) / 2 # vasi_mikri = vasi_megali # pr_closest_point = closest_point bearing = find_bearing(previous_wp, next_wp) # the next is the second way of calculating the next waypoint, by including the distance from the actual position of the UAV #remaining_distance = distance - get_distance(data.latitude, data.longitude, closest_point.latitude, closest_point.longitude) #c_wp = find_command_waypoint(remaining_distance, bearing, closest_point) c_wp = find_command_waypoint(distance, bearing, closest_point) if get_distance(data.latitude, data.longitude, next_wp.latitude, next_wp.longitude) < 0.04: # meters waypoint_iter += 1 rospy.loginfo("Waypoint id: " + str(waypoint_iter + 2)) if waypoint_iter == 45: # introduce angle if needed from previous file rospy.loginfo("For distance of: " + str(distance) + "km the integrated area is: " + str(integrated_area) + " sq.kilometers") exit(0) # SETTING THE SERVICE NEW WP # CONSTRUCTING THE WP the_wp = Waypoint() the_wp.frame = 3 the_wp.command = 16 the_wp.is_current = 1 the_wp.autocontinue = 1 the_wp.param1 = 0 the_wp.param2 = 10 the_wp.param3 = 0 the_wp.param4 = 0 the_wp.x_lat = c_wp.latitude the_wp.y_long = c_wp.longitude the_wp.z_alt = 100 the_home = Waypoint() the_home.frame = 0 the_home.command = 22 the_home.is_current = 0 the_home.autocontinue = 1 the_home.param1 = 25 the_home.param2 = 0 the_home.param3 = 0 the_home.param4 = 0 the_home.x_lat = 41.1393967 the_home.y_long = -8.7332993 the_home.z_alt = 0 the_req = WaypointPushRequest() the_req.waypoints.insert(0, the_home) the_req.waypoints.insert(1, the_wp) # SENDING THE LIST rospy.loginfo(" Intermediate waypoint, latitude: " + str(c_wp.latitude) + ", longitude: " + str(c_wp.longitude)) command_service(the_req) current_service(1)
def _start_mission(self): print(self.namespace, 'waypoints') req = WaypointPushRequest() wp1 = Waypoint() wp1.frame = Waypoint.FRAME_GLOBAL wp1.command = CommandCode.NAV_TAKEOFF wp1.is_current = True wp1.autocontinue = True wp1.param1 = 0.3 # minimum / desired pitch wp1.param2 = 0.0 wp1.param3 = 0.0 wp1.param4 = 0.0 if self.color == 'blue' else -math.pi # yaw angle wp1.x_lat = self.start_position.latitude wp1.y_long = self.start_position.longitude wp1.z_alt = self.start_position.altitude + ALTITUDE_ABOVE_GROUND wp2 = copy.deepcopy(wp1) wp2.command = CommandCode.NAV_WAYPOINT wp2.is_current = False wp2.param1 = 0.0 wp2.param2 = 5.0 # acceptance radius wp2.param4 = 0.0 wp2.x_lat, wp2.y_long = cube_to_global(400.0, 250.0) wp3 = copy.deepcopy(wp2) wp3.autocontinue = False wp3.x_lat, wp3.y_long = cube_to_global(100.0, 250.0) req.waypoints = [wp1, wp2, wp3] service_name = '%s/mavros/mission/push' % self.namespace rospy.wait_for_service(service_name) try: service = rospy.ServiceProxy(service_name, WaypointPush) resp = service.call(req) except rospy.ServiceException as e: print(self.namespace, 'service call to push waypoints failed:', str(e), file=sys.stderr) return False if not resp.success: print(self.namespace, 'failed to push waypoints', file=sys.stderr) return False print(self.namespace, 'pushed waypoints') print(self.namespace, 'start mission') req = SetModeRequest() req.base_mode = 0 # use custom mode # first 0x04: mode auto # second 0x04: mission # third and fourth 0x0000: empty req.custom_mode = str(int('0x04040000', 0)) service_name = '%s/mavros/set_mode' % self.namespace rospy.wait_for_service(service_name) try: service = rospy.ServiceProxy(service_name, SetMode) resp = service.call(req) except rospy.ServiceException as e: print(self.namespace, 'service call to set mode failed:', str(e), file=sys.stderr) return False if not resp.success: print(self.namespace, 'failed to set mode', file=sys.stderr) return False print(self.namespace, 'started mission') self._set_state(STATE_MISSION) return True