Two kinects connect to two Raspberry Pi 3 both streaming to a computer (RTABMAP/RVIZ)

[MrDD90]

I'm trying to figure out how to go about this issue. I can already stream one kinect connected to a raspberry pi 3 and stream it to a client computer by following these instructions.

http://official-rtab-map-forum.206.s1.nabble.com/RGB-D-SLAM-example-on-ROS-and-Raspberry-Pi-3-td1250.html

But I'm having a hard time on how to approach two kinects each connected to their own Raspberry Pi 3 and have both kinects show up in RTABMAP/RVIZ together in the same frame on the client computer. I know there is a demo_two_kinects.launch file that is able to do it if both kinects are connected on the same computer.  

[matlabbe]

Hi,

For a setup where we have two kinects connected to two RPI and that mapping is done on the client computer, you may base your setup on the Remote Mapping tutorial. You would have "freenect_throttle.launch" launched on both RPI (with their own namespace like "camera1" and "camera2"), and then based on demo_two_kinects.launch, you may have a launch file like this on the client computer:

<launch>

  <!-- Frames: Kinects are placed at 90 degrees, clockwise -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="-0.1325 -0.1975 0.0 -1.570796327 0.0 0.0 /base_link /camera2_link 100" />
   
   <!-- Choose visualization -->
   <arg name="rviz"       default="false" />
   <arg name="rtabmapviz" default="true" /> 
  
   <!-- sync rgb/depth images per camera -->
   <group ns="camera1">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera1_nodelet_manager">
      <param name="rgb/image_transport"   value="compressed"/>
      <param name="depth/image_transport" value="compressedDepth"/>
      <remap from="rgb/image"       to="data_throttled_image"/>
      <remap from="depth/image"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="data_throttled_camera_info"/>
    </node>
   </group>
   <group ns="camera2">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera2_nodelet_manager">
      <param name="rgb/image_transport"   value="compressed"/>
      <param name="depth/image_transport" value="compressedDepth"/>
      <remap from="rgb/image"       to="data_throttled_image"/>
      <remap from="depth/image"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="data_throttled_camera_info"/>
    </node>
   </group>
        
  <group ns="rtabmap">
  
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/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"/>       
    </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"/>
	    
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
    </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="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/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>

Here we set /base_link as the base TF of the setup, then added two TF links between /base_link and the two cameras. Make also sure all computers are synchronized (you can use ntpdate). You can also make the two RPIs slave to ROS master that would be on the client computer by setting ROS_MASTER_URI to IP of the master computer on the RPIs. You may debug TF with RVIZ on client computer by launching only the kinects and the static transform publishers, then in RVIZ show up TF (set global fixed frame to base_link) and DepthCloud displays (to see your RGB-D data).

cheers

[MrDD90]

I've setup two of the RPI's as slaves and was able to launch freenect_throttle.launch without any problems and they're able to connect to the master computer, but when I try to launch the custom launch code on the master computer you gave me, I run into some errors. Does it have to do with namespace?

$export ROS_IP=192.168.1.19
$ roslaunch test.launch
... logging to /home/test/.ros/log/811ca048-15c5-11e8-8cf6-080027408a9e/roslaunch-test-VirtualBox-24875.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

Invalid <node> tag:  tag is missing required attribute: 'name'.

Param xml is .

Node xml is <node args="load rtabmap_ros/rgbd_sync camera1_nodelet_manager" name="rgbd_sync" pkg="nodelet" type="nodelet">
     
     
      <remap from="rgb/image" to="data_throttled_image"/>
      <remap from="depth/image" to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="data_throttled_camera_info"/>
    </node>
The traceback for the exception was written to the log file


 

[matlabbe]

There was an error in the launch above:

<param from="rgb/image_transport"   value="compressed"/>
<param from="depth/image_transport" value="compressedDepth"/>

should be:

<param name="rgb/image_transport"   value="compressed"/>
<param name="depth/image_transport" value="compressedDepth"/>

I corrected in the post too.

cheers

[MrDD90]

After launching the custom launch file, I get this error/warning message

[ WARN] [1519091710.713342852]: /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,
   /camera1/rgbd_image,
   /camera2/rgbd_image

I've ran rtpdate on the master computer and on two of the RPI so I don't think its a sync issue with the time stamps. I've already run rostopic hz on
   /rtabmap/odom
   /camera1/rgbd_image
   /camera2/rgbd_image

And they all say "does not appear to be published yet"

I appreciate the continuous help!

[MrDD90]

