toothsmile / vision_to_mavros

ROS package that converts pose data from external localization system (fiducial tags, VIO, SLAM etc.) to mavros topics, taking into account camera orientation in the body frame.

vision_to_mavros

ROS package that listens to /tf, transforms the pose of source_frame_id to target_frame_id, then rotate the frame to match body_frame according to ENU convention with user input roll, pitch, yaw, gamma angles.

Installation (tested with Ubuntu 16.04 LTS):

To install this package with ROS Kinetic:

1. No dependencies required.
2. Setup catkin workspace (if not already done so)

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init

3. Clone the repository and build with catkin_make:

cd ~/catkin_ws/src
git clone https://github.com/hoangthien94/vision_to_mavros.git
cd ../
catkin_make

How it works

• Suppose we have a frame named source_frame_id that is measured in a frame named target_frame_id. Let target_frame_id be the world {W} frame, we want to transform source_frame_id to body {B} frame so that {B} and {W} conform to ENU convention (x is pointing to East direction, y is pointing to the North and z is pointing up).

• Now assume we already have a default {B} and {W} that are correct in ENU. We will rotate {B} in {W} by an angle gamma_world, in right hand rule. For example, gamma_world equals -1.5707963 (-PI/2) will make {B}'s x axis aligns with {W}'s y axis.

• source_frame_id will be aligned with that default {B} by rotating around its own x, y, z axis by angles defined by roll_cam, pitch_cam, yaw_cam, in that order.

Node: vision_to_mavros

Parameters:

• target_frame_id: id of target frame (world/map/base_link)
• source_frame_id: id of source frame (camera/imu/body_link)
• output_rate: the output rate at which the pose data will be published.
• roll_cam, pitch_cam, yaw_cam, gamma_world: angles (in radians) that will convert pose received from source_frame_id to body frame, according to ENU conventions.

Subscribed topic:

• /tf containing pose/odometry data.

Published topic:

• /vision_pose of type geometry_msgs/PoseStamped - single pose to be sent to the FCU autopilot (ArduPilot / PX4), published at a frequency defined by output_rate.
• /body_frame/path of type nav_msgs/Path - visualize trajectory of body frame in rviz.

Example applications

Autonomous flight with Intel® RealSense™ Tracking Camera T265 and ArduPilot:

• A complete guide including installation, configuration and flight tests can be found by the following blog posts.

There are 3 nodes running in this setup. In 3 separated terminals on RPi:

• T265 node: roslaunch realsense2_camera rs_t265.launch. The topic /camera/odom/sample/ and /tf should be published.

• MAVROS node: roslaunch mavros apm.launch (with fcu_url and other parameters in apm.launch modified accordingly).

rostopic echo /mavros/state should show that FCU is connected.

rostopic echo /mavros/vision_pose/pose is not published

• vision_to_mavros node: roslaunch vision_to_mavros t265_tf_to_mavros.launch

rostopic echo /mavros/vision_pose/pose should now show pose data from the T265.

rostopic hz /mavros/vision_pose/pose should show that the topic is being published at 30Hz.

Once you have verified each node can run successfully, next time you can launch all 3 nodes at once with: roslaunch vision_to_mavros t265_all_nodes.launch, with:

• rs_t265.launch as originally provided by realsense-ros.
• apm.launch modified with your own configuration.
• t265_tf_to_mavros.launch as is.

View trajectory on rviz

After running roslaunch vision_to_mavros t265_all_nodes.launch, here's how to view the trajectory of t265 on rviz:

1. On host computer, open up rviz: rosrun rviz rviz.
2. Add Path, topic name: /body_frame/path to rviz.
3. Change Fixed Frame to target_frame_id, in the case of Realsense T265: camera_odom_frame.

Usage with AprilTag:

roslaunch vision_to_mavros apriltags_to_mavros.launch

This will launch usb_cam to capture raw images, perform rectification through image_proc, use apriltag_ros to obtain the pose of the tag in the camera frame, and finally vision_to_mavros to first get the pose of camera in the tag frame, transform to body frame by using camera orientation, and publish the body pose to /mavros/vision_pose/pose topic. Note that mavros should be launch separately since it has a lot of output on the terminal.