search

IntelRealSense / realsense-ros

ROS Wrapper for Intel(R) RealSense(TM) Cameras
http://wiki.ros.org/RealSense
Apache License 2.0
2.57k stars 1.75k forks source link

Trouble getting left and right images from the same camera D455 #3229

Open edoardo-cioffi-1997 opened 1 week ago

edoardo-cioffi-1997 commented 1 week ago

I am currently using ROS Noetic on UBuntu 20.04. In my project, I have built the D455 via the official repository and mounted it on a robotic arm. My problem is that when I try to get the image using :

rosrun rqt_image_view rqt_image_view

I only get like the image from the left camera. In particular , I get:

/camera/rgb/image_raw_mouse_left

Immagine 2024-10-15 112111

How do I get also the right image? Is there a way to get the whole image?

MartyG-RealSense commented 1 week ago

Hi @edoardo-cioffi-1997 If left and right infrared are enabled by setting enable_infra1 and enable_infra2 to true (as the infrared topics are disabled by default) then both infrared topics should be published. For example:

roslaunch realsense2_camera rs_camera.launch enable_infra1:=true enable_infra2:=true

The camera must also be on a USB 3 connection, as the right-side infrared is not supported on a USB 2.1 connection.

The left camera would not be the RGB camera (/camera/rgb/image_raw). It would be /camera/infra1/image_rect_raw and the right camera would be /camera/infra2/image_rect_raw

edoardo-cioffi-1997 commented 1 week ago

Where do I enable those settings? My urdf for the d455 is this one:

`

<?xml version="1.0"?>
<robot name="sensor_d455" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Includes -->
<xacro:include filename="$(find kr20)/urdf//camera/materials.urdf.xacro" />
<xacro:include filename="$(find kr20)/urdf/camera/kinetic_rgb.gazebo.xacro"/>
<xacro:macro name="sensor_d455" params="parent name:=camera use_nominal_extrinsics:=false">
<xacro:arg name="add_plug" default="false" />
<xacro:property name="M_PI" value="3.1415926535897931" />

<!-- The following values are approximate, and the camera node
 publishing TF values with actual calibrated camera extrinsic values -->
<xacro:property name="d455_cam_depth_to_infra1_offset" value="0.0"/>
<xacro:property name="d455_cam_depth_to_infra2_offset" value="-0.095"/>
<xacro:property name="d455_cam_depth_to_color_offset" value="-0.059"/>

<!-- The following values model the aluminum peripherial case for the
d455 camera, with the camera joint represented by the actual
peripherial camera tripod mount -->
<xacro:property name="d455_cam_width" value="0.124"/>
<xacro:property name="d455_cam_height" value="0.029"/>
<xacro:property name="d455_cam_depth" value="0.026"/>
<xacro:property name="d455_cam_mount_from_center_offset" value="0.0158"/>
<!-- glass cover is 0.1 mm inwards from front aluminium plate -->
<xacro:property name="d455_glass_to_front" value="0.1e-3"/>
<!-- see datasheet Revision 009, Fig. 4-4 page 68 -->
<xacro:property name="d455_zero_depth_to_glass" value="4.55e-3"/>
<!-- convenience precomputation to avoid clutter-->
<xacro:property name="d455_mesh_x_offset" value="${d455_cam_mount_from_center_offset-d455_glass_to_front-d455_zero_depth_to_glass}"/>

<!-- The following offset is relative to the physical d455 camera peripherial
camera tripod mount -->
<xacro:property name="d455_cam_depth_px" value="${d455_cam_mount_from_center_offset}"/>
<xacro:property name="d455_cam_depth_py" value="0.0475"/>
<xacro:property name="d455_cam_depth_pz" value="${d455_cam_height/2}"/>

<!-- camera body, with origin at bottom screw mount -->
<joint name="${name}_joint" type="fixed">
  <origin xyz="-0.05 0 0.45" rpy="0 1.5708 0"/>
  <parent link="${parent}"/>
  <child link="${name}_bottom_screw_frame" />
</joint>
<link name="${name}_bottom_screw_frame"/>

<joint name="${name}_link_joint" type="fixed">
  <origin xyz="${d455_mesh_x_offset} ${d455_cam_depth_py} ${d455_cam_depth_pz}" rpy="0 0 0"/>
  <parent link="${name}_bottom_screw_frame"/>
  <child link="${name}_link" />
</joint>

<link name="${name}_link">
  <visual>
    <!-- the mesh origin is at front plate in between the two infrared camera axes -->
    <origin xyz="${d455_zero_depth_to_glass + d455_glass_to_front} ${-d455_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
    <geometry>
        <mesh filename="package://kr20/visual/d455.dae" scale="0.001 0.001 0.001" />
    </geometry>
    <material name="aluminum"/>
  </visual>
  <collision>
    <origin xyz="${d455_zero_depth_to_glass-d455_cam_depth/2} ${-d455_cam_depth_py} 0" rpy="0 0 0"/>
    <geometry>
      <mesh filename="package://kr20/collision/d455.stl" scale="0.001 0.001 0.001" />
    </geometry>
  </collision>
  <inertial>
    <!-- The following are not reliable values, and should not be used for modeling -->
    <mass value="0.072" />
    <origin xyz="0 0 0" />
    <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257" />
  </inertial>
</link>

<!-- Use the nominal extrinsics between camera frames if the calibrated extrinsics aren't being published. e.g. running the device in simulation  -->
<xacro:if value="${use_nominal_extrinsics}">
  <!-- camera depth joints and links -->
  <joint name="${name}_depth_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="${name}_link"/>
    <child link="${name}_depth_frame" />
  </joint>
  <link name="${name}_depth_frame"/>

  <joint name="${name}_depth_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="${name}_depth_frame" />
    <child link="${name}_depth_optical_frame" />
  </joint>
  <link name="${name}_depth_optical_frame"/>

  <!-- camera left IR joints and links -->
  <joint name="${name}_infra1_joint" type="fixed">
    <origin xyz="0 ${d455_cam_depth_to_infra1_offset} 0" rpy="0 0 0" />
    <parent link="${name}_link" />
    <child link="${name}_infra1_frame" />
  </joint>
  <link name="${name}_infra1_frame"/>

  <joint name="${name}_infra1_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="${name}_infra1_frame" />
    <child link="${name}_infra1_optical_frame" />
  </joint>
  <link name="${name}_infra1_optical_frame"/>

  <!-- camera right IR joints and links -->
  <joint name="${name}_infra2_joint" type="fixed">
    <origin xyz="0 ${d455_cam_depth_to_infra2_offset} 0" rpy="0 0 0" />
    <parent link="${name}_link" />
    <child link="${name}_infra2_frame" />
  </joint>
  <link name="${name}_infra2_frame"/>

  <joint name="${name}_infra2_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="${name}_infra2_frame" />
    <child link="${name}_infra2_optical_frame" />
  </joint>
  <link name="${name}_infra2_optical_frame"/>

  <!-- camera color joints and links -->
  <joint name="${name}_color_joint" type="fixed">
    <origin xyz="0 ${d455_cam_depth_to_color_offset} 0" rpy="0 0 0" />
    <parent link="${name}_link" />
    <child link="${name}_color_frame" />
  </joint>
  <link name="${name}_color_frame"/>

  <joint name="${name}_color_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="${name}_color_frame" />
    <child link="${name}_color_optical_frame" />
  </joint>
  <link name="${name}_color_optical_frame"/>

  <!-- IMU -->
  <!-- see datasheet Revision 009, page 114 -->
  <xacro:property name="d455_imu_px" value="-0.01602"/>
  <xacro:property name="d455_imu_py" value="-0.03022"/>
  <xacro:property name="d455_imu_pz" value="+0.0074"/>

  <link name="${name}_accel_frame" />
  <link name="${name}_accel_optical_frame" />
  <link name="${name}_gyro_frame" />
  <link name="${name}_gyro_optical_frame" />
  <link name="${name}_imu_optical_frame" />

  <joint name="${name}_accel_joint" type="fixed">
    <origin xyz = "${d455_imu_px} ${d455_imu_py} ${d455_imu_pz}" rpy = "0 0 0" />
    <parent link="${name}_link" />
    <child link="${name}_accel_frame" />
  </joint>

  <joint name="${name}_accel_optical_joint" type="fixed">
    <origin xyz = "0 0 0" rpy = "${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="${name}_accel_frame" />
    <child link="${name}_accel_optical_frame" />
  </joint>

  <joint name="${name}_gyro_joint" type="fixed">
    <origin xyz = "${d455_imu_px} ${d455_imu_py} ${d455_imu_pz}" rpy = "0 0 0" />
    <parent link="${name}_link" />
    <child link="${name}_gyro_frame" />
  </joint>

  <joint name="${name}_gyro_optical_joint" type="fixed">
    <origin xyz = "0 0 0" rpy = "${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="${name}_gyro_frame" />
    <child link="${name}_gyro_optical_frame" />
  </joint>

  <joint name="${name}_imu_optical_joint" type="fixed">
    <origin xyz = "0 0 0" rpy = "0 0 0" />
    <parent link="${name}_gyro_optical_frame" />
    <child link="${name}_imu_optical_frame" />
  </joint>
</xacro:if>

<xacro:kinetic_rgbd_camera_gazebo name="${name}"/>

<xacro:if value="$(arg add_plug)">
  <xacro:usb_plug parent="${name}_link" name="${name}_usb_plug">
    <origin xyz="${-0.00406-d455_cam_depth_px} ${-0.03701-d455_cam_depth_py} ${-d455_cam_depth_pz}" rpy="${M_PI/2} 0 0"/>
  </xacro:usb_plug>
</xacro:if>
</xacro:macro>
</robot>

`

