Official RTAB-Map Forum - mapping with kinect v2 + laser scan

mapping with kinect v2 + laser scan

Posted by ricardo on
URL: http://official-rtab-map-forum.206.s1.nabble.com/mapping-with-kinect-v2-laser-scan-tp6367.html

Hi matlabbe; I`m working on RTAB-MAp with Kinect v2 and Hokuyo URG-04LX laser scan. I followed the hand held camera tutorial and it works fine. Now I`m trying to map using the laser and de rgb-d sensor. the sequence of commands are

$ roscore
$ roslaunch urg_node urg_lidar.launch
$ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
$ roslaunch my_rtabmap demo_rtabmap.launch

the launch file for the hokuyo laser

<launch>

  <node name="urg_node" pkg="urg_node" type="urg_node" output="screen">
    <param name="ip_address" value=""/>
    <param name="serial_port" value="/dev/ttyACM0"/>
    <param name="serial_baud" value="115200"/>
    <param name="calibrate_time" value="true"/>
    <param name="publish_intensity" value="false"/>
    <param name="publish_multiecho" value="false"/>
    <param name="angle_min" value="-1.5707963"/>
    <param name="angle_max" value="1.5707963"/>
    <param name="pub_map_odom_transform" value="true"/>
	<param name="frame_id" value="laser"/>
    <param name="map_frame" value="map" />
    <param name="base_frame" value="laser" />
    
  </node>

</launch>

launch file for my demo

<launch>

 
  <!-- Choose visualization -->
  <arg name="rviz" default="true" />
  <arg name="rtabmapviz" default="false" />
  
  <!-- Choose hector_slam or icp_odometry for odometry -->
  <arg name="hector" default="true" />

  <param name="use_sim_time" type="bool" value="false"/>
  
  <node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /scanmatcher_frame /base_footprint 100" />

  <!-- Odometry from laser scans -->
  <!-- If argument "hector" is true, we use Hector mapping to generate odometry for us -->
  <node if="$(arg hector)" pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
    
    <!-- Frame names -->
    <param name="map_frame" value="hector_map" />
    <param name="base_frame" value="base_footprint" />
    <param name="odom_frame" value="odom" />
    
    <!-- Tf use -->
    <param name="pub_map_odom_transform" value="false"/>
    <param name="pub_map_scanmatch_transform" value="true"/>
    <param name="pub_odometry" value="true"/>
    
    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="2048"/>
    <param name="map_multi_res_levels" value="2" />
    
    <!-- Map update parameters -->
    <param name="map_update_angle_thresh" value="0.06" />
    
    <!-- Advertising config --> 
    <param name="scan_topic" value="/scan"/>
  </node>
  
  <!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
  <node unless="$(arg hector)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
     <remap from="scan"      to="/scan"/>
     <remap from="odom"      to="/scanmatch_odom"/>
     <remap from="odom_info"      to="/rtabmap/odom_info"/>
	  
     <param name="frame_id"        type="string" value="base_footprint"/>   
     
     <param name="Icp/PointToPlane"  type="string" value="true"/>
     <param name="Icp/VoxelSize"     type="string" value="0.05"/>
     <param name="Icp/Epsilon"       type="string" value="0.001"/>
     <param name="Icp/PointToPlaneK"  type="string" value="5"/>
     <param name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
     <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
     <param name="Icp/PM"             type="string" value="true"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
     <param name="Icp/PMOutlierRatio" type="string" value="0.95"/>
     <param name="Odom/Strategy"        type="string" value="0"/>
     <param name="Odom/GuessMotion"     type="string" value="true"/>
     <param name="Odom/ResetCountdown"  type="string" value="0"/>
     <param name="Odom/ScanKeyFrameThr"  type="string" value="0.9"/>
  </node>

  <group ns="rtabmap">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="/kinect2/sd/image_color_rect/compressed"/>
      <remap from="depth/image"     to="/kinect2/sd/image_depth_rect/compressed"/>
      <remap from="rgb/camera_info" to="/kinect2/sd/camera_info"/>
      
      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
    </node>

    <!-- SLAM -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id" type="string" value="base_footprint"/>
	
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd"  type="bool" value="true"/>
      <param name="subscribe_scan"  type="bool" value="true"/>
	
      <remap from="scan" to="/scan"/>

      <!-- As hector doesn't provide compatible covariance in the odometry topic, don't use the topic and fix the covariance -->
      <param if="$(arg hector)" name="odom_frame_id"            type="string" value="hector_map"/>
      <param if="$(arg hector)" name="odom_tf_linear_variance"  type="double" value="0.0005"/>
      <param if="$(arg hector)" name="odom_tf_angular_variance" type="double" value="0.0005"/>

      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
	
      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Reg/Force3DoF"      type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"      type="string" value="false"/>
    </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_rgbd"      type="bool" value="false"/>
      <param name="subscribe_laserScan" type="bool" value="true"/>
      <param name="frame_id"            type="string" value="base_footprint"/>
    
      <remap from="scan"            to="/scan"/>

      <!-- As hector doesn't provide compatible covariance in the odometry topic -->
      <param if="$(arg hector)" name="odom_frame_id" type="string" value="hector_map"/>

      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
    </node>
  
  </group>
  
  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgbd_image"      to="/rtabmap/rgbd_image"/>
    <remap from="cloud"           to="voxel_cloud" />

    <param name="voxel_size" type="double" value="0.01"/>
  </node>

</launch>

However I cant achieve any results. My terminal keep posting messages like this:

[ WARN] [1573075085.488619475]: /rtabmap/rgbd_sync: 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. 
/rtabmap/rgbd_sync subscribed to (approx sync):
   /kinect2/sd/image_color_rect/compressed/compressed,
   /kinect2/sd/image_depth_rect/compressed/compressedDepth,
   /kinect2/sd/camera_info
[ INFO] [1573075085.576962375]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1573075086.079350690]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1573075086.582101596]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1573075087.085216969]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1573075087.588245462]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1573075088.091082835]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1573075088.594114949]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ WARN] [1573075088.952067557]: /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/rgbd_image,
   /scan
[ INFO] [1573075089.096988050]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1573075089.600014753]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.
[ INFO] [1573075090.102954919]: lookupTransform base_footprint to laser timed out. Could not transform laser scan into base_frame.

and my RVIZ are publishing warnings about TF. RVIZ screen

Thank you for you and your incredible team. best regards