def send_heartbeat(self, event=None): # mav type gcs mavmsg = mavutil.mavlink.MAVLink_heartbeat_message(6, 0, 0, 0, 0, 0) # XXX: hack: using header object to set mav properties mavmsg.pack(mavutil.mavlink.MAVLink_header(0, 0, 0, 2, 1)) rosmsg = mavlink.convert_to_rosmsg(mavmsg) self.pub_mavlink.publish(rosmsg)
def read_message(self, m): if m is not None: if m.get_type() == 'COMMAND_ACK': if m.command == mavutil.mavlink.MAV_CMD_LOGGING_START and not self.got_header_section: if m.result == 0: self.logging_started = True print('Logging started. Waiting for Header...') else: raise Exception('Logging start failed', m.result) return None, 0, 0 is_newer, num_drops = self.check_sequence(m.sequence) if m.get_type() == 'LOGGING_DATA_ACKED': ackmsg = mavutil.mavlink.MAVLink_logging_ack_message( target_system=m.target_system, target_component=self.target_component, sequence=m.sequence ) ackmsg.pack(link) ros_msg = mavlink.convert_to_rosmsg(ackmsg) mavlink_pub.publish(ros_msg) if is_newer: if num_drops > 0: self.num_dropouts += num_drops if m.get_type() == 'LOGGING_DATA': if not self.got_header_section: print('\rHeader received in {:0.2f}s'.format(timer()-self.start_time)) self.logging_started = True self.got_header_section = True self.last_sequence = m.sequence return m.data[:m.length], m.first_message_offset, num_drops else: print('dup/reordered message '+str(m.sequence)) return None, 0, 0
def send_message(msg, mav, pub): """ Send a mavlink message """ msg.pack(mav) rosmsg = convert_to_rosmsg(msg) pub.publish(rosmsg) print("sent message %s" % msg)
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 send_heartbeat(self): hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message( mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) rate = rospy.Rate(2) while not rospy.is_shutdown(): self.mavlink_pub.publish(hb_ros_msg) try: # prevent garbage in console output when thread is killed. rate.sleep() except rospy.ROSInterruptException as e: rospy.logerr("Heartbeat thread error: {0}".format(e))
def send_heartbeat(): # Topic to which we have to send the heartbeat to. mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message( mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) rate = rospy.Rate(2) while not rospy.is_shutdown(): mavlink_pub.publish(hb_ros_msg) try: # prevent garbage in console output when thread is killed. rate.sleep() except rospy.ROSInterruptException as e: rospy.logerr("Heartbeat thread error: {0}".format(e))
def send_heartbeat(self): rate = rospy.Rate(2) # Hz while not rospy.is_shutdown(): try: if self.ids is not None: self.heartbeat_msg = mavutil.mavlink.MAVLink_heartbeat_message( mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) # rospy.loginfo('ID: {0}, {1}'.format(self.ids[0], self.ids[1])) # self.heartbeat_msg.pack(mavutil.mavlink.MAVLink('', self.ids[0], self.ids[1])) # fcu self.heartbeat_msg.pack(mavutil.mavlink.MAVLink('', 1, 1)) # simulator self.heartbeat_ros = mavlink.convert_to_rosmsg(self.heartbeat_msg) self.mavlink_pub.publish(self.heartbeat_ros) rate.sleep() except rospy.ROSInterruptException: pass
def heartbeat(self): hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message( mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) hb_ros_msg = mavlink.convert_to_rosmsg(hb_mav_msg) rate = rospy.Rate(2) while not rospy.is_shutdown(): self.mavlink_pub.publish(hb_ros_msg) rospy.loginfo("Tick tock heartbeat uav: {0}".format( self.topic_prefix)) try: time.sleep(0.5) except rospy.ROSInterruptException as e: rospy.logerr("Heartbeat thread error: {0}".format(e))
def setUp(self): super(self.__class__, self).setUp() self.mc_rad = 5 # meters self.fw_rad = 60 self.fw_alt_rad = 10 self.mission_name = "" 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 setUp(self): super(self.__class__, self).setUp() self.mission_item_reached = -1 # first mission item is 0 self.mission_name = "" self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) self.mission_item_reached_sub = rospy.Subscriber( 'mavros/mission/reached', WaypointReached, self.mission_item_reached_callback) # 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 mavlink_exec(cmd, timeout=3.0): global mavlink_recv mavlink_recv = '' recv_event.clear() if not cmd.endswith('\n'): cmd += '\n' msg = mavutil.mavlink.MAVLink_serial_control_message( device=mavutil.mavlink.SERIAL_CONTROL_DEV_SHELL, flags=mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE | mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI, timeout=3, baudrate=0, count=len(cmd), data=map(ord, cmd.ljust(70, '\0'))) msg.pack(link) ros_msg = mavlink.convert_to_rosmsg(msg) mavlink_pub.publish(ros_msg) recv_event.wait(timeout) return mavlink_recv
from os import listdir from os.path import isfile, join from threading import Event from functools import wraps from pymavlink import mavutil from mavros import mavlink, param, command from mavros_msgs.msg import State, OpticalFlowRad, Mavlink, ParamValue def mavlink_message_handler(msg): print link.decode(mavlink.convert_to_bytes(msg)) if __name__ == '__main__': mavros.set_namespace() recv_event = Event() link = mavutil.mavlink.MAVLink('', 255, 1) pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) mavlink_recv = '' rospy.init_node('mavlink_dump') sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_message_handler) r = rospy.Rate(0.5) while not rospy.is_shutdown(): msg = mavutil.mavlink.MAVLink_heartbeat_message(mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) ros_msg = mavlink.convert_to_rosmsg(msg) pub.publish(ros_msg) r.sleep()
def send_message(msg, mav, pub): #Sends MavLink message msg.pack(mav) rosmsg = convert_to_rosmsg(msg) pub.publish(rosmsg)