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.
To install this package with ROS Kinetic:
- No dependencies required.
- Setup
catkin
workspace (if not already done so)
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
- 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
- Suppose we have a frame named
source_frame_id
that is measured in a frame namedtarget_frame_id
. Lettarget_frame_id
be theworld {W}
frame, we want to transformsource_frame_id
tobody {B}
frame so that{B}
and{W}
conform toENU
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 inENU
. We will rotate{B}
in{W}
by an anglegamma_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 byroll_cam
,pitch_cam
,yaw_cam
, in that order.
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 fromsource_frame_id
to body frame, according to ENU conventions.
/tf
containing pose/odometry data.
/vision_pose
of type geometry_msgs/PoseStamped - single pose to be sent to the FCU autopilot (ArduPilot / PX4), published at a frequency defined byoutput_rate
./body_frame/path
of type nav_msgs/Path - visualize trajectory of body frame in rviz.
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
(withfcu_url
and other parameters inapm.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 byrealsense-ros
.apm.launch
modified with your own configuration.t265_tf_to_mavros.launch
as is.
After running roslaunch vision_to_mavros t265_all_nodes.launch
, here's how to view the trajectory of t265 on rviz:
- On host computer, open up rviz:
rosrun rviz rviz
. - Add
Path
, topic name:/body_frame/path
to rviz. - Change
Fixed Frame
totarget_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.