Gazebo simulation problem

Hello,

I am running a simulation in Gazebo with RGBD camera, and I get the following issue. Pose estimation (published in orb_slam2_rgbd/pose topic) and also the pointcloud do not correctly get the TF from camera to base link, and therefore they are wrongly aligned. The offset you see in this RViZ image is exactly the 0.2 meters that camera is elevated from base. What am I missing here? I attach camera urdf description and my launch file. I tried different settings and camera_link & camera_link_optical do not cause this issue.

gith

Camera simulation:

<joint name="camera_joint" type="fixed">
  <parent link="base_link" />
  <child link="camera_link" />
  <origin xyz="0 0 0.2" rpy="0 0 0" />
</joint>
<link name="camera_link">
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <box size="0.05 0.05 0.05"/>
    </geometry>
  </collision>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <box size="0.05 0.05 0.05"/>
    </geometry>
  </visual>
  <inertial>
    <mass value="0.0001" />
    <origin xyz="0 0 1" rpy="0 0 0"/>
    <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
      iyy="0.0001" iyz="0.0"
      izz="0.0001" />
  </inertial>
</link>
<joint name="camera_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
  <parent link="camera_link"/>
  <child link="camera_link_optical"/>
</joint>
<link name="camera_link_optical">
</link>
<sensor name="camera" type="depth">
  <update_rate>30</update_rate>
  <camera>
    <horizontal_fov>1.57</horizontal_fov>
    <image>
      <width>640</width>
      <height>480</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.05</near>
      <far>100</far>
    </clip>
  </camera>
  <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>1.0</updateRate>
    <cameraName>camera</cameraName>
    <imageTopicName>rgb/image_raw</imageTopicName>
    <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
    <depthImageTopicName>depth_registered/image_raw</depthImageTopicName>
    <depthImageInfoTopicName>depth_registered/camera_info</depthImageInfoTopicName>
    <pointCloudTopicName>depth_registered/points</pointCloudTopicName>
    <frameName>camera_link_optical</frameName>
    <pointCloudCutoff>0.5</pointCloudCutoff>
    <pointCloudCutoffMax>10.0</pointCloudCutoffMax>
    <baseline>0.0</baseline>
    <focalLength>320</focalLength>
    <distortionK1>0.0</distortionK1>
    <distortionK2>0.0</distortionK2>
    <distortionK3>0.0</distortionK3>
    <distortionT1>0.0</distortionT1>
    <distortionT2>0.0</distortionT2>
    <CxPrime>320</CxPrime>
    <Cx>320.0</Cx>
    <Cy>240.0</Cy>
    <hackBaseline>0.0</hackBaseline>
  </plugin>
</sensor>

ORBSLAM launch file:

<launch>
  <node name="orb_slam2_rgbd" pkg="orb_slam2_ros" type="orb_slam2_ros_rgbd" output="screen">
    <param name="publish_pointcloud" type="bool" value="true" />
    <param name="publish_pose" type="bool" value="true" />
    <param name="localize_only" type="bool" value="false" />
    <param name="reset_map" type="bool" value="false" />

    <param name="load_map" type="bool" value="false" />
    <param name="map_file" type="string" value="map.bin" />
    <param name="voc_file" type="string" value="$(find orb_slam2_ros)/orb_slam2/Vocabulary/ORBvoc.txt" />
    <param name="pointcloud_frame_id" type="string" value="map" />
    <param name="camera_frame_id" type="string" value="camera_link" />
    <param name="min_num_kf_in_map" type="int" value="5" />

    <param name="/ORBextractor/nFeatures" type="int" value="1000" />
    <param name="/ORBextractor/scaleFactor" type="double" value="1.2" />
    <param name="/ORBextractor/nLevels" type="int" value="8" />
    <param name="/ORBextractor/iniThFAST" type="int" value="20" />
    <param name="/ORBextractor/minThFAST" type="int" value="7" />

    <param name="camera_fps" type="int" value="30" />
    <param name="camera_rgb_encoding" type="bool" value="true" />
    <param name="ThDepth" type="double" value="40.0" />
    <param name="depth_map_factor" type="double" value="1.0" />
    <param name="load_calibration_from_cam" type="bool" value="true" />
  </node>
</launch>

appliedAI-Initiative / orb_slam_2_ros

Gazebo simulation problem #112