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

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Two-kinects-connect-to-two-Raspberry-Pi-3-both-streaming-to-a-computer-RTABMAP-RVIZ-tp4295p4297.html

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