ergoCubSim wrong urdf parameters on gz-sim

[SimoneMic]

I have ported the ergoCub navigation simulation from gazebo classic to gazebo sim and I have faced the following issues:

• 1. The lidar parameters in the urdf are wrong:
  • max_distance must be 25.0 meters instead of 5
  • resolution must be 1 instead of 0.5. Otherwise only half lidar will be displayed.
• 2. wrappers/motorControl/all-mc_remapper.xml should be enabled by the flag enable_ros2, otherwise the ros2 simulation will not work.
• 3. Camera parameters are off, as you can clearly see from these images:

The red dots are the lidar points on the ground, as you can see they are aligned with the robot feet. However the pointcloud goes underneath the ground.

For reference of the scene:

Here the camera depth seen by ros2:

[Nicogene]

Hello @SimoneMic, this was not happening in gazebo classic?
If both lidar and camera parameters where fine we can do a side-by-side comparison

[SimoneMic]

Hi @Nicogene,
The lidar parameters were still different in classic:

<scan>
    <horizontal>
      <samples>360</samples>
      <resolution>2</resolution>
      <min_angle>0</min_angle>
      <max_angle>6.28</max_angle>
    </horizontal>
  </scan>
  <range>
    <min>0.04</min>
    <max>5</max>
    <resolution>0.01</resolution>
  </range>

The samples were 360 with a resolution of 2 (instead of 720 with 0.5 resolution). The range was still 5 and I was using as 5. But now I am using wider environments and I need the real sensor characteristics. Also the minimum range was different at 0.04.

For what regards the camera I don't remember to have used it in the past simulations. But the camera parameters are the same. I will try to test it on an old simulation to see if it was working.

[Nicogene]

Have you tried to put

<scan>
    <horizontal>
      <samples>360</samples>
      <resolution>2</resolution>
      <min_angle>0</min_angle>
      <max_angle>6.28</max_angle>
    </horizontal>
  </scan>

In the urdf and test in gz-sim? I recall that I had some issues:

• Models are not working on gz-sim under wsl due to gpu_lidar #337

And while fixing them probably I changed them respect the past

[SimoneMic]

After some testing I have found out that the intrinsic parameters are not being loaded by the yarp plugin.
If the echo of the ros2 topic /camera_rgbd/depth/camera_info is:

header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: depth_frame
height: 480
width: 640
distortion_model: plumb_bob
d:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
k:
- 277.0
- 0.0
- 319.5
- 0.0
- 277.0
- 239.5
- 0.0
- 0.0
- 1.0
r:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
p:
- 277.0
- 0.0
- 319.5
- 0.0
- 0.0
- 277.0
- 239.5
- 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

Which provides some wrong intrinsic parameters, which explain the erroneous pointcloud.

The config that I use is this one:

<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<robot name="ergoCubSim" xmlns:xi="http://www.w3.org/2001/XInclude">
   <devices>

       <device xmlns:xi="http://www.w3.org/2001/XInclude"  name="depth_nwc_yarp" type="RGBDSensorClient">

           <param name="remoteImagePort">/ergocubSim/depthCamera/rgbImage:o</param>
           <param name="remoteDepthPort">/ergocubSim/depthCamera/depthImage:o</param>
           <param name="remoteRpcPort">/ergocubSim/depthCamera/rpc:i</param>
           <param name="localImagePort">/camera/color/image_rgb:i</param>
           <param name="localDepthPort">/camera/depth/image_depth:i</param>
           <param name="localRpcPort">/camera/depth/rpc</param>
           <param name="ImageCarrier">mjpeg</param>
           <param name="DepthCarrier">fast_tcp+send.portmonitor+file.depthimage_compression_zlib+recv.portmonitor+file.depthimage_compression_zlib+type.dll</param>
       </device>

       <device xmlns:xi="http://www.w3.org/2001/XInclude"  name="pc_nws_ros" type="rgbdToPointCloudSensor_nws_ros2">
           <param name="period">0.1</param>
           <param name="node_name">pc_ros2_node</param>
           <param name="topic_name">/camera/depth/color/points</param>
           <param name="frame_id">realsense_rgb_frame</param>
           <action phase="startup" level="5" type="attach">
               <paramlist name="networks">
                   <elem name="the_device"> depth_nwc_yarp </elem>
               </paramlist>
           </action>
           <action phase="shutdown" level="5" type="detach" />
       </device>

   </devices>
</robot>

I also noticed that the intrinsic parameters are wrong in simulation:

focalLengthX 389.453
focalLengthY 389.453
tangentialPointX 0.0
tangentialPointY 0.0

The computations provide instead:

fx = 640 / (2 * tan(1.2217 / 2))
fx = 457.022 px
fx = fy
cx = (width  - 1) / 2 = (640 - 1) / 2 = 319.5
cy = (height - 1) / 2 = (480 - 1) / 2 = 239.5

So I have fixed it by setting directly on the URDF the following parameters inside <camera name="intel_realsense_rgbd_camera">

<lens>
    <intrinsics>
      <fx>457.022184</fx>
      <fy>457.022184</fy>
      <cx>319.5</cx>
      <cy>239.5</cy>
      <s>0</s>
    </intrinsics>
  </lens>

After this change I see a proper colored pointcloud:

[Nicogene]

The intrinsic of the rgdb camera has been set copying pasting from other robots, we can fix them in the configuration file of the rgbd sensor and adding the tag in the xml blob of the sensor.

After some testing I have found out that the intrinsic parameters are not being loaded by the yarp plugin.
If the echo of the ros2 topic /camera_rgbd/depth/camera_info is:

This instead it seems more a gz-sim-yarp-plugin issue, maybe you can open an issue there

[Nicogene]

@SimoneMic issue opened on gz-sim-yarp-plugins side:

• Depth Camera plugin does not handle correclty the intrinsic params robotology/gz-sim-yarp-plugins#319

[martinaxgloria]

Hi @SimoneMic, I was able to reproduce the error reported regarding the intrinsic parameters of the camera. Currently, without the intrinsic parameters explicitly defined in the URDF, launching the yri and the ros2 nws and reading both the gz sim and ros2 topic related to the camera info, the situation is:

gz-sim topic:

intrinsics {
  k: 457.02217102050781
  k: 0
  k: 320
  k: 0
  k: 457.02215194702148
  k: 240
  k: 0
  k: 0
  k: 1
}

principalPointX and principalPointY are wrong; they should be 319.5 and 239.5. The focalLength is ok.

ros2 topic:

k:
- 277.0
- 0.0
- 319.5
- 0.0
- 277.0
- 239.5
- 0.0
- 0.0
- 1.0

Both the focalLengthX/Y and the principalPointX/Y are wrong.

After:

So I have fixed it by setting directly on the URDF the following parameters inside <camera name="intel_realsense_rgbd_camera">

<lens>
    <intrinsics>
      <fx>457.022184</fx>
      <fy>457.022184</fy>
      <cx>319.5</cx>
      <cy>239.5</cy>
      <s>0</s>
    </intrinsics>
  </lens>

gz-sim topic:

intrinsics {
  k: 457.022184
  k: 0
  k: 319.5
  k: 0
  k: 457.022184
  k: 239.5
  k: 0
  k: 0
  k: 1
}

ros2 topic:

k:
- 457.022184
- 0.0
- 319.5
- 0.0
- 457.022184
- 239.5
- 0.0
- 0.0
- 1.0

Those values seem to be correct. Can you confirm @SimoneMic?

[Nicogene]

@SimoneMic you can find here the URDF to be tested:

• Automatic update of urdf triggered by GitHub Action #381