UPDATE

So I'm able to see PointCloud2 from Kinect #1 and Kinect #2, but the PointCloud2 from Kinect#2 is displaying its point cloud in front of the point cloud coming from Kinect #1 instead of having Kinect #2 point cloud 90 degrees from Kinect #1 point cloud. This all works if I set Fixed Frame to camera_link, due to the fact base_link doesn't work.


I'm still getting the same warning messageas stated before
[ WARN] [1519091710.713342852]: /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,
   /camera1/rgbd_image,
   /camera2/rgbd_image  

[matlabbe]

Hi,

Can you show your TF Tree ($ rosrun tf view_frames)? The base_link would be linked to cameras with those static_transform_publishers:

<node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="-0.1325 -0.1975 0.0 -1.570796327 0.0 0.0 /base_link /camera2_link 100" />

What package do you use to start the kinects? I think if the camera namespace is set to camera1, the camera link would be named /camera1_link. If both camera nodes publish TF under same name /camera_link, the cameras will be shown one over the other. The frames would appear at 90 degrees like in the first screenshot of this post: https://github.com/introlab/rtabmap_ros/issues/134#issuecomment-263350458

cheers,
Mathieu

[MrDD90]

Here is my TF tree



I hope this what you meant when you ask about packages to start the Kinects.
On RPI#1 I run
$ export ROS_MASTER_URI=http://192.168.1.19:11311
$ export ROS_IP=192.168.1.9
$ ROS_NAMESPACE=camera1 roslaunch freenect_throttle.launch

On RPI#2 I run
$ export ROS_MASTER_URI=http://192.168.1.19:11311
$ export ROS_IP=192.168.1.3
$ ROS_NAMESPACE=camera2 roslaunch freenect_throttle.launch

This is what I have in the freenect_throttle.launch file
I copied the code straight from here
http://wiki.ros.org/rtabmap_ros/Tutorials/RemoteMapping

<launch>
  <include file="$(find freenect_launch)/launch/freenect.launch">
     <arg name="depth_registration" value="True" />
  </include>

  <arg name="rate"  default="5"/>
  <arg name="decimation"  default="1"/> 
  <arg name="approx_sync" default="true" />

 
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
     
     
     

      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>

      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
    </node>
  </group>     
</launch>

[matlabbe]

You cannot use the same freenect_throttle.aunch as is in the Remote tutorial for the two RPIs, you should set the camera namespace:

<launch>
  <arg name="camera_ns" value="camera"/>

  <include file="$(find freenect_launch)/launch/freenect.launch">
     <arg name="depth_registration" value="True" />
     <arg name="camera" value="$(arg camera_ns)" />
  </include>

  <arg name="rate"  default="5"/>
  <arg name="decimation"  default="1"/> <!-- Reduce the image size, e.g., 2 means "width/2 x height/2". -->
  <arg name="approx_sync" default="true" />

  <!-- Use same nodelet used by Freenect/OpenNI -->
  <group ns="$(arg camera_ns)">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="$(arg rate)"/>
      <param name="decimation" type="int" value="$(arg decimation)"/>
      <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>

      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>

      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
    </node>
  </group>      
</launch>

Then run it on each RPI:

// On RPI-1
$roslaunch freenect_throttle.launch camera_ns:="camera1"
// On RPI-2
$roslaunch freenect_throttle.launch camera_ns:="camera2"

cheers,
Mathieu

[MrDD90]

I appreciate the continuous help!
We were able to have both PointCloud2 from both Kinects show in the correct 90 degree orientation in RVIZ, thanks to your namespace fixed, but we are still having the same warning message

[ WARN] [1519493474.015567453]: /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,
   /camera1/rgbd_image,
   /camera2/rgbd_image

In addition, everything works well in RVIZ except the Map Cloud in Image#1 and RTABMAP keeps showing a black screen in Image #2.



I've also ran rostopic hz /rtabmap/mapData and the output is "no new messages"

[matlabbe]

As the warning is saying, verify that these topics are published:

$ rostopic hz /rtabmap/odom
$ rostopic hz /camera1/rgbd_image
$ rostopic hz /camera2/rgbd_image 

If /camera1/rgbd_image or /camera2/rgbd_image is not published, very that inputs of rgbd_sync are also published.

If /rtabmap/odom is not published, very that inputs of rgbd_odometry are published.

You can use "rosnode info" to see inputs/outputs of a node to debug. I recommend to use "rqt_graph" to see better how all nodes are linked and track some problems.

cheers,
Mathieu