def setUp(self): rospy.init_node('test_node', anonymous=True) self.rate = rospy.Rate(10) # 10hz self.has_global_pos = False self.local_position = PoseStamped() self.global_position = NavSatFix() self.extended_state = ExtendedState() self.home_alt = 0 self.mc_rad = 5 self.fw_rad = 60 self.fw_alt_rad = 10 self.last_alt_d = 9999 self.last_pos_d = 9999 self.mission_name = "" # need to simulate heartbeat for datalink loss detection rospy.Timer(rospy.Duration(0.5), self.send_heartbeat) rospy.wait_for_service('mavros/cmd/command', 30) self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True) self._srv_wp_push = rospy.ServiceProxy('mavros/mission/push', WaypointPush, persistent=True) rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback) rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback) rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback)
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 _are_agents_on_ground(self): topics = [t for t in self.topics if 'mavros/extended_state' in t] for topic in topics: extended = ExtendedState() extended = rospy.wait_for_message(topic, ExtendedState) drone = get_drone(topic) if extended.landed_state != extended.LANDED_STATE_ON_GROUND: rospy.logerr( '{} disarm failed. Vehicle not on ground.'.format(drone)) return False return True
def setUp(self): self.rate = rospy.Rate(10) # 10hz self.has_global_pos = False self.global_position = NavSatFix() self.extended_state = ExtendedState() self.altitude = Altitude() self.state = State() self.mc_rad = 5 self.fw_rad = 60 self.fw_alt_rad = 10 self.last_alt_d = None self.last_pos_d = None self.mission_name = "" self.sub_topics_ready = { key: False for key in ['global_pos', 'home_pos', 'ext_state', 'alt', 'state'] } # setup ROS topics and services try: rospy.wait_for_service('mavros/mission/push', 30) rospy.wait_for_service('mavros/cmd/arming', 30) rospy.wait_for_service('mavros/set_mode', 30) except rospy.ROSException: self.fail("failed to connect to mavros services") self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', WaypointPush) self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode) 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.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) # need to simulate heartbeat to prevent datalink loss detection self.hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message( mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) self.hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) self.hb_ros_msg = mavlink.convert_to_rosmsg(self.hb_mav_msg) self.hb_thread = Thread(target=self.send_heartbeat, args=()) self.hb_thread.daemon = True self.hb_thread.start()
def __init__(self,debug,rate): rospy.Subscriber('/mavros/state', State, self.stateCb) rospy.Subscriber('/mavros/extended_state', ExtendedState, self.extstateCb) rospy.Subscriber('/mavros/rc/in', RCIn, self.rcin_update) self.modeHandle = rospy.ServiceProxy("/mavros/set_mode", SetMode) self.armHandle= rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.landHandle= rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) self.currentState = State() self.extState = ExtendedState() self.rate = rate self.debug = debug self.rcResumeSw = False
def __init__(self, controller_name='mpc'): # super(offboard_controller, self).__init__() # only workable for python3 # mavros_core_function.__init__(self, ) # handle default mavros info self.last_request = None self.state = State() self.extended_state = ExtendedState() ## Topics self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.Subscriber('/mavros/state', State, self.mavros_state_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) ## Service self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode) ## Timer self.rate = rospy.Rate(50) if controller_name == 'pid': rospy.wait_for_service('uav_pid_server') self.control_state_check_function = rospy.ServiceProxy( 'uav_pid_server', SetBool) self.controller_state = self.control_state_check_function(True) # print(self.controller_state) elif controller_name == 'mpc': rospy.wait_for_service('uav_mpc_server') self.control_state_check_function = rospy.ServiceProxy( 'uav_mpc_server', SetBool) self.controller_state = self.control_state_check_function(True) else: rospy.info("Unknown controller") # self.set_local_position() if self.controller_state.success: # setup the arm and ready for the flight # self.set_arm(True, 10) self.offboard_ready = True else: rospy.loginfo( 'UAV controller server is not ready yet, please launch a controller first' ) self.offboard_ready = False
def __init__(self): self.state = State() self.altitude = Altitude() self.global_position = HomePosition() self.local_position = PoseStamped() #fcu local position self.local_position_odom = Odometry() self.extended_state = ExtendedState() self.mode = '' self.rate = rospy.Rate(10) self.service_timeout = 30 self.offboard_request_threshold = rospy.Duration(5.0) self.arming_request_threshold = rospy.Duration(5.0) self.imu = Imu() self.current_yaw = None self.setup_pubsub() self.setup_services() self.current_target_pose = PositionTarget() self.current_pose = Point() self.home_pose = Point(0.0,0.0,0.0) self.waypoint_1 = Point(0.0,0.0,3.0) self.should_start_mission = False
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): self.altitude = Altitude() self.extended_state = ExtendedState() self.global_position = NavSatFix() self.local_position = PoseStamped() self.state = State() self.mav_type = None self.local_velocity = TwistStamped() self.sub_topics_ready = { key: False for key in ['alt', 'ext_state', 'global_pos', 'local_pos', '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) # Subscribers self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) self.local_vel_sub = rospy.Subscriber( 'mavros/local_position/velocity_body', TwistStamped, self.local_vel_callback) # Publishers self.desired_vel = TwistStamped() self.desired_vel.twist.angular.z = 0 self.desired_vel.twist.linear.x = 0 self.desired_vel.twist.linear.y = 0 self.desired_vel.twist.linear.z = 0 #self.radius = 0.3 #self.yaw_th = 0.05 self.yaw_current = 0 self.isLand = False self.yawrate = 0.5 self.xvel = 0.5 self.z_target = 0 self.z_in = 0.1 self.z_ub = 2.5 # Height upper bound self.z_lb = 0.5 # Height lower bound self.z_p_gain = 1.0 self.cmd_vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=1) # send setpoints in seperate thread to better prevent failsafe self.pos_thread = Thread(target=self.send_vel, args=()) self.pos_thread.daemon = True self.pos_thread.start() # Keyboard input thread self.key_thread = Thread(target=self.keyInput, args=()) self.key_thread.daemon = True self.key_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.local_position = PoseStamped() self.state = State() self.mav_type = None self.prev_position_x = 0 self.prev_position_y = 0 self.prev_position_z = 0 self.drift_x = 0 self.drift_y = 0 self.drift_z = 0 self.drift_threshold = 0.07 self.IsDrift = False self.sub_topics_ready = { key: False for key in ['alt', 'ext_state', 'global_pos', 'local_pos', '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.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, self.altitude_callback) self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self.global_position_callback) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.ext_state_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, self.extended_state_callback) # Publisher self.desired_pos = PoseStamped() self.desired_pos.pose.position.x = 0 self.desired_pos.pose.position.y = 0 self.desired_pos.pose.position.z = 0 self.radius = 0.3 self.yaw_th = 0.05 self.z_in = 0.1 self.z_ub = 2.7 # Height upper bound self.z_lb = 0.5 # Height lower bound 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, context): super(PosTeleop, self).__init__(context) # Give QObjects reasonable names self.setObjectName('PosTeleop') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) if not args.quiet: print 'arguments: ', args print 'unknowns: ', unknowns # Create QWidget self._widget = QWidget() # Get path to UI file which should be in the "resource" folder of this package ui_file = os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'), 'resource', 'PositionTeleop.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('PosTeleopUi') # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # Add logo pixmap = QPixmap( os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'), 'resource', 'jderobot.png')) self._widget.img_logo.setPixmap(pixmap.scaled(121, 121)) # Set Variables self.play_code_flag = False self.takeoff = False self._widget.term_out.setReadOnly(True) self._widget.term_out.setLineWrapMode(self._widget.term_out.NoWrap) # Set functions for each GUI Item self._widget.takeoffButton.clicked.connect(self.call_takeoff_land) self._widget.stopButton.clicked.connect(self.stop_drone) self._widget.playButton.clicked.connect(self.call_play) self._widget.posControlButton.clicked.connect(self.set_pos) # Add Publishers self.play_stop_pub = rospy.Publisher('gui/play_stop', Bool, queue_size=1) # Add global variables self.extended_state = ExtendedState() self.shared_pose_msg = Pose() self.shared_twist_msg = Twist() self.current_pose = Pose() self.pose_frame = '' self.current_twist = Twist() self.twist_frame = '' self.is_running = True self.stop_icon = QIcon() self.stop_icon.addPixmap( QPixmap( os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'), 'resource', 'stop.png')), QIcon.Normal, QIcon.Off) self.play_icon = QIcon() self.play_icon.addPixmap( QPixmap( os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'), 'resource', 'play.png')), QIcon.Normal, QIcon.Off) self.sensors_widget = SensorsWidget(self) self._widget.sensorsCheck.stateChanged.connect( self.show_sensors_widget) # Add widget to the user interface context.add_widget(self._widget) # Add Subscribers rospy.Subscriber('drone_wrapper/local_position/pose', PoseStamped, self.pose_stamped_cb) rospy.Subscriber('drone_wrapper/local_position/velocity_body', TwistStamped, self.twist_stamped_cb) rospy.Subscriber('drone_wrapper/extended_state', ExtendedState, self.extended_state_cb) # Add Timer self.update_status_info()
def __init__(self, screen): rospy.init_node('status_node', argv=sys.argv) self.rate = rospy.get_param('~rate', default=1.0) # Curses setup self.screen = curses.initscr() self.rows, self.cols = self.screen.getmaxyx() height_status = 15 self.status = curses.newwin(height_status, self.cols, 1, 2) # self.console = curses.newwin(self.rows - height_status, self.cols, 12, 2) self.lines = 0 self.text = '' self.screen.keypad(True) curses.curs_set(False) # Hide cursor colors = [ curses.COLOR_BLACK, curses.COLOR_BLUE, curses.COLOR_CYAN, curses.COLOR_GREEN, curses.COLOR_MAGENTA, curses.COLOR_RED, curses.COLOR_WHITE, curses.COLOR_YELLOW ] # Curses color setup curses.use_default_colors() for color in colors: curses.init_pair(color, color, -1) # Default variables self.status_battery_perc = None self.state = State() self.state_sub = rospy.Subscriber('mavros/state', State, callback=self.state_callback, queue_size=1) self.battery = BatteryState() self.battery_sub = rospy.Subscriber('mavros/battery', BatteryState, callback=self.battery_callback, queue_size=1) self.extended = ExtendedState() self.extended_sub = rospy.Subscriber('mavros/extended_state', ExtendedState, callback=self.extended_callback, queue_size=1) # self.statustext = StatusText() # self.statustext_sub = rospy.Subscriber('mavros/statustext/recv', StatusText, # callback=self.statustext_callback, # queue_size=1) self.gps = NavSatFix() self.gps_sub = rospy.Subscriber('mavros/global_position/raw/fix', NavSatFix, callback=self.gps_callback, queue_size=1) self.local_pose = PoseStamped() self.local_pose_sub = rospy.Subscriber( 'mavros/local_position/pose', PoseStamped, callback=self.local_pose_callback, queue_size=1) self.global_pose = PoseStamped() self.global_pose_sub = rospy.Subscriber( 'global_position/pose', PoseStamped, callback=self.global_pose_callback, queue_size=1) self.diagnostics = DiagnosticArray() self.diagnostic_gps = DiagnosticStatus() self.diagnostics_sub = rospy.Subscriber( '/diagnostics', DiagnosticArray, callback=self.diagnostics_callback, queue_size=1) self.setpoint = PositionTarget() self.setpoint_sub = rospy.Subscriber('mavros/setpoint_raw/local', PositionTarget, callback=self.setpoint_callback, queue_size=1) self.cameras = ['front', 'right', 'back', 'left'] self.image_subscribers = [] self.images = {c: deque(maxlen=10) for c in self.cameras} for camera in self.cameras: topic = f'camera_{camera}/image_raw' subscriber = rospy.Subscriber(topic, Image, callback=self.image_callback, callback_args=camera, queue_size=1, buff_size=2**24) self.image_subscribers.append(subscriber)
def __init__(self, name='drone', ns='', verbose=False): if name != 'rqt': if verbose: rospy.init_node(name, anonymous=True, log_level=rospy.DEBUG) else: rospy.init_node(name) self.ns = ns self.state = State() self.extended_state = ExtendedState() self.pose_stamped = PoseStamped() self.vel_body_stamped = TwistStamped() self.rate = rospy.Rate(20) self.setpoint_raw = PositionTarget() self.setpoint_raw_flag = False self.vz_factor = 0.4 self.bridge = CvBridge() self.is_z = False self.is_xy = False self.posx = 0 self.posy = 0 self.height = 0 self.vx = 0 self.vy = 0 self.vz = 0 self.setpoint_raw_timer = rospy.Timer(rospy.Duration(nsecs=50000000), self.repeat_setpoint_raw) self.setpoint_raw_timer.shutdown() self.stay_armed_stay_offboard_timer = rospy.Timer( rospy.Duration(5), self.stay_armed_stay_offboard_cb) self.stay_armed_stay_offboard_timer.shutdown() rospy.wait_for_service(self.ns + 'mavros/cmd/arming') self.arm_client = rospy.ServiceProxy(ns + 'mavros/cmd/arming', CommandBool) rospy.wait_for_service(self.ns + 'mavros/set_mode') self.mode_client = rospy.ServiceProxy(ns + 'mavros/set_mode', SetMode) rospy.wait_for_service(self.ns + 'mavros/cmd/land') self.land_client = rospy.ServiceProxy(ns + 'mavros/cmd/land', CommandTOL) self.rqt_extended_state_publisher = rospy.Publisher( self.ns + 'drone_wrapper/extended_state', ExtendedState, queue_size=1) self.rqt_pose_publisher = rospy.Publisher( self.ns + 'drone_wrapper/local_position/pose', PoseStamped, queue_size=1) self.rqt_velocity_body_publisher = rospy.Publisher( self.ns + 'drone_wrapper/local_position/velocity_body', TwistStamped, queue_size=1) self.rqt_cam_frontal_publisher = rospy.Publisher( self.ns + 'drone_wrapper/cam_frontal/image_raw', Image, queue_size=1) self.rqt_cam_ventral_publisher = rospy.Publisher( self.ns + 'drone_wrapper/cam_ventral/image_raw', Image, queue_size=1) rospy.Subscriber(self.ns + 'mavros/state', State, self.state_cb) rospy.Subscriber(self.ns + 'mavros/extended_state', ExtendedState, self.extended_state_cb) rospy.Subscriber(self.ns + 'mavros/local_position/pose', PoseStamped, self.pose_stamped_cb) rospy.Subscriber(self.ns + 'mavros/local_position/velocity_body', TwistStamped, self.vel_body_stamped_cb) rospy.Subscriber(self.ns + 'mavros/global_position/global', NavSatFix, self.global_position_cb) cam_frontal_topic = rospy.get_param('cam_frontal_topic', '/iris/cam_frontal/image_raw') cam_ventral_topic = rospy.get_param('cam_ventral_topic', '/iris/cam_ventral/image_raw') rospy.Subscriber(cam_frontal_topic, Image, self.cam_frontal_cb) rospy.Subscriber(cam_ventral_topic, Image, self.cam_ventral_cb) self.setpoint_raw_publisher = rospy.Publisher( self.ns + 'mavros/setpoint_raw/local', PositionTarget, queue_size=1)
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 __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()
class OffbPosCtl: curr_pose = PoseStamped() des_pose = PoseStamped() init_pose = PoseStamped() curr_state = ExtendedState() def __init__(self): rospy.init_node('offboard_sequences', anonymous=True) self.pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) self.pose_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback=self.pose_cb) self.ext_state_sub = rospy.Subscriber('/mavros/extended_state', ExtendedState, callback=self.ext_state_cb) self.rate = rospy.Rate(10) # Hz self.rate.sleep() self.init_pose = self.copy_pose(self.curr_pose) self.des_pose = self.copy_pose(self.curr_pose) print self.init_pose def takeoff(self): """ UAV will takeoff to a height defined as TAKEOFF_HEIGHT, maintaining the initial x,y coordinates and orientation. """ if not rospy.is_shutdown(): print "offboard takeoff initiated" init_time = rospy.get_time() while rospy.Duration.from_sec(rospy.get_time() - init_time ) < rospy.Duration(HOVER_DURATION): if self.curr_pose.pose.position.z < TAKEOFF_HEIGHT and self.des_pose.pose.position.z < TAKEOFF_HEIGHT: self.des_pose.pose.position.z += MOTION_INCREMENT else: self.des_pose.pose.position.z = TAKEOFF_HEIGHT self.pose_pub.publish(self.des_pose) # print self.des_pose self.rate.sleep() def land(self): """ UAV will land at the current x,y position, maintaining orientation. """ print "offboard landing initiated" init_z = self.init_pose.pose.position.z landed = False while not rospy.is_shutdown(): while not landed: if self.curr_state.landed_state != 1: self.des_pose.pose.position.z -= MOTION_INCREMENT self.pose_pub.publish(self.des_pose) # print self.des_pose self.rate.sleep() else: landed = True landed_pose = self.copy_pose(self.curr_pose) # print "landed: " + str(landed) self.pose_pub.publish( landed_pose) # TODO quad moves after landing. change mode? # print landed_pose self.rate.sleep() def circle(self): """ UAV will complete a circular path, around (0,0) at the current height and orientation. """ print "circle path initiated" t_init = rospy.get_time() t = rospy.Duration(0) while t <= rospy.Duration(7): self.des_pose.pose.position.x = CIRCLE_RAD * 1 self.des_pose.pose.position.y = CIRCLE_RAD * 0 self.pose_pub.publish(self.des_pose) # print self.des_pose self.rate.sleep() t = rospy.Duration.from_sec(rospy.get_time() - t_init) t_init = rospy.get_time() t = 0 while t <= NCIRCLES: theta = t * 2 * math.pi self.des_pose.pose.position.x = CIRCLE_RAD * math.cos(theta) self.des_pose.pose.position.y = CIRCLE_RAD * math.sin(theta) self.pose_pub.publish(self.des_pose) # print self.des_pose self.rate.sleep() t = (rospy.get_time() - t_init) / CIRCLE_DURATION def square(self): """ UAV will complete a square path, around (0,0) at the current height and orientation. """ print "square path initiated" SIDE = 2 HALF_SIDE = SIDE / 2 corners = [(HALF_SIDE, HALF_SIDE), (-HALF_SIDE, HALF_SIDE), (-HALF_SIDE, -HALF_SIDE), (HALF_SIDE, -HALF_SIDE)] for corner in corners: t_init = rospy.get_time() t = rospy.Duration(0) while t <= rospy.Duration(8): self.des_pose.pose.position.x = corner[0] self.des_pose.pose.position.y = corner[1] self.pose_pub.publish(self.des_pose) # print self.des_pose self.rate.sleep() t = rospy.Duration.from_sec(rospy.get_time() - t_init) def copy_pose(self, pose): """ creates a copy of a PoseStamped object """ pt = pose.pose.position quat = pose.pose.orientation copied_pose = PoseStamped() copied_pose.header.frame_id = pose.header.frame_id copied_pose.header.stamp = rospy.Time.now() copied_pose.pose.position = Point(pt.x, pt.y, pt.z) copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z, quat.w) return copied_pose def pose_cb(self, msg): # print msg self.curr_pose = msg def ext_state_cb(self, msg): self.curr_state = msg
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