MAVROSPY
June 2024 - Present
Check out my tutorial HERE
Introduction
MAVROS is a MAVLink extendable communication node for ROS. In other words, it allows users to integrate their ROS based architectures with Ardupilot and/or PX4 autopliot firmwares which support MAVLink communication. By incorperating ROS and multicopter autopilots, users can easily dive into more advanced autonomous navigation systems like motion capture, lidar, obstacle avoidance, visual inertial odometry (VIO), and more.
This can be very useful as it acts as a abstraction layer between your ROS topics and your flight controller. You can simply send ROS topics through MAVROS and let it translate those topics to MAVLink messages which the flight controller can understand.
MAVROSPY serves as an additional layer to the MAVROS node. It allows users to easily manipulate the movement of their multicopter with simple Python methods while still integrating with the ROS framework. As such, it makes it even easier to tackle on your advanced ROS projects.
Video
Description
MAVROSPY provides support for GPS Navigation, Motion Capture, and Simulation.
GPS Navigation
Navigating via GPS is quite simple as it is the traditional way UAVs navigate autonomously.
We can manipulate the movement of the drone via GPS via the SET_POSITION_TARGET_GLOBAL_INT or SET_POSITION_TARGET_LOCAL_NED MAVLink message. MAVROSPY utilizes the latter as it provides more flexibily when navigating relative to its local frame rather then the global (GPS) frame.
The SET_POSITION_TARGET_LOCAL_NED message takes in a x, y, and z position, velocity, and acceleration in the NED (North, East, Down) frame as well as yaw and yaw rate. This message doesn't correlate well with ROS because it uses the ENU (East, North, Up) frame. Additionally, ROS uses the geometry_msgs/Pose message which uses an x, y, z geometry_msgs/Point message and an x, y, z, w geometry_msgs/Quaternion message. This is where MAVROS comes to play. It transforms the MAVLink/FCU NED frame into the ROS ENU frame and converts the x, y, z position and yaw into a geometry_msgs/Pose message via the /mavros/setpoint_position/local topic.
As such, MAVROSPY can simply publish messages to the /mavros/setpoint_position/local topic to maneuver the UAV:
def goto(self, pose):
"""
Set the given pose as a next set point by sending a SET_POSITION_TARGET_LOCAL_NED message.
The copter must be in OFFBOARD mode for this to work.
"""
# initialize ROS PoseStamped message
pose_stamped = PoseStamped()
pose_stamped.header.stamp = self.timestamp
pose_stamped.pose = pose
try:
self.cmd_pos_pub.publish(pose_stamped) # publish PoseStamed message
except rospy.ROSException as e:
self.log_error(e)
We can generate the pose argument via the goto_xyz_rpy method:
def goto_xyz_rpy(self, x, y, z, roll, pitch, yaw, timeout=30, isClose=True, checkMode=True):
"""
Sets the given pose as a next set point for given timeout (seconds).
isClose: check if UAV is close to target set point
checkMode: check if UAV is in OFFBOARD mode
"""
# Check if in OFFBOARD mode
if checkMode:
self.check_offboard()
# initialize Pose message
pose = Pose()
pose.position.x, pose.position.y, pose.position.z = x, y, z
# convert euler angles (roll, pitch, yaw) to quaternion (x, y, z, w)
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw + self.pi_2)
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = quaternion[0], quaternion[1], quaternion[2], quaternion[3]
for i in range(int(timeout * self.freq)): # loop for given timeout
self.goto(pose) # move to set point
# check if isClose
if (isClose and
self.is_close(x, self.pose.position.x, 0.2) and
self.is_close(y, self.pose.position.y, 0.2) and
self.is_close(z, self.pose.position.z, 0.5) and
self.is_close(quaternion[0], self.pose.orientation.x, 0.1, True) and
self.is_close(quaternion[1], self.pose.orientation.y, 0.1, True) and
self.is_close(quaternion[2], self.pose.orientation.z, 0.1, True) and
self.is_close(quaternion[3], self.pose.orientation.w, 0.1, True)):
self.log_info("Reached target position")
break
self.pause()
Where is_close is a method that compares whether the current and target values are within a set tolerance.
The demonstration video for GPS Navigation was displayed in the Video section.
Motion Capture
Now that we can integrate our UAV into the ROS framework using MAVROS and maneuver the UAV using MAVROSPY, we can begin integrating other ROS features into the design. Using a motion capture system is crucial for rapid prototyping as you can test movement scripts in a controlled lab environment, without GPS, and without having to go outside.
In order to do this, we must be able to send information to the UAV regarding its position as GPS is very unreliable indoors. We can do this via the VISION_POSITION_ESTIMATE MAVLink message which requires a local x, y, and z position and a roll, pitch, and yaw angle. Again, MAVROS integrates this into ROS via the same geometry_msgs/Pose message. As such, all MAVROSPY has to do is relay the motion caputure topic into the /mavros/vision_pose/pose topic to inform the UAV of its position.
If we want to utilize any PX4 Auto Modes (Mission, Return, Land, Hold, & Orbit) with a local position estimate, we would have to mimic a GPS signal. To do this, we can utilize the MAVROS fake_gps plugin which requires a geometry_msgs/Transform ROS message. This message "represents the transform between two coordinate frames in free space". In other words, we need to convert the motion capture geometry_msgs/Pose message into a quaternion (orientation) and a 3-D vector from the local origin to the UAV's current location:
# Create a TransformStamped message
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "map" # or "world" depending on your setup
t.child_frame_id = "fix" # frame of your vehicle
# Copy the position
t.transform.translation.x = pose_msg.pose.position.x
t.transform.translation.y = pose_msg.pose.position.y
t.transform.translation.z = pose_msg.pose.position.z
# Copy the orientation
t.transform.rotation.x = pose_msg.pose.orientation.x
t.transform.rotation.y = pose_msg.pose.orientation.y
t.transform.rotation.z = pose_msg.pose.orientation.z
t.transform.rotation.w = pose_msg.pose.orientation.w
Now we can integrate the goto_xyz_rpy method as well as any PX4 Auto modes utilzing a motion capture local position estimate:
Simulation
Being able to simulate in any robotics project and especially for UAVs is very important if you want to avoid damaging hardware. With ROS fully integrated into our system, we can easily use the Gazebo Simulator to simulate our UAV.
Luckily, PX4 makes this very easy as its source code is all open-source and can be compiled on almost any system to simulate its flight controller software. We can then use MAVROS to integrate the simualted flight controller into ROS and MAVROSPY to maneuver our simulated drone: