Ask Your Question
0

Using 2 D435 cameras and an RPLidar with Rtabmap

asked 2020-12-04 20:33:13 -0600

tdam2112 gravatar image

updated 2020-12-10 13:02:21 -0600

matlabbe gravatar image

hello, I'm trying to use rtabmap_ros with with 2 realsense D435 / D435i cameras and an RPLidar simultaneously to build a 3d map.

I'm using an Nvidia Jetson AGX Xavier with Jetpack 4.2.2 [rev.1] ubuntu 18.04 I've built everything using apt-get with exception of the rplidar_ros library and octomap_rviz_plugin which I built from source.

The following code has worked for me before to do as I desire:

<?xml version="1.0"?>

<launch>

  <node pkg="tf" type="static_transform_publisher" name="map_to_odom_tf"  args="0.0 0.0 0.0 0.0 0.0 0.0 /map /odom 100" />

<!-- Lidar Node initialization -->

  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
  <param name="serial_baudrate"     type="int"    value="256000"/><!--A3 -->
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  <param name="scan_mode"           type="string" value="Sensitivity"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser_tf"  args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 100" />
  <!-- Multi-cameras demo with 2 D435 cameras -->

  <!-- Choose visualization -->
  <arg name="rviz"       default="true" />
  <arg name="rtabmapviz" default="false" /> 

  <!-- Cameras -->
  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="camera" value="D435i_camera" />
    <arg name="serial_no" value="027422070495" />
    <arg name="align_depth" value="true" />
  </include>

  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="camera" value="D435_camera" />
    <arg name="serial_no" value="827312072603" />
    <arg name="align_depth" value="true" />
  </include>

  <!-- Frames: Cameras are placed at 90 degrees, clockwise -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_D435i_camera_tf" args="0.067765 0.011754 -0.041 -0.349066 0.0 1.5708 /base_link /D435i_camera_link 100" />

 <node pkg="tf" type="static_transform_publisher" name="base_to_D435_camera_tf" args="0.067765 -0.011754 -0.041 0.349066 0.0 1.5708 /base_link /D435_camera_link 100" />

   <!-- sync rgb/depth images per camera -->
   <group ns="D435i_camera">
    <node pkg="nodelet" type="nodelet" name="nodelet_manager1" args="manager"/>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync1" args="load rtabmap_ros/rgbd_sync nodelet_manager1">
      <remap from="rgb/image"       to="color/image_raw"/>
      <remap from="depth/image"     to="aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="color/camera_info"/>
      <param name="approx"          value="false"/>
    </node>
   </group>
   <group ns="D435_camera">
    <node pkg="nodelet" type="nodelet" name="nodelet_manager2" args="manager"/>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync2" args="load rtabmap_ros/rgbd_sync nodelet_manager2">
      <remap from="rgb/image"       to="color/image_raw"/>
      <remap from="depth/image"     to="aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="color/camera_info"/>
      <param name="approx"          value="false"/>
    </node>
   </group>

    <node pkg="nodelet" type="nodelet" name="nodelet_manager1" args="manager"/>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync1" args="load rtabmap_ros/rgbd_sync nodelet_manager1">
      <remap from="rgb/image"       to="color/image_raw"/>
      <remap from ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-12-10 13:12:14 -0600

matlabbe gravatar image

You have this warning:

[ WARN] [1607135102.288544514]: /rgbd_sync1: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
   /rgbd_sync1 subscribed to (approx sync):
   /color/image_raw,
   /aligned_depth_to_color/image_raw,
   /color/camera_info

I am pretty sure your image topics are not called only /color/image_raw, /aligned_depth_to_color/image_raw or /color/camera_info.

Maybe the second rgbd_sync1 kills the previous rgbd_sync1 having the right D435i namespace.

I don't remember if the current rtabmap binaries are built with more than 1 camera callbacks, but if not, you may only rebuild rtabmap_ros (clone rtabmap_ros in your catkin workspace, checkout melodic_devel branch and do catkin_make -DRTABMAP_SYNC_MULTI_RGBD=ON). If you want to use rplidar in rtabmap node, set subscribe_scan to true and remap scan topic.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-12-04 20:33:13 -0600

Seen: 202 times

Last updated: Dec 10 '20