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 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 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.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
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()
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)