Depth Camera plugin does not handle correclty the intrinsic params

[Nicogene]

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:

Originally posted by @SimoneMic in #374