Gazebo仿真记录 Turtlebot3 + D435i

在Gazebo环境中在Turtlebot3上添加深度相机D435和IMU。

步骤

1.准备工作

创建ROS工作空间，下载turtlebot3相关代码和realsense2_description 模型文件放到工作空间下。
Turtlebot3：主要是turtlebot3、turtlebot3_simulations、turtlebot3_msgs
realsense2_description：存放Realsense相关型号的相机模型文件及xacro文件。这里的相机型号包括d415、d345、r410、r430和t265。可以用view_d415_model.launch等在Gazebo中打开查看。

2.添加相机D435

以在turtlebot-burger上添加D435为例，首先需要包含_d435.urdf.xacro文件。

<xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />

然后，将上述文件中构造的d435传感器模型添加到turtlebot-burger上，

  <xacro:sensor_d435 name="camera" topics_ns="camera" parent="base_link" publish_pointcloud="true">
    <origin xyz="0 0 0.1" rpy="0 0 0" />
  </xacro:sensor_d435>

3.添加IMU

turtlebot_burger文件里面已经添加了imu的相关设置。

  <gazebo>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <alwaysOn>true</alwaysOn>
      <bodyName>imu_link</bodyName>
      <frameName>imu_link</frameName>
      <topicName>imu</topicName>
      <serviceName>imu_service</serviceName>
      <gaussianNoise>0.0</gaussianNoise>
      <updateRate>0</updateRate>
      <imu>
        <noise>
          <type>gaussian</type>
          <rate>
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </plugin>
  </gazebo>

也可以参考[1]的方法添加IMU

  <gazebo reference="camera_link">
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <update_rate>200</update_rate>
      <visualize>true</visualize>
      <topic>__default_topic__</topic>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
        <topicName>d435i/imu</topicName>
        <bodyName>camera_link</bodyName>
        <updateRateHZ>200.0</updateRateHZ>
        <gaussianNoise>0.0</gaussianNoise>
        <xyzOffset>0.0 0.0 0.0</xyzOffset>
        <rpyOffset>0.0 0.0 0.0</rpyOffset>
        <frameName>imu_link_</frameName>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

在Gazebo仿真时发现没有IMU话题发布，后来发现可能是libgazebo_ros_imu_sensor.so文件缺失，但是并没有报错。重新安装Gazebo之后问题解决。
sudo apt-get install ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control

仿真结果

获取相机内参

执行 rostopic echo /camera/color/camera_info，得到

 header: 
  seq: 1114
  stamp: 
    secs: 37
    nsecs: 526000000
  frame_id: "camera_color_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: []
K: [462.1379699707031, 0.0, 320.0, 0.0, 462.1379699707031, 240.0, 0.0, 0.0, 1.0]
R: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
P: [462.1379699707031, 0.0, 320.0, 0.0, 0.0, 462.1379699707031, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

对上述参数理解参考[4]

# Projection/camera matrix
#     [fx'  0  cx' Tx]
# P = [ 0  fy' cy' Ty]
#     [ 0   0   1   0]

#     [fx  0 cx]
# K = [ 0 fy cy]
#     [ 0  0  1]

其他

最早的计划是直接添加D435i模型，但是折腾了很久没有实现。同时参考[1]，对于实体realsense-d435i,会有相关realsense软件包实现时间同步等工作，但是在Gazebo环境中与普通的camera+imu组合并没有区别，最终选择了分别添加D435和IMU.

Github项目Turtlebot3_with_Realsense_D435i里面给出了D435i模型，但并没有找到Tutrlebot3_with_Realsense_D435i/turtlebot3/turtlebot3_description/urdf_d435i.gazebo.xacro文件中所需要的librealsense_gazebo_accel_sensor.so和librealsense_gazebo_gyro_sensor.so文件。

本文的urdf文件修改主要参考了leochien1110/turtlebot3_d435,如果有更好的方法或建议可以私信或者评论区，在此感谢！

参考链接[5]给出了在无人机上添加realsense D435i的实现。

修正

上述的leochien1110/turtlebot3_d435项目中的，在添加burger模型时，对base_footprint 和base_link 之间的变换做了一些修改：
原来的代码为：

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.010" rpy="0 0 0"/>
  </joint>

而在上述项目中被修改为：

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.010" rpy="0 0 3.14"/>
  </joint>

即rpy的值做了修改，会导致算法仿真时出现一些错误，需要注意。

问题解决

D435i仿真无点云输出

使用rostopic list命令，可以查询到深度点云话题/camera/depth/color/points，但是当通过rostopic echo /camera/depth/color/points获取深度点云数据时，会出现以下报错：

对比了leochien1110/turtlebot3_d435项目的文件，发现在如下的文件中重复包含了realsense2_description/urdf/_d435.urdf.xacro文件(第6行)

在自己对应的文件中同样添加 <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />，问题解决，原因不详。

参考资料：
[1] Gazebo环境下VIO SLAM系统实现
[2] Gazebo使用笔记（10)–gazebo插件与传感器的添加
[3] Gazebo官方教程Tutorial: Using Gazebo plugins with ROS
[4] Gazebo仿真Kinect相机内参获取，以及fx fy cx cy 的理解
[5] realsense D435i gazebo slam(px4)仿真