包含realsense T265 D435i的urdf和sdf文件、realsense_gazebo_plugin包及realsense 模型文件使用示例。
[catkin_ws]表示自定义的工作目录
mkdir -p [catkin_ws]/src
cd [catkin_ws]/src
git clone https://gitee.com/nie_xun/realsense_ros_gazebo.git
cd [catkin_ws]
catkin_make
source devel/setup.sh
roslaunch realsense_ros_gazebo simulation_sdf.launch
运行结果：
相比D435多一个camera/imu topic
roslaunch realsense_ros_gazebo simulation_D435i_sdf.launch
本示例使用px4的iris 无人机模型作为示例，本文使用的Firmware为v1.8版本。
在realsense_ros_gazebo的sdf文件夹下已存放携带D435i的iris sdf文件。
以下[px4]为px4 对应的Firmware的根路径
<?xml version="1.0"?>
<launch>
<arg name="x" default="0"/>
<arg name="y" default="0"/>
<arg name="z" default="0"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0"/>
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="vehicle" default="iris"/>
<arg name="world" default="$(find px4)/Tools/sitl_gazebo/worlds/empty.world"/>
<arg name="sdf" default="$(find px4)/Tools/sitl_gazebo/models/iris_realsense_camera/iris_realsense_camera.sdf"/>
<arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_vo"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<arg name="respawn_gazebo" default="false"/>
<!-- MAVROS configs -->
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
<arg name="respawn_mavros" default="false"/>
<!-- PX4 configs -->
<arg name="interactive" default="true"/>
<!-- PX4 SITL and Gazebo -->
<include file="$(find px4)/launch/posix_sitl.launch">
<arg name="x" value="$(arg x)"/>
<arg name="y" value="$(arg y)"/>
<arg name="z" value="$(arg z)"/>
<arg name="R" value="$(arg R)"/>
<arg name="P" value="$(arg P)"/>
<arg name="Y" value="$(arg Y)"/>
<arg name="world" value="$(arg world)"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="sdf" value="$(arg sdf)"/>
<arg name="rcS" value="$(arg rcS)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="interactive" value="$(arg interactive)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="gcs_url" value=""/>
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
</include>
</launch>
cd [px4]//posix-configs/SITL/init/ekf2
cp iris iris_vo
将EKF2_AID_MASK和EKF2_HGT_MODE对应项改成：
param set EKF2_AID_MASK 24
param set EKF2_HGT_MODE 3
cp realsense_ros_gazebo/sdf/* [px4]/Tools/sitl_gazebo/models/ -r
roscd px4/launch
roslaunch iris_realsense_camera_px4_mavros_vo.launch
<remap from="/camera/odom" to="/mavros/vision_pose/pose" />
2）本文使用的slam坐标系为WUN，发布的odom消息类型为/camera/odometry
使用的转换代码文件如下：
nav_msg_to_mavros.cpp
#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
ros::Publisher camera_pose_publisher;
int data_source;
enum {
GAZEBO_GT = 0,
CAMERA_VO = 1
};
void vision_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
geometry_msgs::PoseStamped msg_body_pose;
if (data_source == GAZEBO_GT) {
// 0 means use gazebo grougtruth
msg_body_pose.header.stamp = msg->header.stamp;
msg_body_pose.header.frame_id = "world";
msg_body_pose.pose.position.x = msg->pose.pose.position.x;
msg_body_pose.pose.position.y = msg->pose.pose.position.y;
msg_body_pose.pose.position.z = msg->pose.pose.position.z;
msg_body_pose.pose.orientation.x = msg->pose.pose.orientation.x;
msg_body_pose.pose.orientation.y = msg->pose.pose.orientation.y;
msg_body_pose.pose.orientation.z = msg->pose.pose.orientation.z;
msg_body_pose.pose.orientation.w = msg->pose.pose.orientation.w;
} else if (data_source == CAMERA_VO) {
// 1 means use camera vo
// Create PoseStamped message to be sent
msg_body_pose.header.stamp = msg->header.stamp;
msg_body_pose.header.frame_id = "world";
msg_body_pose.pose.position.x = msg->pose.pose.position.z;
msg_body_pose.pose.position.y = msg->pose.pose.position.x;
msg_body_pose.pose.position.z = msg->pose.pose.position.y;
msg_body_pose.pose.orientation.x = msg->pose.pose.orientation.z;
msg_body_pose.pose.orientation.y = msg->pose.pose.orientation.x;
msg_body_pose.pose.orientation.z = msg->pose.pose.orientation.y;
msg_body_pose.pose.orientation.w = msg->pose.pose.orientation.w;
}
// Publish pose of body frame in world frame
camera_pose_publisher.publish(msg_body_pose);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "vision_to_mavros");
ros::NodeHandle nh("~");
nh.param<int>("data_source", data_source, 0);
ros::Subscriber gazebo_sub = nh.subscribe<nav_msgs::Odometry>("/camera/odometry", 50, vision_cb);
camera_pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 50);
ros::spin();
}
2.启动想使用的slam
结果示例：
mavros：enu
px4：ned
在外部传感器定位，非GPS自身定位，世界坐标系由外部传感器定义。
如optirack，使用自身的定义的原点坐标系，由定义原点时，使用的定位杆长短轴及motive软件的Y/Z up综合定义。
Y up: 前(Z+), 左(X+), 上(Y+) 。
Z up: 在Y up的坐标系基础上绕x轴旋转-90度。Z up即使用的enu的坐标系。
坐标系的定位也有相对之分，比如optirack相对于GPS坐标系就是相对坐标系，GPS作为地球坐标系(更大的坐标系)则作为绝对坐标系，当使用相机定位做对比实验，则又能将optirack看做绝对坐标系，相机坐标系作为相对坐标系。
f:first l:left b:back u:up
因此像enu end这种坐标系更多的是说明xyz三轴的相对关系，比如(flu, lbu)都是一种enu坐标系，更多的是说明机体坐标系来用的。如下图所示：通过旋转后的坐标系，满足三轴规则，还是enu坐标系。
当optirack vo等相对局部的坐标系只要满足xyz的右手关系，即，食指指向x，对应中指就指向y, 大拇指就是z方向。则就满足enu坐标系。
当无人机以固件启动时作为原点，偏航初始化为0，当无人机以外部传感器作为定位及yaw的输入时(roll pitch由无人机自身imu获得)，需要对准机头方向，使外部传感器得到的偏航和机体得到的偏航一致。
例子1：外部定位使用optirack，Zup(enu), 将初始化时将机头于optirack的x轴对齐，则直接将/vrpn_client_node/<rigid_body_name>/poseremap到mavros/vision_pose/pose即可。
使用mavros/vision_pose发送，mavros采用的enu坐标系，mavros会自动两enu转换到ned坐标系，则初始化时将机头于optirack的x轴对齐，从而能够使px4初始化时，px4解算偏航与optirack解算的机体偏航一致，约为0。或者不对齐时，需减去optirack的偏航测量值再赋值给mavros/vision_pose
例子2：外部定位使用vo，相机前向与无人机绑定，坐标系为(LUF)，将vo输出转换到ENU坐标系赋值给mavros/vision_pose/pose，则对应的转换：类似将(LUF->FLU)
poseX = voZ
poseY = voX
poseZ = voY
此时，直接将机头与mavros的enu的初始化x轴对齐了，即将相机的前向(机头方向)作为mavros的x轴。
总结：无论是vo还是optitrack，要保证的是初始化时，机头要对准赋值给mavros/vision_pose的x。mavros/vision_pose在ENU坐标系下。
附vins 的配置文件
vins-fusion，仅作参考，实际应根据realsense_ros_gazebo/sdf/D435i/model.sdf计算得。
本人只用了单目、imu及深度图，因此没有去解算双目的变换矩阵。以下仅供参考：建议将estimate_extrinsic设为1。
realsense_stereo_imu_config.yaml
imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_raw"
image1_topic: "/camera/infra2/image_raw"
output_path: "/home/xxx/output"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -5.7586305857286746e-03, -4.0463318787729019e-03,
9.9997523237933461e-01, 2.0329267950355900e-02,
-9.9998287214160420e-01, -1.0224590553211677e-03,
-5.7628118925283633e-03, 7.9325209639615653e-03,
1.0457519809151661e-03, -9.9999129084997906e-01,
-4.0403746097850135e-03, 2.8559824645148020e-03, 0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -1.0021770212322867e-03, 3.6313480322730518e-04,
9.9999943188700535e-01, 1.5285779565991807e-02,
-9.9999216342926500e-01, -3.8303422615924010e-03,
-1.0007788055728661e-03, -5.2435791444330505e-02,
3.8299766679101843e-03, -9.9999259827824449e-01,
3.6697063849344680e-04, 8.6931302450199057e-03, 0., 0., 0., 1. ]
left.yaml/right.yaml
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
projection_parameters:
fx: 383.6
fy: 383.3
cx: 317.0
cy: 240.0