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 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 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()
def setUp(self): self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.home_position = HomePosition() self.local_position = PoseStamped() self.mission_wp = WaypointList() self.state = State() self.mav_type = None self.sub_topics_ready = { key: False for key in [ 'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos', 'mission_wp', 'state' ] } # ROS services service_timeout = 30 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/param/get', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.loginfo("ROS services are up") except rospy.ROSException: self.fail("failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) # ROS subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback)
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 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, node_name): self.node_name = node_name self.state = State() self.mission_wp = WaypointList() self.pos = np.array([0, 0, 0]) self.vel = np.array([0, 0, 0]) self.acc = np.array([0, 0, 0]) self.yaw = 0. self.local_position = None self.local_goal_point = None self.local_goal_direction = None self.start_pos = np.array([0, 0, 0]) self.hasLimitedVision = False self.visionRadius = 15.
def MC_to_MR(req): mrMission = WaypointList() mrMission.current_seq = 0 mcMission = req.mcMission.waypoints #set Home if given in MissionControl if (req.mcMission.home.point.lat != 0): #Home Position is set mrMission.waypoints.append(MR_Home_Waypoint(req.mcMission.home)) #set Gimbal Pitch from Param 'gimbalPitch' mrMission.waypoints.append(MR_Gimbal_Pitch_Waypoint(rospy.get_param('gimbalPitch'))) counter = 0 speed = 0 cam_trigger_dist = 0 for mcWaypoint in mcMission: #set Speed, if it has changed if (mcWaypoint.speed != speed): #change Speed speed = mcWaypoint.speed mrMission.waypoints.append(MR_Speed_Waypoint(speed)) #set Nav_Waypoint mrMission.waypoints.append(MR_Nav_Waypoint(mcWaypoint)) counter += 1 #if cam_trigger_distance would be defined in MissionControl, it could be implemented here. Same way as speed. #until implementation, camera_trigger_distance is set after the first Waypoint #value is set in param 'imageDistance' if (counter == 1): mrMission.waypoints.append(MR_Photo_Distance_Waypoint(rospy.get_param('imageDistance'))) #quit Photos mrMission.waypoints.append(MR_Photo_Distance_Waypoint(0)) return mcTOmrResponse(mrMission)
def received_rawdata(self): """ input: (none) output: thedata (std_msgs.msg.String) returns raw data that was received, in its original ROS format """ self.lock.acquire() try: thedata = self.mystring_ros self.mystring_ros = WaypointList() finally: self.lock.release() return thedata
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
def setUp(self): super(MavrosOffboardPosctlTest, self).setUp() self.matlab_topics_ready = {key: False for key in ['task']} self.pos = PoseStamped() self.radius = 1 # Subscribing to matlab message self.matlab_task = WaypointList() self.matlab_task_sub = rospy.Subscriber("/task", PoseStamped, self.matlab_callback) self.pos_setpoint_pub = rospy.Publisher( '/uav0/mavros/setpoint_position/local', PoseStamped, queue_size=1) # send setpoints in seperate thread to better prevent failsafe self.pos_thread = Thread(target=self.send_pos, args=()) self.pos_thread.daemon = True self.pos_thread.start()
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 __init__(self): self.br = None self.server = None self.rate_config = rospy.get_param('~rate', 10.0) self.current_waypoint_position_x = rospy.get_param( '~current_waypoint_position_x', 0.0) self.current_waypoint_position_y = rospy.get_param( '~current_waypoint_position_y', 0.0) self.current_waypoint_position_z = rospy.get_param( '~current_waypoint_position_z', 0.0) self.g_set_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) # mavros marker setup # configure waypoint publisher self.mavros_waypoint_pub = rospy.Publisher("/mavros/mission/waypoints", Waypoint, queue_size=1) self.waypoint_msg = WaypointList() # define rate self.rate = rospy.Rate(self.rate_config) self.br = TransformBroadcaster() self.server = InteractiveMarkerServer("waypoint_interactive_publisher") position = Point(self.current_waypoint_position_x, self.current_waypoint_position_y, 0) self.make_auv_waypoint_Marker(position) self.server.applyChanges() # infinity loop while not rospy.is_shutdown(): self.update_waypoints() self.rate.sleep()
#!/usr/bin/env python import rospy import os import json from sensor_msgs.msg import Imu from mavros import mavlink from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached, State, WaypointList from mavros_msgs.srv import WaypointPush, SetMode, CommandBool from pymavlink import mavutil from threading import Thread state = State() mission_wp = WaypointList() # Execute the mission that has been specified. def mission(mission_file_path): try: mission = read_mission(mission_file_path) except IOError as e: print e send_wps(mission, 30) def send_wps(waypoints, timeout): global mission_wp rospy.loginfo("sending mission waypoints") loop_freq = 1 # Hz rate = rospy.Rate(loop_freq)
def __init__(self, *args): super(MavrosQuad, self).__init__(*args) self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.home_position = HomePosition() self.local_position = PoseStamped() self.mission_wp = WaypointList() self.state = State() self.mav_type = None self.local_velocity = TwistStamped() self.gazebo_load_name = 'rigid_body_load_1_vehicle::rb_link' self.gazebo_load_pose = Pose() self.gazebo_load_twist = Twist() self.gazebo_quad_name = 'rigid_body_load_1_vehicle::base_link_0' self.gazebo_quad_pose = Pose() self.gazebo_quad_twist = Twist() self.gazebo_imu_name = 'rigid_body_load_1_vehicle::iris_0/imu_link' self.gazebo_imu_pose = Pose() self.gazebo_imu_twist = Twist() self.q_input = np.array([0, 0, 0, 1]) self.T_input = 0 self.offboard_position_active = True self.offboard_attitude_active = False self.offboard_load_active = False self.sub_topics_ready = { key: False for key in [ 'alt', 'ext_state', 'gazebo', 'global_pos', 'home_pos', 'local_pos', 'local_vel', 'mission_wp', 'state', 'imu' ] } # ROS services service_timeout = 30 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/param/get', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.loginfo("ROS services are up") except rospy.ROSException: rospy.logerr("failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) # ROS subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.local_vel_sub = rospy.Subscriber( 'mavros/local_position/velocity_local', TwistStamped, self.local_velocity_callback) self.imu_sub = rospy.Subscriber('/mavros/imu/data', Imu, self.imu_callback) self.gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.gazebo_callback) # ROS Publishers # Attitude self.att = AttitudeTarget() self.att_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) # send setpoints in seperate thread to better prevent failsafe self.att_thread = Thread(target=self.send_att, args=()) self.att_thread.daemon = True self.att_thread.start() # Pose self.pos = PoseStamped() self.pos_setpoint_pub = rospy.Publisher( 'mavros/setpoint_position/local', PoseStamped, queue_size=1) # send setpoints in seperate thread to better prevent failsafe self.pos_thread = Thread(target=self.send_pos, args=()) self.pos_thread.daemon = True self.pos_thread.start()
def __init__(self, ros_rate=10, # slow as nothing happens in the main loop state_estimation_mode=State_estimation_method.GPS, enforce_height_mode_flag=False, height_mode_req=0, ): self.sem = state_estimation_mode self._node_alive = True self.ros_rate = ros_rate self.wd_initialised = False self.wd_fault_detected = False self.fault_this_loop = False # initialise data containers self.altitude = Altitude() self.altitude_bottom_clearance = Float32() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.optic_flow_raw = OpticalFlowRad() self.optic_flow_range = Range() self.home_position = HomePosition() self.local_position = PoseStamped() # self.gt_position = PoseStamped() self.mission_wp = WaypointList() self.state = State() self.mocap_pose = PoseStamped() self.camera_pose = PoseStamped() self.camera_yaw = None self.local_x = Float64().data self.local_y = Float64().data self.local_z = Float64().data self.gt_x = Float64().data self.gt_y = Float64().data self.gt_z = Float64().data # todo - we can probably live with just XX_vel_bod data?: self.x_vel = Float64().data self.y_vel = Float64().data self.gt_x_vel = Float64().data self.gt_y_vel = Float64().data self.vel_ts = Float64().data self.xy_vel = Float64().data self.x_vel_bod = Float64().data self.y_vel_bod = Float64().data self.xy_vel_bod = Float64().data self.body_yaw_rate = Float64().data self.global_compass_hdg_deg = Float64().data self.enforce_height_mode_flag = enforce_height_mode_flag self.height_mode_req = height_mode_req # threading locks self.setpoint_lock = Lock() # used for setting lock in our setpoint publisher so that commands aren't mixed # ROS services service_timeout = 10 rospy.loginfo("Searching for mavros services") try: rospy.wait_for_service('mavros/param/get', service_timeout) rospy.wait_for_service('mavros/param/set', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.wait_for_service('/mavros/cmd/set_home') # rospy.wait_for_service('mavros/fcu_url', service_timeout) # todo - check how this is used in px4 self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) self.set_param_srv = rospy.ServiceProxy('mavros/param/set', ParamSet) self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) self.takeoff_srv = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL) self.cmd_home_srv = rospy.ServiceProxy('/mavros/cmd/set_home', CommandHome) rospy.loginfo("Required ROS services are up") except rospy.ROSException: self.shut_node_down(extended_msg="failed to connect to Mavros services - was the mavros node started?") # make sure we have information about our connection with FCU rospy.loginfo("Get our fcu string") try: self.fcu_url = rospy.get_param('mavros/fcu_url') except Exception as e: print (e) self.shut_node_down(extended_msg="cant find fcu url") # ensure that our height mode is as we expect it to be (if required) rospy.loginfo('check height_mode {}'.format(self.enforce_height_mode_flag)) if self.enforce_height_mode_flag: # todo - allow multiple attempts at this # res = self.mavros_interface.get_param_srv(param) param_read_attempts = 0 try: while param_read_attempts < 5: res = self.get_param_srv('EKF2_HGT_MODE') if res.success: self.height_mode = res.value.integer if self.height_mode == self.height_mode_req: rospy.loginfo('height mode {} as expected'.format(self.height_mode)) break else: raise Exception ("height mode is {} - (expected heightmode is {}) change parameter with QGround control and try again".format(self.height_mode, self.height_mode_req)) break else: rospy.logerr( "Couldn't read EKF2_HGT_MODE param on attempt {} - trying again".format(param_read_attempts)) param_read_attempts += 1 rospy.sleep(2) except Exception as e: rospy.logerr( "Couldn't read EKF2_HGT_MODE - shutting down".format(param_read_attempts)) self.shut_node_down(extended_msg= "height_mode error - traceback is {}".format(e)) # todo: ensure that our state estimation parameters are as available (this requires the state estimation # topic name so can't test this until we do something with mocap again) Actually, we can incorporate this into # the watchdog # if state_estimation_mode == State_estimation_method.MOCAP: # ROS subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state',ExtendedState,self.extended_state_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global',NavSatFix, self.global_position_callback) self.optic_flow_raw_sub = rospy.Subscriber('mavros/px4flow/raw/optical_flow_raw',OpticalFlowRad, self.optic_flow_raw_callback) self.optic_flow_range_sub = rospy.Subscriber('mavros/px4flow/ground_distance',Range,self.optic_flow_range_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.mocap_pos_sub = rospy.Subscriber('mavros/vision_pose/pose', PoseStamped, self.mocap_pos_callback) # self.camera_pose_sub = rospy.Subscriber(self.camera_pose_topic_name, PoseStamped, self.cam_pose_cb) # todo - add check for this signal to watchdog - or remap /mavros/local_position/velocity -> /mavros/local_position/velocity_local self.velocity_local_sub = rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.vel_callback) self.velocity_body_sub = rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.vel_bod_callback) self.compass_sub = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, self.compass_hdg_callback) self.ground_truth_sub = rospy.Subscriber('/body_ground_truth', Odometry, self.gt_position_callback) ## Ros publishers self.local_pos_pub_raw = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1) # ROS topics - this must come after our ROS subscribers topics_timeout = 30 rospy.loginfo("waiting for ROS topics") try: # check that essential messages are being subscribed to regularly for _ in np.arange(2): rospy.wait_for_message('mavros/local_position/pose', PoseStamped, topics_timeout) rospy.wait_for_message('mavros/extended_state', ExtendedState, topics_timeout) except rospy.ROSException: self.shut_node_down(extended_msg="Required ros topics not published") rospy.loginfo("ROS topics are up") # create a watchdog thread that checks topics are being received at the expected rates self.watchdog_thread = Thread(target=self.watchdog, args=()) self.watchdog_thread.daemon = True self.watchdog_thread.start()
def __init__(self): self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.imu_data = Imu() self.home_position = HomePosition() self.local_position = PoseStamped() self.mission_wp = WaypointList() self.state = State() self.mav_type = None self.pos = PoseStamped() self.radius = 1 self.sub_topics_ready = { key: False for key in [ 'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos', 'mission_wp', 'state', 'imu' ] } # ROS services service_timeout = 30 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/param/get', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.loginfo("ROS services are up") except rospy.ROSException: self.fail("failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) # ROS subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.imu_data_sub = rospy.Subscriber('mavros/imu/data', Imu, self.imu_data_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) # ROS publisher self.pos_setpoint_pub = rospy.Publisher( 'mavros/setpoint_position/local', PoseStamped, queue_size=1) self.pos_thread = Thread(target=self.send_pos, args=()) self.pos_thread.daemon = True self.pos_thread.start()
def __init__(self, *args): super(MavrosQuad, self).__init__(*args) self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.home_position = HomePosition() self.local_position = PoseStamped() self.mission_wp = WaypointList() self.state = State() self.mav_type = None self.local_velocity = TwistStamped() self.gazebo_load_name = 'rigid_body_load_1_vehicle::rb_link' self.gazebo_load_pose = Pose() self.gazebo_load_twist = Twist() self.gazebo_quad_name = 'rigid_body_load_1_vehicle::base_link_0' self.gazebo_quad_pose = Pose() self.gazebo_quad_twist = Twist() self.gazebo_imu_name = 'rigid_body_load_1_vehicle::iris_0/imu_link' self.gazebo_imu_pose = Pose() self.gazebo_imu_twist = Twist() self.sub_topics_ready = { key: False for key in [ 'alt', 'ext_state', 'gazebo', 'global_pos', 'home_pos', 'local_pos', 'local_vel', 'mission_wp', 'state', 'imu' ] } # ROS services service_timeout = 30 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/param/get', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) rospy.loginfo("ROS services are up") except rospy.ROSException: rospy.logerr("failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) # ROS subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber('mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.local_vel_sub = rospy.Subscriber('mavros/local_position/velocity_local', TwistStamped, self.local_velocity_callback) self.imu_sub = rospy.Subscriber('/mavros/imu/data', Imu, self.imu_callback) self.gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.gazebo_callback)
def setUp(self): print('@ctrl_server@ uav{0} setUp'.format(self.uav_number)) self.pos = PoseStamped() self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.home_position = HomePosition() self.local_position = PoseStamped() self.misson_wp = WaypointList() self.state = State() self.scan = LaserScan() self.mav_type = None self.ready = False # Target offset radius self.radius = 0.25 self.sub_topics_ready = { key: False for key in [ 'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos', 'mission_wp', 'state', 'scan' ] } # ROS services service_timeout = 60 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service(self.uav_prefix + 'mavros/param/get', service_timeout) rospy.wait_for_service(self.uav_prefix + 'mavros/cmd/arming', service_timeout) rospy.wait_for_service(self.uav_prefix + 'mavros/mission/push', service_timeout) rospy.wait_for_service(self.uav_prefix + 'mavros/mission/clear', service_timeout) rospy.wait_for_service(self.uav_prefix + 'mavros/set_mode', service_timeout) rospy.loginfo("ROS services are up") except rospy.ROSException: print("@ctrl_server@ failed to connect to services") self.get_param_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy(self.uav_prefix + 'mavros/mission/push', WaypointPush) # ROS subscribers self.alt_sub = rospy.Subscriber(self.uav_prefix + 'mavros/altitude', Altitude, self.altitude_callback) self.ext_state_sub = rospy.Subscriber(self.uav_prefix + 'mavros/extended_state', ExtendedState, self.extended_state_callback) self.global_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/global_position/global', NavSatFix, self.global_position_callback) self.home_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/home_position/home', HomePosition, self.home_position_callback) self.local_pos_sub = rospy.Subscriber(self.uav_prefix + 'mavros/local_position/pose', PoseStamped, self.local_position_callback) self.mission_wp_sub = rospy.Subscriber(self.uav_prefix + 'mavros/mission/waypoints', WaypointList, self.mission_wp_callback) self.state_sub = rospy.Subscriber(self.uav_prefix + 'mavros/state', State, self.state_callback) self.get_scan_srv = rospy.Subscriber('iris_rplidar_' + self.uav_number + '/laser/scan', LaserScan, self.scan_callback) # ROS publisers self.pos_setpoint_pub = rospy.Publisher( self.uav_prefix + 'mavros/setpoint_position/local', PoseStamped, queue_size=1) # send setpoints in seperate thread self.pos_thread = Thread(target=self.send_pos, args=()) self.pos_thread.daemon = True self.pos_thread.start() print('@ctrl_server@ setUp over') pass
def task_listener(self): self.matlab_task = WaypointList() rospy.init_node('/uav0_task') self.matlab_task_sub = rospy.Subscriber("task", WaypointList, self.matlab_callback) rospy.spin()
def __init__(self): self.mystring = None #"" # unpacked self.mystring_ros = WaypointList() # raw format self.lock = threading.Lock()
def handle_set_mission_waypoints(req): global lat_x global lng_y global alt lat_x = req.lat lng_y = req.lng alt = req.height_agl print "MISSION WAYPOINTS REQUEST -> " print req print "" if lat_x is None: message = "No waypoints received." return False, message else: publish_waypoint_data(lat_x, lng_y) waypoint_list = [] final_waypoint = WaypointList() # for i in range(len(lat_x)): # # print "reached " # # x = lat_x[i] # # y = lng_y[i] # # z = alt[i] # print "X AND Y COORDS ->" # print x # print y # print "--------------" # try: # print "Waiting for position_set_global" # rospy.wait_for_service('/flytos/navigation/position_set_global',timeout=2) # print "After " # handle = rospy.ServiceProxy('/flytos/navigation/position_set_global', PositionSetGlobal) # req_msg = PositionSetGlobalRequest(lat_x=x, long_y=y, rel_alt_z=z, yaw=0, tolerance=0.2, async=False, yaw_valid=False) # resp = handle(req_msg) # except rospy.ServiceException, e: # rospy.logerr("global pos set service call failed %s", e) # return False, e for i in range(len(lat_x)): waypoint = Waypoint() waypoint.frame = 3 waypoint.command = 16 if i == 0: waypoint.is_current = True else: waypoint.is_current = False waypoint.autocontinue = True waypoint.param1 = 2.0 waypoint.param2 = 0 waypoint.param3 = 0 waypoint.param4 = 0 waypoint.x_lat = lat_x[i] waypoint.y_long = lng_y[i] waypoint.z_alt = 2.5 print "Printing Waypoint ....." print waypoint waypoint_list.append(waypoint) final_waypoint.waypoints = [waypoint_list] try: rospy.wait_for_service('/flytos/navigation/waypoint_set', timeout=10) handle = rospy.ServiceProxy('/flytos/navigation/waypoint_set', WaypointSet) req_msg = WaypointSetRequest(waypoints=waypoint_list) resp = handle(req_msg) message = "Sent mission waypoints to the drone successfully." if resp.success is True: print "Mission sent successfully to the drone.Executing setpoints." try: rospy.wait_for_service( '/flytos/navigation/waypoint_execute') handle = rospy.ServiceProxy( '/flytos/navigation/waypoint_execute', WaypointExecute) req_msg = WaypointExecuteRequest() resp = handle(req_msg) message = "Executing mission!" return resp.success, resp.message except Exception as e: print e return False, str(e) except Exception as e: message = "Error sending waypoints to the drone" + " " + str(e) return False, message