where kinetic_rgbd_camera_gazebo is defined as:

`

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
<xacro:macro name="kinetic_rgbd_camera_gazebo" params="name">
 <gazebo reference="${name}_link">
  <!-- Depth (IR) -->
  <sensor type="depth" name="${name}_frame_sensor">
    <always_on>true</always_on>
    <update_rate>6.0</update_rate>
    <camera>
      <horizontal_fov>${58.0 * deg_to_rad}</horizontal_fov>
      <image>
        <format>R8G8B8</format>
        <width>800</width>
        <height>600</height>
      </image>
      <clip>
        <near>0.05</near>
        <far>1.5</far>
      </clip>
    </camera>

    <plugin name="${name}_frame_controller" filename="libgazebo_ros_openni_kinect.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>6.0</updateRate>
      <cameraName>${name}</cameraName>
      <imageTopicName>ir/image_raw</imageTopicName>
      <cameraInfoTopicName>ir/camera_info</cameraInfoTopicName>
      <depthImageTopicName>depth/image_raw</depthImageTopicName>
      <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
      <pointCloudTopicName>depth/points</pointCloudTopicName>
      <frameName>${name}_optical_frame</frameName>
      <pointCloudCutoff>0.05</pointCloudCutoff>
      <pointCloudCutoffMax>5</pointCloudCutoffMax>

      <rangeMax>1.5</rangeMax>
      <!-- Distortion parameters not supported in gazebo 1.9.* plugins -->
      <!--distortionK1>0.00000001</distortionK1>
      <distortionK2>0.00000001</distortionK2>
      <distortionK3>0.00000001</distortionK3>
      <distortionT1>0.00000001</distortionT1>
      <distortionT2>0.00000001</distortionT2-->
    </plugin>
  </sensor>

  <!-- RGB -->

  <sensor type="depth" name="${name}_frame_sensor">
    <always_on>true</always_on>
    <update_rate>6.0</update_rate>

    <camera>
      <horizontal_fov>${58.0 * deg_to_rad}</horizontal_fov>
      <image>
        <format>R8G8B8</format>
        <width>800</width>
        <height>600</height>
      </image>
      <clip>
        <near>0.05</near>
        <far>1.5</far>
      </clip>
    </camera>
    <plugin name="${name}_frame_controller" filename="libgazebo_ros_openni_kinect.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>6.0</updateRate>
      <cameraName>${name}</cameraName>

      <imageTopicName>rgb/image_raw</imageTopicName>
      <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
      <pointCloudTopicName>rgb/points</pointCloudTopicName>

      <depthImageTopicName>depth/image_raw</depthImageTopicName>
      <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName> 

      <frameName>${name}_optical_frame</frameName>
      <pointCloudCutoff>0.05</pointCloudCutoff>
      <pointCloudCutoffMax>5</pointCloudCutoffMax>
      <rangeMax>1.5</rangeMax>
    </plugin>   
 </sensor> 
</gazebo>
</xacro:macro>
</robot>

`

