Could not find a connection between 'map' and 'icp_odom' because they are not part of the same tree."

[Masoumeh]

Hi matieu,

I want to use 3d lidar and i have activated icp_odometry as well. but i cannot show /rtabmap/odom in rviz, i have the following error:
"Could not find a connection between 'map' and 'icp_odom' because they are not part of the same tree."

in the following you can see my setting:

<arg name="navigation"   default="true"/>
  <arg name="localization" default="false"/>
  <arg name="icp_odometry" default="true"/>
  <arg name="rtabmapviz"   default="false"/>
  <arg name="camera"       default="false"/>
  <arg name="lidar2d"      default="false"/>
  <arg name="lidar3d"      default="true"/>
  <arg name="lidar3d_ray_tracing" default="true"/>
  <arg name="slam2d"       default="true"/>       
  <arg name="depth_from_lidar" default="false"/>

  <arg     if="$(arg lidar3d)" name="cell_size" default="0.3"/>
  <arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>
 
   
   
   
  <arg if="$(arg lidar2d)" name="lidar_args" default="
      --Reg/Strategy 1
      --RGBD/NeighborLinkRefining true
      --Grid/CellSize $(arg cell_size)
      --Icp/PointToPlaneRadius 0
      --Icp/MaxTranslation 1"/>
 
  <arg if="$(arg lidar3d)" name="lidar_args" default="
      --Reg/Strategy 1
      --RGBD/NeighborLinkRefining true
      --RGBD/ProximityBySpace true
      --RGBD/ProximityPathMaxNeighbors 1
      --ICP/PM true
      --Icp/PMOutlierRatio 0.7
      --Icp/VoxelSize $(arg cell_size)
      --Icp/MaxCorrespondenceDistance 1
      --Icp/PointToPlaneGroundNormalsUp 0.9
      --Icp/Iterations 10
      --Icp/Epsilon 0.001
      --OdomF2M/ScanSubtractRadius $(arg cell_size)
      --OdomF2M/ScanMaxSize 15000
      --Grid/ClusterRadius 1
      --Grid/RangeMax 20
      --Grid/RayTracing $(arg lidar3d_ray_tracing)
      --Grid/CellSize $(arg cell_size)
      --Icp/PointToPlaneRadius 0
      --Icp/PointToPlaneNormalK 10
      --Icp/MaxTranslation 1"/>

 
  <remap from="/rtabmap/grid_map" to="/map"/>
  <include file="$(find rtabmap_ros)/launch/mainrtabmap_stereo.launch">
    <arg     if="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args)" />
    <arg unless="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args) -d" /> 
    <arg name="localization"    value="$(arg localization)" />
    <arg name="visual_odometry" value="false" />
    <arg name="approx_sync"     value="$(eval camera or not icp_odometry)" />
    <arg name="imu_topic"       value="/imu/data" />
    <arg unless="$(arg icp_odometry)" name="odom_topic" value="/odometry/filtered"/>   
    <arg name="frame_id"        value="base_link" />
    <arg name="rtabmapviz"      value="$(arg rtabmapviz)" />
    <arg name="gps_topic"       value="/gps/fix"/>

   
    <arg name="subscribe_scan"  value="$(arg lidar2d)" />
    <arg if="$(arg lidar2d)" name="scan_topic"  value="/scan" />
    <arg unless="$(arg lidar2d)" name="scan_topic"  value="/scan_not_used" />   


   
    <arg name="subscribe_scan_cloud"  value="$(arg lidar3d)" />
    <arg if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
    <arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />
   
   
    <arg name="depth"             value="$(eval camera and not depth_from_lidar)" />
    <arg name="subscribe_rgb"     value="$(eval camera)" />
    <arg name="rgbd_sync"         value="$(eval camera and not depth_from_lidar)" />
    <arg name="rgb_topic"         value="/camera/color/image_raw" />   
    <arg name="camera_info_topic" value="/camera/color/camera_info" />         
    <arg name="depth_topic"       value="/camera/depth/image_rect_raw" /> 
    <arg name="approx_rgbd_sync"  value="false" />
   
   
    <arg name="gen_depth"                  value="$(arg depth_from_lidar)" />
    <arg name="gen_depth_decimation"       value="4" />
    <arg name="gen_depth_fill_holes_size"  value="3" />
    <arg name="gen_depth_fill_iterations"  value="1" />
    <arg name="gen_depth_fill_holes_error" value="0.3" />
   

   
    <arg if="$(arg icp_odometry)" name="icp_odometry" value="true" />
    <arg if="$(arg icp_odometry)" name="odom_guess_frame_id" value="odom" />
    <arg if="$(arg icp_odometry)" name="vo_frame_id" value="icp_odom" />
    <arg unless="$(arg slam2d)"   name="wait_imu_to_init" value="true" />
    <arg if="$(arg lidar3d)"      name="odom_args" value="--Icp/CorrespondenceRatio 0.01"/>
  </include>


AND the other launch file is:

      
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
  <arg name="subscribe_rgb"  default="$(arg depth)"/>

 
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="true" />
 
 
  <arg name="localization"            default="true"/>
  <arg     if="$(arg localization)" name="args"  default=""/>
  <arg unless="$(arg localization)" name="args"  default="--delete_db_on_start"/>
 
 
  <arg name="use_sim_time"            default="true"/>
 
 
 
  <arg name="cfg"                     default="" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
 
  <arg name="frame_id"                default="base_link" />     
  <arg name="odom_frame_id"           default="odom" />     
  <arg name="odom_frame_id_init"      default=""/>   
  <arg name="map_frame_id"            default="map" />
  <arg name="ground_truth_frame_id"   default=""/>     
  <arg name="ground_truth_base_frame_id" default=""/> 
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/> 
  <arg name="rtabmap_args"            default="$(arg args)"/> 
  <arg name="gdb"                     default="false"/>     
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="output"                  default="screen"/>       
  <arg name="publish_tf_map"          default="true"/>