def heartbeat(hsock): request = messages_pb2.Request() request.command = messages_pb2.Command.HEART_BEAT data = request.SerializeToString() print(len(data)) hsock.sendto(data, ('127.0.0.1', 4444)) received = hsock._recv_msg(1024) reply = messages_pb2.Reply() reply.ParseFromString(received) a = BytesIO() a.write(reply.heartbeatData.mavrosState) mavros_state = State() mavros_state.deserialize(reply.heartbeatData.mavrosState) print (reply) print (mavros_state)
def __init__(self): # Reqired call - I think if you are running more than one drone it allows you to give them different topic namespaces, but defaults to mavros mavros.set_namespace() self._curr_state = State() self._is_done = False self.flight_path = [] self.rate = rospy.Rate(20) self.nextPos = PoseStamped() self.lat = 0 self.lng = 0 # Various ros publishers and subscribers - somewhat self documenting self.gps_subscriber = rospy.Subscriber( mavros.get_topic('global_position', 'global'), NavSatFix, self._get_fix) self.state_subscriber = rospy.Subscriber("mavros/state", State, self._update_state) self._local_position_publisher = rospy.Publisher( "mavros/setpoint_position/local", PoseStamped, queue_size=10) self._arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) self._set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode) self._takeoff_client = rospy.ServiceProxy("mavros/cmd/takeoff", CommandTOL)
def __init__(self): self.state = "disarm" #создаем топики, для публикации управляющих значений: self.pub_pt = rospy.Publisher(f"/mavros{1}/setpoint_raw/local", PositionTarget, queue_size=10) self.pt = PositionTarget() self.pt.coordinate_frame = self.pt.FRAME_LOCAL_NED self.t0 = time.time() self.dt = 0 #params self.p_gain = 2 self.max_velocity = 5 self.arrival_radius = 0.6 self.waypoint_list = [ np.array([6., 7., 6.]), np.array([0., 14., 7.]), np.array([18., 14., 7.]), np.array([0., 0., 5.]) ] self.current_waypoint = np.array([0., 0., 0.]) self.pose = np.array([0., 0., 0.]) self.velocity = np.array([0., 0., 0.]) self.mavros_state = State() self.subscribe_on_topics()
def setup_mavros(self): self.mavros_state = State() self.mavros_state.connected = False self.arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) self.landing_client = rospy.ServiceProxy('mavros/cmd/land', CommandTOL) self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) self.pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.position_cb) self.vel_sub = rospy.Subscriber( '/mavros/local_position/velocity_local', TwistStamped, self.velocity_cb) self.global_pos_sub = rospy.Subscriber( '/mavros/global_position/global', NavSatFix, self.global_position_cb) self.state.pose.pose.orientation.x = 0.0 self.state.pose.pose.orientation.y = 0.0 self.state.pose.pose.orientation.z = 0.0 self.state.pose.pose.orientation.w = 1.0
def __init__(self): # Communication variables self._last_pose = PoseStamped() self._setpoint_msg = PositionTarget() self._setpoint_msg.type_mask = _DEFAULT self._last_state = State() # ROS Communication self._setpoint_pub = rospy.Publisher("mavros/setpoint_raw/local", PositionTarget, queue_size=1) self._pose_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped, self._pose_cb) self._state_sub = rospy.Subscriber('mavros/state', State, self._state_cb) # wait for mavros to start publishing rospy.logdebug("Waiting for MAVROS to start") rospy.wait_for_message("mavros/mission/waypoints", WaypointList) # Make drone less aggressive rospy.wait_for_service("mavros/param/set") mavparam = rospy.ServiceProxy('mavros/param/set', ParamSet) mavparam("MC_PITCH_P", ParamValue(0, 2.0)).success mavparam("MC_ROLL_P", ParamValue(0, 2.0)).success mavparam("MPC_XY_VEL_MAX", ParamValue(0, 3.0)).success # Send a few setpoint to make MAVROS happy rate = rospy.Rate(20.0) for _ in range(40): rate.sleep() self._publish_setpoint(None) rospy.Timer(rospy.Duration(0.05), self._publish_setpoint) rospy.loginfo("Drone Initialized")
def __init__(self, control_reference_frame='bu'): # Create node with name 'controller' rospy.init_node('translation_controller') if control_reference_frame not in _COORDINATE_FRAMES: raise ValueError("Invalid control reference frame: " + control_reference_frame) self.control_reference_frame=control_reference_frame # A subscriber to the topic '/mavros/local_position/pose. self.pos_sub_cb is called when a message of type 'PoseStamped' is recieved self.pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.pos_sub_cb) # Quaternion representing the rotation of the drone's body frame (bu) in the LENU frame. # Initiallize to identity quaternion, as bu is aligned with lenu when the drone starts up. self.quat_bu_lenu = (0, 0, 0, 1) # A subscriber to the topic '/mavros/state'. self.state_sub_cb is called when a message of type 'State' is recieved self.state_sub = rospy.Subscriber("/mavros/state", State, self.state_sub_cb) # Flight mode of the drone ('OFFBOARD', 'POSCTL', 'MANUAL', etc.) self.mode = State().mode # A publisher which will publish the desired linear and anglar velocity to the topic '/setpoint_velocity/cmd_vel_unstamped' self.velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size = 1) # Initialize linear setpoint velocities self.vx = 0 self.vy = 0 self.vz = 0 # Publishing rate self.rate = rospy.Rate(_RATE) # Boolean used to indicate if the streaming thread should be stopped self.stopped = False
def __init__(self): #rospy.init_node('align') self.drone_state = State() #rospy.init_node('align_reference', anonymous=True) self.rate = rospy.Rate(20) # 20hz rospy.Subscriber("/cv_detection/color_range/detection", Point, self.point_callback, queue_size=None) # rospy.Subscriber( # "/control/align_reference/set_image_shape", Point, self.set_image_shape, queue_size=None) # rospy.Subscriber( # "/control/align_reference/set_goal_point", Point, self.set_goal_point, queue_size=None) # rospy.Subscriber( # "/control/align_reference/set_camera_angle", Float, self.set_camera_angle, queue_size=None) self.setpoint_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=1) self.alginned = rospy.Publisher( "/control/align_reference/aligned", Bool, queue_size=1) ''' Usar, condicionalmente /mavros/state e drone_state Parametrizar estado /mavros/state com rosparam ''' rospy.Subscriber("/mavros/state", State, self.state_callback, queue_size=1) self.enable = "" rospy.Subscriber("pack/node/set", String, self.enable_cb, queue_size=None) self.return_pub = rospy.Publisher("pack/node/return", String, queue_size=1)
def __init__(self): # Communication variables self._last_pose = None self._last_twist = None self._setpoint_msg = PositionTarget() self._last_state = State() self._camera = Camera(imgtopic="drone/camera_front/image", infotopic="drone/camera_front/info") # ROS Communication self._setpoint_pub = rospy.Publisher("mavros/setpoint_raw/local", PositionTarget, queue_size=1) self._pose_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped, self._pose_cb) self._twist_sub = rospy.Subscriber("mavros/local_position/velocity", TwistStamped, self._twist_cb) self._state_sub = rospy.Subscriber('mavros/state', State, self._state_cb) self._publish_setpoint_active = False rospy.Timer(rospy.Duration(0.05), self._publish_setpoint) rospy.loginfo("Drone Initialized")
def __init__(self): rospy.init_node("mav_control_node") rospy.Subscriber("/mavros/global_position/global",NavSatFix,self.gps_callback) rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback) rospy.Subscriber('mavros/state',State,self.arm_callback) rospy.Subscriber('mavros/battery',BatteryState,self.battery_callback) rospy.Subscriber('mavros/global_position/raw/satellites',UInt32,self.sat_num_callback) rospy.Subscriber('mavros/global_position/rel_alt',Float64,self.rel_alt_callback) rospy.Subscriber('mavros/imu/data',Imu,self.imu_callback) rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback) rospy.Subscriber('mavros/vfr_hud',VFR_HUD,self.hud_callback) self.gps = NavSatFix() self.pose = PoseStamped() self.arm_status = State() self.battery_status = BatteryState() self.sat_num = UInt32() self.rel_alt = Float64() self.imu = Imu() self.rc = RCIn() self.hud = VFR_HUD() self.timestamp = rospy.Time() self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1) self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1) self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1) self.mode_service = rospy.ServiceProxy("/mavros/set_mode", SetMode) self.arm_service = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool) self.takeoff_service = rospy.ServiceProxy("/mavros/cmd/takeoff", CommandTOL) self.stream_service = rospy.ServiceProxy("/mavros/set_stream_rate", StreamRate) self.home_service = rospy.ServiceProxy("/mavros/cmd/set_home", CommandHome)
def __init__(self): #initializing the class variables self.pose = PoseStamped() self.glob_pos = NavSatFix() self.velocity = TwistStamped() self.state = State() #self.bridge = CvBridge() # streaming of subscribers rospy.wait_for_service("mavros/set_stream_rate") set_stream_rate = rospy.ServiceProxy("mavros/set_stream_rate", StreamRate) set_stream_rate(StreamRateRequest.STREAM_POSITION, 50, True) set_stream_rate(StreamRateRequest.STREAM_EXTRA1, 50, True) #camera #self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.callback) # position rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.process_position) # global position rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.glob_posi) #mode rospy.Subscriber("/mavros/state", State, self.process_state) # velocity rospy.Subscriber("/mavros/local_position/velocity_local", TwistStamped, self.process_velocity)
def __init__(self, controller, a_id, namespace, address, rx_port): self.calibration_path = Path() self.controller = controller self.a_id = a_id self.namespace = namespace self.local_map_name = '%s_map' % (namespace, ) self.local_map_transform = TransformStamped() self.local_map_transform_calculated = False self.heartbeat_time = None self.command_id = 1 self.mavros_state = MavrosState() self.local_pose = PoseStamped() self.global_position = NavSatFix() self.rx_port = rx_port self._create_socket(address) self.recv_q = Queue.Queue() self.recv_thread = threading.Thread(target=self._recv_thread) self.recv_thread.start() self.publishers = {} self.subscribers = {} self._register_publisher() self._register_subscribers() self._create_calibration_path() self.calibration_poses = [] self.command_thread = None self.last_command = (1, True ) # last_command_id, last_command_completed rospy.loginfo("%s: Initialized", self.namespace)
def network_state_changed_callback(self, new_num_nodes): self.num_nodes_in_swarm = new_num_nodes for i in range(1, self.num_nodes_in_swarm.data + 1): uav_name = "uav" + str(i) if (i != self.id ): # Create objects to use to subscribe to neighbor stats if i not in self.neighbor_position_objects.keys(): self.neighbor_position_objects[str(i)] = PoseStamped() self.neighbor_global_position_objects[str(i)] = NavSatFix() self.neighbor_velocity_objects[str(i)] = TwistStamped() self.neighbor_state_objects[str(i)] = State() if i not in self.neighbor_position_objects.keys(): self.neighbor_state_sub[str(i)] = rospy.Subscriber( uav_name + "/mavros/state", State, self.neighbor_state_changed_callback, i) self.neighbor_pos_sub[str(i)] = rospy.Subscriber( uav_name + "/mavros/local_position/pose", PoseStamped, self.neighbor_position_changed_callback, i) self.neighbor_vel_sub[str(i)] = rospy.Subscriber( uav_name + "/mavros/local_position/velocity_local", TwistStamped, self.neighbor_velocity_changed_callback, i) self.neighbor_global_pos_sub[str(i)] = rospy.Subscriber( uav_name + "/mavros/global_position/global", NavSatFix, self.neighbor_global_pos_changed_callback, i)
def __init__(self): self.pose = PoseStamped() self.radius = 0.1 self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=1) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_cb) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) service_timeout = 30 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/set_mode', service_timeout) except rospy.ROSInterruptException: self.fail("failed to connect to service") self.arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) # callback method for state sub self.current_state = State() self.offb_set_mode = SetMode #new threading for self.send_pos. and daemon with true means the threading while stoped with the main one. self.pos_thread = Thread(target=self.send_pos, args=()) self.pos_thread.daemon = True self.pos_thread.start()
def setUp(self): self.local_position = PoseStamped() self.state = State() self.pos = PoseStamped() self.sub_topics_ready = { key: False for key in ['local_pos', 'home_pos', 'state'] } # setup ROS topics and services try: 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.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('/mavros/set_mode', SetMode) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.pos_setpoint_pub = rospy.Publisher( 'mavros/setpoint_position/local', PoseStamped, queue_size=10) # 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, drone_id): # self.id = 0 self.id = drone_id self.id_state = 0 self.id_pub = rospy.Publisher("ID", Int32) self.arming_cmd = CommandBool() self.arming_cmd.value = True rospy.init_node("controller", anonymous=True) rospy.Subscriber("request_ID", Int32, self.get_id, queue_size=1) time.sleep(1) while self.id_state == 0: self.id_pub.publish(self.id_state) print("Waiting for ID") time.sleep(0.5) self.rate = rospy.Rate(20) print("ID received", self.id) rospy.Subscriber(str(self.id) + "/mavros/state", State, self.state_cb) rospy.Subscriber( str(self.id) + "/mavros/local_position/pose", PoseStamped, self.update_pose) self.set_mode_client = rospy.ServiceProxy( str(self.id) + "/mavros/set_mode", SetMode) self.local_pos_pub = rospy.Publisher(str(self.id) + "/mavros/setpoint_position/local", PoseStamped, queue_size=10) self.state = State() self.pose = PoseStamped() self.mode = self.state.mode self.target = PoseStamped() time.sleep(2)
def init(mission): global state, setpoint, run, pose, home, init global arming_client, set_mode_client #Global variable initialisation state, pose, setpoint, home = State(), PoseStamped(), Point(), Point() run = True init = True # Node initiation rospy.init_node(NODE_NAME) rospy.loginfo('node initializeed') time.sleep(1) # setting up the pins for electromagnet wpi.wiringPiSetup() wpi.pinMode(0,1) # Publishers, subscribers and services pose_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, Pose_Callback) state_sub = rospy.Subscriber('/mavros/state', State, State_Callback) rospy.wait_for_service('mavros/set_mode') set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) rospy.wait_for_service('mavros/cmd/arming') arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) # Thread to send setpoints - mission execution tSetPoints = Thread(target=sendSetpoint,args=(mission,)).start() # Monitor security while not rospy.is_shutdown(): InterfaceKeyboard()
def __init__(self): # rospy.loginfo("ARDistChecker Started!") self.ar_pose_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, self.ar_pose_cb) # A subscriber to the topic '/mavros/state'. self.state is called when a message of type 'State' is recieved self.state_subscriber = rospy.Subscriber("/mavros/state", State, self.update_state) # State of the drone. self.state.mode = flight mode, self.state.armed = are motors armed (True or False), etc. self.state = State() # List of all the "captured" markers self.captured = [] # The closest marker to the drone (in the z direction) self.marker = None # Points to the next marker in Constants.MARKERS that needs to be captured self.index = 0 if len(MARKERS) > 0: # True if all markers have been captured, Fasle otherwise self.done = False else: rospy.loginfo("DONE!") self.done = True
def __init__(self, mav_name, mav_type="mavros"): #rospy.init_node("MAV_" + mav_name) self.rate = rospy.Rate(20) self.drone_pose = PoseStamped() self.goal_pose = PoseStamped() self.goal_vel = TwistStamped() self.drone_state = State() self.battery = BatteryState() ############## Publishers ############### self.local_position_pub = rospy.Publisher( CONFIG[mav_type + "_local_position_pub"], PoseStamped, queue_size=20) self.velocity_pub = rospy.Publisher(CONFIG[mav_type + "_velocity_pub"], TwistStamped, queue_size=5) self.mavlink_pub = rospy.Publisher('/mavlink/to', Mavlink, queue_size=1) ########## Subscribers ################## self.local_atual = rospy.Subscriber(CONFIG[mav_type + "_local_atual"], PoseStamped, self.local_callback) self.state_sub = rospy.Subscriber(CONFIG[mav_type + "_state_sub"], State, self.state_callback) self.battery_sub = rospy.Subscriber("/mavros/battery", BatteryState, self.battery_callback) ############# Services ################## self.arm = rospy.ServiceProxy(CONFIG[mav_type + "_arm"], mavros_msgs.srv.CommandBool) self.set_mode = rospy.ServiceProxy(CONFIG[mav_type + "_set_mode"], mavros_msgs.srv.SetMode)
def __init__(self): self.local_pose_subscriber_prev = 0 startup_start = timeit.default_timer() print('start') # Rate init self.rate = rospy.Rate(20.0) # MUST be more then 2Hz self.current_state = State() # Current state sub self.state_sub = rospy.Subscriber("mavros/state", State, self.state_subscriber_callback) # Init last_request self.last_request = rospy.get_rostime() # wait for FCU connection print('Waiting for FCU connection...') while not self.current_state.connected: self.rate.sleep() print('Connection sucessful!!') startup_stop = timeit.default_timer() print('startup time is' + str(startup_stop - startup_start)) # Quadcopter imu subscriber init attitude_target_sub = rospy.Subscriber( "/mavros/setpoint_raw/target_attitude", AttitudeTarget, self.attitude_target_sub_callback)
def __init__(self, rate=20): # Rate has to be greater than 2Hz for px4 self.rate = rospy.Rate(rate) self.current_state = State() self.pose = PoseStamped() self.setpoint_pos = PoseStamped() self.vel = TwistStamped() self.setpoint_vel = Twist() self.state_sub = rospy.Subscriber("mavros/state", State, callback=self.state_cb) self.pose_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped, callback=self.pose_cb) self.vel_sub = rospy.Subscriber("mavros/local_position/velocity", TwistStamped, callback=self.vel_cb) self.setpoint_pos_pub = rospy.Publisher( "mavros/setpoint_position/local", PoseStamped, queue_size=10) self.setpoint_vel_pub = rospy.Publisher( "mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=10) rospy.wait_for_service('mavros/cmd/arming') self.arm_service = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) rospy.wait_for_service('mavros/set_mode') self.set_mode_service = rospy.ServiceProxy('mavros/set_mode', SetMode)
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 init(): # Input data # Output data global state, setpoint, yawSetPoint, setPointsCount, PositionsCount, \ run, laser_position_count, laserposition, pose, lasers_raw, position_control # Publishers global local_pos_pub, arming_client, set_mode_client # Objects # Global variable initialisation lasers_raw = Distance(lasers=[0, 0, 0, 0, 0, 0], status=[0, 0, 0, 0, 0, 0]) pose = PoseStamped() laserposition = PoseStamped() setpoint = Point() setpoint.x = 1 setpoint.y = 1 setpoint.z = 1 # When true, setpoints are positions # When false, setpoints is a velocity position_control = True yawSetPoint = 0 laser_position_count = 0 run = True setPointsCount = 0 PositionsCount = 0 state = State() # Node initiation rospy.init_node('laserpack_control') local_pos_pub = rospy.Publisher('mavros/mocap/pose', PoseStamped, queue_size=1) time.sleep(1) # Publishers, subscribers and services pose_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, Pose_Callback) laser_pose_sub = rospy.Subscriber('lasers/filtered', PoseStamped, laser_callback) state_sub = rospy.Subscriber('mavrqos/state', State, State_Callback) laser_sub = rospy.Subscriber('lasers/raw', Distance, lasers_raw_callback) task_sub = rospy.Subscriber('web/task', Task, Task_Callback) rospy.wait_for_service('mavros/set_mode') set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) rospy.wait_for_service('mavros/cmd/arming') arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) # Thread to send setpoints tSetPoints = Thread(target=sendSetpoint).start() # Thread to send Lidar height tLidarZ = Thread(target=sendLidar).start() # In case we want to send positions with reduced rate, just use this thread # tPositions = Thread(target=sendPosition).start() while not rospy.is_shutdown(): InterfaceKeyboard()
def __init__(self): self.imu_cb_flag = False self.vicon_cb_flag = False self.traj_cb_flag = False self.local_pose_subscriber_prev = 0 # remove startup_start = timeit.default_timer() # remove print('start') # # Rate init self.rate = rospy.Rate(10.0) # 10 Hz self.current_state = State() # Init last_request self.last_request = rospy.get_rostime() # remove # vicon yaw vicon_attitude_sub = rospy.Subscriber("/intel_aero_quad/odom", Odometry, self.vicon_attitude_sub_callback) # IMU yaw attitude_target_sub = rospy.Subscriber( "/mavros/imu/data", Imu, self.attitude_target_sub_callback) traj_yaw_sub = rospy.Subscriber("/px4_quad_controllers/traj_yaw", PoseStamped, self.traj_yaw_sub_sub_callback) self.yaw_target_pub = rospy.Publisher( "/px4_quad_controllers/yaw_setpoint", PoseStamped, queue_size=10) while not rospy.is_shutdown(): if (self.vicon_cb_flag and self.imu_cb_flag): self.vicon_yaw_sp = rospy.get_param( '/attitude_thrust_publisher/vicon_yaw_sp') # What is this? if (not self.traj_cb_flag): self.traj_yaw_sp = 3.14 self.yaw_sp = self.current_yaw_vicon - \ (self.vicon_yaw_sp + self.traj_yaw_sp) target_yaw = PoseStamped() target_yaw.header.frame_id = "home" target_yaw.header.stamp = rospy.Time.now() target_yaw.pose.position.x = self.current_yaw_imu - self.yaw_sp self.yaw_target_pub.publish(target_yaw) # print('VICON setpoint yaw = '+ str(self.vicon_yaw_sp)) # print('VICON current yaw = '+ str(self.current_yaw_vicon)) # print('quad CURRENT yaw = '+ str(self.current_yaw_imu)) # print('final setpoint yaw = \ # '+ str(target_yaw.pose.position.x)) # print('\n') self.rate.sleep()
def __init__(self): # Create node with name 'tracker' rospy.init_node('tracker') # Initialize instance of CvBridge to convert images between OpenCV images and ROS images self.bridge = CvBridge() # A subscriber to the topic '/mavros/local_position/pose. self.pos_sub_cb is called when a message of type 'PoseStamped' is recieved self.pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.pos_sub_cb) # Quaternion representing the rotation of the drone's body frame in the NED frame. initiallize to identity quaternion self.quat_bu_lenu = (0, 0, 0, 1) # A subscriber to the topic '/mavros/state'. self.state_sub_cb is called when a message of type 'State' is recieved self.state_sub = rospy.Subscriber("/mavros/state", State, self.state_sub_cb) # State of the drone. # self.state.mode = flight mode (e.g. 'OFFBOARD', 'POSCTL', etc), # self.state.armed = are motors armed (True or False), etc. self.mode = State().mode # A subscriber to the topic '/line/param'. self.line_sub_cb is called when a message of type 'Line' is recieved self.line_sub = rospy.Subscriber('/line/param', LineArray, self.line_sub_cb) # A subscriber to the topic '/aero_downward_camera/image'. self.image_sub_cb is called when a message is recieved self.camera_sub = rospy.Subscriber('/aero_downward_camera/image', Image, self.camera_sub_cb) self.image = None # A publisher which will publish an image annotated with the detected line to the topic 'line/tracker_image' self.tracker_image_pub = rospy.Publisher('/tracker_image', Image, queue_size=1) # A publisher which will publish the desired linear and anglar velocity to the topic '/setpoint_velocity/cmd_vel_unstamped' self.velocity_pub = rospy.Publisher('/tracker/vel', Twist, queue_size=1) # Linear setpoint velocities in downward camera frame self.vx__dc = 0.0 self.vy__dc = 0.0 self.vz__dc = 0.0 # Yaw setpoint velocities in downward camera frame self.wz__dc = 0.0 self.height = 0.0 # Publishing rate self.rate = rospy.Rate(_RATE) # Boolean used to indicate if the streaming thread should be stopped self.stopped = False self.error = np.array([0.0, 0.0, 0.0]) self.prev_error = np.array([0.0, 0.0, 0.0])
def __init__(self, maneuver_velocity_setpoint=np.array([0.1, 0.0, 0.0]), maneuver_reference_frame='bu', maneuver_duration=3.0): """ Object that manages velocity commands for OFFBOARD mode Attributes: - vel_sepoint_pub: rospy publisher for cmd_vel_unstamped topic - vel_setpoint_bu_lenu__lenu: Twist message for desired velocity of quadrotor expressed in local ENU coords in m/s - state_sub: rospy subscriber for mavros/state topic - current_state: State() object to access current flight mode - prev_state: State() object to track most recent flight mode before current - rate: command rate - offboard_point_streaming: boolean for if offboard commands should stream or not - static_transforms: StaticTransforms object to hold relative orientation of different frames (bu, bd, fc, etc) - maneuver_velocity_setpoint: desired velocity vector for manuever [m/s], related to vel_setpoint_bu_lenu__lenu - maneuver_reference_frame: frame in which maneuver_velocity_setpoint is expressed - maneuver_duration: [s] duration of maneuver """ # Create node with name 'translation_controller' and set update rate rospy.init_node('translation_controller') # A publisher which will publish the desired linear and anglar velocity to the topic '/.../cmd_vel_unstamped' self.vel_setpoint_pub = rospy.Publisher( '/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1) self.vel_setpoint_bu_lenu__lenu = Twist() # A subscriber to the topic '/mavros/state'. self.state is called when a message of type 'State' is recieved self.state_sub = rospy.Subscriber("/mavros/state", State, self.state_cb) self.current_state = State() self.prev_state = State() # A subscriber to the /mavros/local_position/pose topic that is used to access the transform between the body-up # and local ENU frames self.pose_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.pose_sub_cb) self.q_bu_lenu = None self.rate = rospy.Rate(Constants.RATE) self.offboard_point_streaming = False self.static_transforms = StaticTransforms() self.maneuver_velocity_setpoint = maneuver_velocity_setpoint self.maneuver_reference_frame = maneuver_reference_frame self.maneuver_duration = maneuver_duration
def __init__(self, hz=60): rospy.loginfo("ARObstacleController Started!") mavros.set_namespace() self.feed_sub = rospy.Subscriber( "/camera/rgb/image_rect_color/compressed", CompressedImage, self.image_cb) self.state_sub = rospy.Subscriber("/mavros/state", State, self.state_cb) self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_cb) self.local_pose_sp_pub = rospy.Publisher( "/mavros/setpoint_position/local", PoseStamped, queue_size=1) if _INTEGRATED: self.local_vel_sp_pub = rospy.Publisher("/ar_vel", TwistStamped, queue_size=1) else: self.local_vel_sp_pub = rospy.Publisher( "/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=1) self.pub_error = rospy.Publisher( "/obs_error", Twist, queue_size=1) # set data type to publish to error self.ar_vel = rospy.Publisher("/ar_vel", TwistStamped, queue_size=1) self.ar_pose_sub = rospy.Subscriber("/ar_aero_pose", AlvarMarkers, self.ar_pose_cb) self.rate = rospy.Rate(hz) self.current_state = State() self.current_pose = None self.current_vel = None ''' 0 is hovering in space, 1 is flying to obstacle 2 is ring flythru (marker id: 24) 3 is hurdle flyover (marker id: 12) 4 is gate flyunder (marker id: 9) ''' self.finite_state = 0 self.markers = [] self.vel_hist = [[], [], [], []] self.current_obstacle_seq = 0 self.current_obstacle_tag = None self.t_marker_last_seen = None self.t_obstacle_start = None self.local_vel_sp = TwistStamped() self.local_pose_sp = None self.offboard_vel_streaming = False self.tl = tf.TransformListener()
def __init__(self): def state_cb(state): self.current_state0 = state self.current_state0 = State() self.local_pos_pub0 = rospy.Publisher('uav0/mavros/setpoint_position/local', PoseStamped, queue_size=1) self.state_sub0= rospy.Subscriber('/uav0/mavros/state', State, state_cb) # $This topic was wrong self.arming_client0 = rospy.ServiceProxy('/uav0/mavros/cmd/arming', CommandBool) self.set_mode_client0 = rospy.ServiceProxy('uav0/mavros/set_mode', SetMode)
def __init__(self, ID=0, vehicle_cfg=[]): # super(Vehicle, self).__init__(*args) self.ID = ID self.state = State() print("Vehicle: ", vehicle_cfg['mavros']) # ROS services service_timeout = 10 rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service(vehicle_cfg['mavros'] + '/mavros/param/get', service_timeout) rospy.wait_for_service( vehicle_cfg['mavros'] + '/mavros/cmd/arming', service_timeout) rospy.wait_for_service( vehicle_cfg['mavros'] + '/mavros/mission/push', service_timeout) rospy.wait_for_service( vehicle_cfg['mavros'] + '/mavros/mission/clear', service_timeout) rospy.wait_for_service(vehicle_cfg['mavros'] + '/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( vehicle_cfg['mavros'] + '/mavros/param/get', ParamGet) self.set_arming_srv = rospy.ServiceProxy( vehicle_cfg['mavros'] + '/mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy( vehicle_cfg['mavros'] + '/mavros/set_mode', SetMode) self.wp_clear_srv = rospy.ServiceProxy( vehicle_cfg['mavros'] + '/mavros/mission/clear', WaypointClear) self.wp_push_srv = rospy.ServiceProxy( vehicle_cfg['mavros'] + '/mavros/mission/push', WaypointPush) # self.state_sub = rospy.Subscriber(vehicle_cfg['mavros'] + '/mavros/state', # State, # self.state_callback) # # ROS subscribers # self.sub_topics_ready = { # key: False # for key in [ # 'state' # ] # } # send setpoints in seperate thread to better prevent failsafe self.thread = Thread(target=self.activate, args=()) self.thread.daemon = 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): self.rate = rospy.Rate(20) self.current_state = State() rospy.Subscriber('/mavros/state', State, self.state_cb) while not rospy.is_shutdown() and not self.current_state.connected: self.rate.sleep() rospy.loginfo('Connection Successful') self.local_pose = PoseStamped() rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.pose_cb)