MartyG-RealSense commented 1 week ago

My knowledge of URDF is admittedly limited. https://github.com/introlab/rtabmap_ros/issues/318#issuecomment-1824594236 has a demonstration though of remapping the infrared stream to rgb/image in the xacro file.

  remappings=[
          ('imu', '/imu/data'),
          ('rgb/image', '/camera/infra1/image_rect_raw'),
          ('rgb/camera_info', '/camera/infra1/camera_info'),
          ('depth/image', '/camera/depth/image_rect_raw')]

I do not know though how you could also obtain the Infra2 image unless you perhaps mapped it to depth/image

edoardo-cioffi-1997 commented 1 week ago

All right. By the way, the following frames : depth_optical_frame color_optical_frame infrared1_optical_frame infrared2_optical_frame accel_optical_frame gyro_optical_frame

are not defined similarly in the d455 urdf. As you can see: ` 

   <link name="${name}_accel_frame" />
   <link name="${name}_accel_optical_frame" />
   <link name="${name}_gyro_frame" />
   <link name="${name}_gyro_optical_frame" />
   <link name="${name}_imu_optical_frame" />

<joint name="${name}_accel_joint" type="fixed">
  <origin xyz = "${d455_imu_px} ${d455_imu_py} ${d455_imu_pz}" rpy = "0 0 0" />
  <parent link="${name}_link" />
  <child link="${name}_accel_frame" />
</joint>

<joint name="${name}_accel_optical_joint" type="fixed">
  <origin xyz = "0 0 0" rpy = "${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="${name}_accel_frame" />
  <child link="${name}_accel_optical_frame" />
</joint>

<joint name="${name}_gyro_joint" type="fixed">
  <origin xyz = "${d455_imu_px} ${d455_imu_py} ${d455_imu_pz}" rpy = "0 0 0" />
  <parent link="${name}_link" />
  <child link="${name}_gyro_frame" />
</joint>

<joint name="${name}_gyro_optical_joint" type="fixed">
  <origin xyz = "0 0 0" rpy = "${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="${name}_gyro_frame" />
  <child link="${name}_gyro_optical_frame" />
</joint>

<joint name="${name}_imu_optical_joint" type="fixed">
  <origin xyz = "0 0 0" rpy = "0 0 0" />
  <parent link="${name}_gyro_optical_frame" />
  <child link="${name}_imu_optical_frame" />
</joint>

`

and I would'nt even know how to define these new frames, like the infrared1_optical_frame

MartyG-RealSense commented 1 week ago

In the RealSense ROS1 wrapper's D455 URDF they are called infra1_optical_frame and infra2_optical_frame

https://github.com/IntelRealSense/realsense-ros/blob/ros1-legacy/realsense2_description/urdf/_d455.urdf.xacro#L114 https://github.com/IntelRealSense/realsense-ros/blob/ros1-legacy/realsense2_description/urdf/_d455.urdf.xacro#L129

depth_optical_frame is here: https://github.com/IntelRealSense/realsense-ros/blob/ros1-legacy/realsense2_description/urdf/_d455.urdf.xacro#L99

color_optical_frame is here: https://github.com/IntelRealSense/realsense-ros/blob/ros1-legacy/realsense2_description/urdf/_d455.urdf.xacro#L144

accel_optical_frame and gyro_optical_frame are here: https://github.com/IntelRealSense/realsense-ros/blob/ros1-legacy/realsense2_description/urdf/_d455.urdf.xacro#L155-L157