thien94 / vision_to_mavros

A collection of ROS and non-ROS (Python) code that converts data from vision-based system (external localization system like fiducial tags, VIO, SLAM, or depth image) to corresponding mavros topics or MAVLink messages that can be consumed by a flight control stack (with working and tested examples for ArduPilot).
https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html
GNU General Public License v3.0
246 stars 146 forks source link

A collection of ROS and non-ROS (Python, cpp) code that converts data from vision-based system (external localization system like fiducial tags, VIO, SLAM, or depth image) to corresponding MAVROS topics or MAVLink messages that can be consumed by a flight control stack to support precise localization and navigation tasks.

The code has been tested and come with instructions to be used with ArduPilot. The main sensor is the Intel Realsense Tracking camera T265.


Installation and setup with ArduPilot:

ROS:

Follow this wiki page: https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html or this blog post

non-ROS:

Follow this wiki page: https://ardupilot.org/copter/docs/common-vio-tracking-camera.html


What's included (the main stuffs):

ROS nodes:

non-ROS scripts:


ROS nodes

vision_to_mavros_node

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](https://dev.px4.io/en/ros/external_position_estimation.html#ros_reference_frames) with user input roll, pitch, yaw, gamma angles. ### 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. ### 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](http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html) - 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](http://docs.ros.org/api/nav_msgs/html/msg/Path.html) - visualize trajectory of body frame in rviz. ### Example applications ### Autonomous flight with [Intel® RealSense™ Tracking Camera T265](https://www.intelrealsense.com/tracking-camera-t265/) and [ArduPilot](http://ardupilot.org/):

* A complete guide including installation, configuration and flight tests can be found by the following [blog posts](https://discuss.ardupilot.org/t/gsoc-2019-integration-of-ardupilot-and-vio-tracking-camera-for-gps-less-localization-and-navigation/42394). 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`](http://docs.ros.org/api/nav_msgs/html/msg/Path.html), 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](https://github.com/AprilRobotics/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. ## `t265_fisheye_undistort_node` Image stream from one of the T265’s cameras will be processed to detect [AprilTag](https://april.eecs.umich.edu/software/apriltag.html) visual marker, then we will follow MAVLink’s [Landing Target Protocol](https://mavlink.io/en/services/landing_target.html) that is supported by ArduPilot to perform precision landing. -------------------------------------------------------------------------- # non-ROS scripts ## `t265_test_streams` Testing the installation of `librealsense` and USB connection with the Realsense T265. Extracted from [here](https://github.com/IntelRealSense/librealsense/tree/master/examples). ## `t265_to_mavlink` The main Python script that integrates the T265 with ArduPilot. The usage is documented in the following blog posts and wiki pages: - Wiki for [non-ROS](https://ardupilot.org/copter/docs/common-vio-tracking-camera.html). - [Overall introduction](https://discuss.ardupilot.org/t/gsoc-2019-integration-of-ardupilot-and-vio-tracking-camera-for-gps-less-localization-and-navigation/42394). - [Installation](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-1-getting-started-with-the-intel-realsense-t265-on-rasberry-pi-3b/43162/1) and [setup](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-4-non-ros-bridge-to-mavlink-in-python/44001). - [Flight test](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-3-indoor-autonomous-flights-and-performance-tests/43626) and [advanced usage](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-5-camera-position-offsets-compensation-scale-calibration-and-compass-north-alignment-beta/44984). ## `t265_precland_apriltags` Same as [`t265_fisheye_undistort_node`](#t265_fisheye_undistort_node), but in Python instead of ROS.