Using 2 D435 cameras and an RPLidar with Rtabmap
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 rplidarros library and octomaprviz_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="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 ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgbd_image0" to="/D435i_camera/rgbd_image"/>
<remap from="rgbd_image1" to="/D435_camera/rgbd_image"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="gen_scan" type="bool" value="true"/>
<param name="map_negative_poses_ignored" type="bool" value="false"/> <!-- refresh grid map even if we are not moving-->
<param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/> <!-- don't fill empty space between the generated scans-->
<remap from="rgbd_image0" to="/D435i_camera/rgbd_image"/>
<remap from="rgbd_image1" to="/D435_camera/rgbd_image"/>
<param name="Grid/FromDepth" type="string" value="false"/>
<param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="2"/>
<remap from="rgbd_image0" to="/D435i_camera/rgbd_image"/>
<remap from="rgbd_image1" to="/D435_camera/rgbd_image"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
</launch>
My issue is that after rebuilding ros I get some errors regarding running two cameras at once, these are the errors I'm getting trying to run it now:
[ WARN] [1607135092.757078502]: Parameter "map_negative_poses_ignored" has been removed. Use "map_always_update" instead.
[ WARN] [1607135092.767043461]: Parameter "map_negative_scan_empty_ray_tracing" has been removed. Use "map_empty_ray_tracing" instead.
/rtabmap/rgbd_odometry subscribed to (approx sync):
/D435i_camera/rgbd_image,
/D435_camera/rgbd_image
[ERROR] (2020-12-04 18:24:56.494) OdometryF2M.cpp:243::computeTransform() Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.
and
/rtabmap/rgbd_odometry subscribed to (approx sync):
/D435i_camera/rgbd_image,
/D435_camera/rgbd_image
[ERROR] (2020-12-04 18:24:56.494) OdometryF2M.cpp:243::computeTransform() Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.
[ 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
[ WARN] (2020-12-04 18:25:02.458) OdometryF2M.cpp:533::computeTransform() Registration failed: "Not enough inliers 0/20 (matches=1) between -1 and 35"
Am I missing a package? Can it be done or do i need to build rtabmap from source (this has given me a lot of headaches with cv_bridge)
Any help would be greatly appreciated, Thanks
Asked by tdam2112 on 2020-12-04 21:33:13 UTC
Answers
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.
Asked by matlabbe on 2020-12-10 14:12:14 UTC
Comments
Is possible set-up rtabmap support depth camea and lidar same time?
I also try this condition, but i saw the below warning
[ WARN] [1644476167.484487789]: rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.
And CommonDataSubscriber.cpp in rtabmap_ros package, below code are exist.
if(subscribeScan2d && subscribeScan3d)
{
ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
subscribeScan3d = false;
It look like scan3d(point cloud) and scan2d( lidar ) is not work same time.
If I know wrong, please let us me.
Asked by myddpp on 2022-02-10 19:57:12 UTC
Comments