Esempio n. 1
0
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
Esempio n. 2
0
 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)
Esempio n. 3
0
    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()
Esempio n. 4
0
    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')
Esempio n. 6
0
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
Esempio n. 7
0
    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()
Esempio n. 8
0
    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
Esempio n. 9
0
    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))
Esempio n. 10
0
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))
Esempio n. 11
0
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)
Esempio n. 12
0
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)
Esempio n. 13
0
    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
Esempio n. 14
0
    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)
Esempio n. 15
0
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)
Esempio n. 16
0
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
Esempio n. 17
0
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
Esempio n. 18
0
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
Esempio n. 19
0
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)
Esempio n. 20
0
        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)
Esempio n. 21
0
        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)
Esempio n. 22
0
    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
        '''
Esempio n. 23
0
    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
Esempio n. 24
0
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)
Esempio n. 25
0
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
Esempio n. 26
0
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
Esempio n. 29
0
 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
Esempio n. 31
0
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
Esempio n. 32
0
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
Esempio n. 35
0
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)
Esempio n. 36
0
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
Esempio n. 37
0
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
Esempio n. 38
0
    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