Need help getting RealSense D415 to display map data in RViz

classic Classic list List threaded Threaded
3 messages Options
Reply | Threaded
Open this post in threaded view
|

Need help getting RealSense D415 to display map data in RViz

Exordium_AndTerminus
This post was updated on .
I am not getting any messages from rtabmap/mapData for my MapCloud or MapGraph RViz Plugins. I'm not sure why it says 0 points and 0 nodes and why I'm not seeing anything from it. Doing a rostopic echo and ros hz on rtabmap/mapData does not yield anything either. I'm not sure why the topic is not being published to.

My setup is as follows: RealSense 415 connected via USB2.0 to a laptop running a docker container that is running ROS Noetic on Ubuntu 20.04. RTABMap_Ros was installed via binary, apt install method using ros-noetic-rtabmap-ros. I have a separate PC that is going to handle the actual visualization and have already ensured that I can get images from the camera itself in RViz - just not the mapping. I also noticed the timezone is not correct between my two PCs - one is 7 hours ahead of the other - not sure if this matters?

Below are some diagnostics to assist with troubleshooting. Let me know if you need any further info and thanks very much in advance!

The launch file I use on the RealSense-hosting laptop is this:

<launch>
 
 <include file="$(find realsense2_camera)/launch/rs_camera.launch">
  <arg name="align_depth" value="true"/>
 </include>
 
 <arg name="rate"  default="5"/>
 <arg name="approx_sync" default="true" /> <!-- true for freenect driver -->
 <arg name="rgbd_sync" default="true"/>
 
 
 
 <group ns="camera">
  <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen">
        <param name="compressed_rate"  type="double" value="$(arg rate)"/>
        <param name="approx_sync"      type="bool"   value="$(arg approx_sync)"/>

        <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"/>

        <remap from="rgbd_image"      to="rgbd_image"/>
      </node>
 </group>
</launch>

RTabmap continually puts out this error, but I am not sure how to rectify it:

[ WARN] [1627604739.359593677]: /rtabmap/rtabmap: 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /rtabmap/odom \
   /camera/rgbd_image_relay \
   /rtabmap/odom_info

Rqt shows the nodes are not communicating, but I don't understand why as I did the topic remapping that rgbd_sync expects from the RealSense's camera topic outputs (as seen in the launch file)

Reply | Threaded
Open this post in threaded view
|

Re: Need help getting RealSense D415 to display map data in RViz

Exordium_AndTerminus
I got it to work finally! Had to change the rgbd_sync portion of the launch file launch file from

<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen">

to

<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/>
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync standalone_nodelet" output="screen">

Mattlabbe, this may be a good note to put in your Remote Mapping tutorial - which is what I used largely to get this working - for those that want to use the RealSense cameras instead of the kinect. Thanks also for the tutorial in the first place! I also had to ensure my topics were remapped appropriately, but the nodelet configuration was the major fix here.

Reply | Threaded
Open this post in threaded view
|

Re: Need help getting RealSense D415 to display map data in RViz

matlabbe
Administrator
Hi,

that's what I though, glad you have found the problem, I updated the wiki: wiki.ros.org/rtabmap_ros/Tutorials/RemoteMapping with a note bout that.

For rgbd_sync, there is now also a node format for convenience, and you can also start the nodelet as standalone without having to create a manager. Examples:

<!-- standalone nodelet -->
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">

<!-- or using the node interface -->
<node pkg="rtabmap_ros" type="rgbd_sync" name="rgbd_sync" output="screen">