Our current project relies on using ROS to interact with a drone and control station (GCS). The drone and GCS both communicate via MAVLink, so the obvious answer is to use Mavros. Mavros connects to the drone and control system and forwards messages between the two, while making a lot of status information available on ROS topics, and allowing commands to be sent to ROS topics to control the drone.
This seemed like a good idea at first, but there was a problem for our project, we actually wanted to intercept the messages and add some logic to decide if they should be forwarded. Mavros - running as a node - unfortunately, provides no way to intercept the messages. We tried modifying the Mavros source to our needs, and ended up breaking its ability to receive messages. A different approach was needed.
PyMavLink allows python code to make direct connections to a mavlink enabled device. Running it as a Ros node was easy enough, and by running one node each for the drone and GCS we could accept mavlink messages, convert them to ROS messages, and send them to the ROS system. The documentation is a little sparse on how to do this though, so after a day of experimenting we got to this:
from pymavlink import mavutil
from mavros import mavlink# ip of drone
conn = mavutil.mavlink_connection("tcp:192.168.0.2:5760")
while True:
msg = conn.recv_msg()
rosmsg = mavlink.convert_to_rosmsg(msg)
print(rosmsg)
We’re still using mavros for its utility in translating messages between mavlink and ros, but no longer relying on its built in node
Turning a ROS message back into a mavlink message, however, is not well documented. After a full day working on it, we discovered it’s relatively easy:
from pymavlink import mavutil
from mavros import mavlink# new import!
from pymavlink.dialects.v20.common import MAVLink # ip of drone
conn = mavutil.mavlink_connection("tcp:192.168.0.2:5760") while True:
msg = conn.recv_msg()
rosmsg = mavlink.convert_to_rosmsg(msg)
print(rosmsg) # Now we turn it back
bytes = mavlink.convert_to_bytes(rosmsg)
original_message = conn.mav.decode(bytes)
print(original_message)
The multiple uses of the word MAVLink, with varying case, is a bit awkward, and there’s nothing in the documentation about doing this, it came from a lot of reading of the source and experimenting. However, we are now able to independently read and write to the drone and GCS, and process their messages without automatically forwarding them.
Further dwon the line, we ran into a problem sending the messages across a ROS topic, somehow the crc was not calculating correctly. The message needed repacking with the local mav instance:
from pymavlink import mavutil
from mavros import mavlink
from pymavlink.dialects.v20.common import MAVLink# ip of drone
conn = mavutil.mavlink_connection("tcp:192.168.0.2:5760")while True:
msg = conn.recv_msg()
msg.pack(conn.mav) # <----- new line to fix crc issue
rosmsg = mavlink.convert_to_rosmsg(msg)
print(rosmsg)# Now we turn it back
bytes = mavlink.convert_to_bytes(rosmsg)
original_message = conn.mav.decode(bytes)
print(original_message)
Hopefully this will save someone the hours we’ve spent on it.
