RTABMap with 3D lidar did not make map

[2sherry]

I would like to make a map using 3d lidar. The lidar I'm using has a viewing angle of 135 degrees. I now have a tf connected by odom -> base_link -> lidar. My problem is that the map frame is not generated.

.
And my launch file.

# -*- mode: Python -*-

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node, SetRemap
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import Shutdown
import launch

################### user configure parameters for ros2 start ###################
manufacture_id = 'autol'
model_id = 'G32'
input_type = 1
frame_rate = 25

lidar_count = 1
lidar_port_1 = 5001
lidar_port_2 = 5002
lidar_port_3 = 5003
lidar_port_4 = 5004
lidar_port_5 = 5005
lidar_port_6 = 5006

pcap_path = ''
packet_per_frame = 180
read_once = 0
read_fast = 0
repeat_delay = 0.0
calibration = False

rviz_config=get_package_share_directory('autol_driver')+'/rviz/pointcloud2_config.rviz'

autol_node_parameters = [
  #Sensor Parameter
    {"manufacture_id": manufacture_id},
    {"model_id": model_id},
    {"input_type" : input_type},
    {"frame_rate": frame_rate},
  #Socket Parameter
    {"lidar_count": lidar_count},
    {"lidar_port_1": lidar_port_1},
    {"lidar_port_2": lidar_port_2},
    {"lidar_port_3": lidar_port_3},
    {"lidar_port_4": lidar_port_4},
    {"lidar_port_5": lidar_port_5},
    {"lidar_port_6": lidar_port_6},
  #Pcap Parameter
    {"pcap_path": pcap_path},
    {"packet_per_frame": packet_per_frame},
    {"read_once": read_once},
    {"read_fast": read_fast},
    {"repeat_delay": repeat_delay},
  #calibration
    {"calibration" : calibration},
]

urdf_name = 'test.urdf.xml'
urdf_file = os.path.join(get_package_share_directory('autol_driver')+'/urdf',urdf_name)
with open(urdf_file, 'r') as infp:
  robot_desc = infp.read()

autol_prefix = get_package_share_directory('autol_driver')
autol_config_dir = LaunchConfiguration('autol_config_dir', default=os.path.join(
                                                  autol_prefix, 'configuration_files'))
configuration_basename = LaunchConfiguration('configuration_basename',
                                                 default='test.lua')

def generate_launch_description():
             
  autol_driver = Node(
    package='autol_driver',
    executable='autol_driver_node',
    name='autol_lidar_publisher',
    output='screen',
    parameters=autol_node_parameters,
    remappings=[
    	('/autol_pointcloud_1', '/scan_cloud')
    ]
  )

  rtabmap_odom = Node(
    package = 'rtabmap_odom',
    executable = 'icp_odometry',
    output = 'screen',
    parameters = [{
      'frame_id':'base_link',
      'odom_frame_id':'odom',
      'wait_for_transform':0.2,
      'expected_update_rate':15.0,
    }],
    arguments=[
      'Icp/PointToPlane', 'false',
      'Icp/Iterations', '10',
      'Icp/VoxelSize', '0.2',
      'Icp/Epsilon', '0.001',
      'Icp/PointToPlaneK', '20',
      'Icp/PointToPlaneRadius', '0',
      'Icp/MaxTranslation', '2',
      'Icp/MaxCorrespondenceDistance', '1',
      'Icp/Strategy', '1',
      'Icp/OutlierRatio', '0.7',
      'Icp/CorrespondenceRatio', '0.01',
      'Odom/ScanKeyFrameThr', '0.4',
      'OdomF2M/ScanSubtractRadius', '0.1',
      'OdomF2M/ScanMaxSize', '15000',
      'OdomF2M/BundleAdjustment', 'false',
      'Odom/GuessMotion', 'true',
      'Odom/ResetCountdown', '1',
    ])
    
  rtabmap_util = Node(
    package = 'rtabmap_util',
    executable = 'point_cloud_assembler',
    output = 'screen',
    parameters = [{
      'max_cloud':10,
      'fixed_frame_id':'odom',
    }]
  )
  
  rtabmap_slam = Node(
    package = 'rtabmap_slam',
    executable = 'rtabmap',
    output = 'screen',
    parameters = [{
      'frame_id':'base_link',
      'subscribe_depth':False,
      'subscribe_rgb':False,
      'subscribe_scan':False,
      'subscribe_scan_cloud':True,
      'approx_sync':False,
      'wait_for_transform':0.2,
    }],
    arguments = [
      '-d',
      'RGBD/ProximityMaxGraphDepth', '0',
      'RGBD/ProximityPathMaxNeighbors', '1',
      'RGBD/AngularUpdate', '0.05',
      'RGBD/LinearUpdate', '0.05',
      'RGBD/CreateOccupancyGrid', 'true',
      'Mem/NotLinkedNodesKept', 'false',
      'Mem/STMSize', '30',
      'Mem/LaserScanNormalK', '20',
      'Reg/Strategy', '1',
      'Icp/VoxelSize', '0.1',
      'Icp/PointToPlaneK', '20',
      'Icp/PointToPlaneRadius', '0',
      'Icp/PointToPlane', 'true',
      'Icp/Iterations', '10',
      'Icp/Epsilon', '0.001',
      'Icp/MaxTranslation', '3',
      'Icp/MaxCorrespondenceDistance', '1',
      'Icp/Strategy', '1',
      'Icp/OutlierRatio', '0.7',
      'Icp/CorrespondenceRatio', '0.2',
    ])
    
  rtabmap_viz = Node(
    package = 'rtabmap_viz',
    executable = 'rtabmap_viz',
    output = 'screen',
    parameters = [{
      'frame_id':'base_link',
      'odom_frame_id':'odom',
      'subscribe_odom_info':True,
      'subscribe_scan_cloud':True,
      'approx_sync':False,
      }],
  )
  
  robot_state_publisher = Node(
    package = 'robot_state_publisher',
    executable = 'robot_state_publisher',
    parameters=[
      {'robot_description': robot_desc}],
    output = 'screen'
  )

  return LaunchDescription([
    autol_driver,
    rtabmap_odom,
    rtabmap_util,
    rtabmap_slam,
    rtabmap_viz,
    robot_state_publisher
  ])

Where and what did I miss?

[matlabbe]

Are there any warnings on the terminal?

The odom -> base_link TF frame from "default _authority" looks strange to me, it should be published by "icp_odometry". If two nodes are publishing "odom", that could create issues on rtabmap node.

[2sherry]

The terminal is exporting an alert. But I don't know if this has anything to do with publishing topics of the same topic. On ICP odometry

[WARN] [1719823789.655151813] [icp_odometry]: Odometry lost! Odometry will be reset after next 1 consecutive unsuccessful odometry updates...
[WARN] [1719823789.668568977] [icp_odometry]: Odometry automatically reset to latest odometry pose available from TF (odom->base_link)!
[INFO] [1719823789.668650004] [icp_odometry]: Odom: ratio=0.000000, std dev=0.000000m|0.000000rad, update time=0.013600s
[INFO] [1719823789.705445004] [icp_odometry]: Odom: ratio=0.000000, std dev=99.995000m|99.995000rad, update time=0.000559s
[INFO] [1719823790.272023608] [icp_odometry]: Odom: ratio=0.826923, std dev=0.011438m|0.003617rad, update time=0.132054s
[WARN] [1719823790.736538007] [icp_odometry]: Odometry lost! Odometry will be reset after next 1 consecutive unsuccessful odometry updates...
[WARN] [1719823790.737340801] [icp_odometry]: Odometry automatically reset to latest odometry pose available from TF (odom->base_link)!
[INFO] [1719823790.738058499] [icp_odometry]: Odom: ratio=0.000000, std dev=0.000000m|0.000000rad, update time=0.001613s
[WARN] [1719823791.702692474] [icp_odometry]: icp_odometry: 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. 

On rtabmap viz

[WARN] [1719823652.151233456] [rtabmap_viz]: rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic 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_viz subscribed to:
   /scan_cloud
[WARN] [1719823657.154653390] [rtabmap_viz]: rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic 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_viz subscribed to:
   /scan_cloud

On rtabmap slam

[WARN] [1719823688.412401888] [rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic 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 subscribed to (approx sync):
   /odom \
   /scan_cloud
[WARN] [1719823693.423493220] [rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic 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 subscribed to (approx sync):
   /odom \
   /scan_cloud

[matlabbe]

The odometry keeps getting lost, so the downstream nodes (e.g., mapping) cannot do anything. Do you have a small rosbag to share to see why odometry is getting lost?

[2sherry]

https://drive.google.com/drive/folders/1cJ3wVCB3AkytegcS4kamUrtEJHDceVSL

Will this be enough?

[matlabbe]

Th scan topic doesn't have a valid timestamp:

ros2 topic echo /scan_cloud --field header
stamp:
  sec: 0
  nanosec: 0
frame_id: point2

That's why odometry continuously resets.

[2sherry]

Still broadcaster is "default _authority" and did not receive data warning message, but I can now make a map. Thank you for your help!

[matlabbe]

Hi,

The frame rate on icp_odometry seems low, you could increase "Icp/VoxelSize".

cheers,
Mathieu