rtabmap
rtabmap copied to clipboard
introlab
Map overlap
OS: ubuntu 22.04 ROS2 humble
I use RTABmap to draw map using only 3D lidar signal from SICK nanoscan100 and odomentry from wheel. but map is overlapped. can you help me fix it? thanks
launch file:
def generate_launch_description():
ld = LaunchDescription()
sick_scan_pkg_prefix = get_package_share_directory('sick_scan_xd')
sick_launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/sick_multiscan_rtabmap.launch')
sick_node_arguments=[sick_launch_file_path]
# Append optional commandline arguments in name:=value syntax
for arg in sys.argv:
if len(arg.split(":=")) == 2:
sick_node_arguments.append(arg)
# Create sick_scan_xd node
sick_node = Node(
package='sick_scan_xd',
executable='sick_generic_caller',
output='log',
arguments=sick_node_arguments
)
# Common parameter
wait_for_transform = 0.01
point_cloud_topic = "/cloud_all_fields_fullframe" # Point cloud topic published by sick_scan_xd
point_cloud_frame_id = "base_cloud" # Point cloud frame id published by sick_scan_xd
imu_topic = "imu_not_used" #"/multiScan/imu" # IMU topic published by sick_scan_xd
deskewing = True # Optional lidar deskewing
if deskewing: # Create deskewing node
point_cloud_topic_desk = f"{point_cloud_topic}/deskewed"
deskewing_node = Node(
package='rtabmap_util', executable='lidar_deskewing', name="lidar_deskewing", output="screen",
parameters=[{
"wait_for_transform": wait_for_transform,
"fixed_frame_id": point_cloud_frame_id,
"slerp": False,
}],
remappings=[("input_cloud", point_cloud_topic)],
arguments=[],
namespace="deskewing")
else:
point_cloud_topic_desk = point_cloud_topic
deskewing_node = None
# Create rtabmap_odom node
rtabmap_odom_node = Node(
package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
parameters=[{
"frame_id": point_cloud_frame_id,
"odom_frame_id": "odom",
"guess_frame_id": point_cloud_frame_id,
"wait_imu_to_init": False,
"wait_for_transform_duration": str(wait_for_transform),
"Icp/PointToPlane": "True",
"Icp/Iterations": "10",
"Icp/VoxelSize": "0.2",
"Icp/DownsamplingStep": "1",
"Icp/Epsilon": "0.001",
"Icp/PointToPlaneK": "20",
"Icp/PointToPlaneRadius": "0",
"Icp/MaxTranslation": "2",
"Icp/MaxCorrespondenceDistance": "1",
"Icp/PM": "True",
"Icp/PMOutlierRatio": "0.1",
"Icp/CorrespondenceRatio": "0.01",
"Icp/ReciprocalCorrespondences": "False",
"Odom/ScanKeyFrameThr": "0.8",
"Odom/Strategy": "0",
"OdomF2M/ScanSubtractRadius": "0.2",
"OdomF2M/ScanMaxSize": "15000",
}],
remappings=[("scan_cloud", point_cloud_topic_desk), ("imu", imu_topic)],
arguments=[],
namespace="rtabmap")
# Create rtabmap_slam node, see https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_launch/launch/rtabmap.launch.py for an example configuration
rtabmap_slam_node = Node(
package='rtabmap_slam', executable='rtabmap', name="rtabmap", output="screen",
parameters=[{
'Grid/RayTracing': 'true',
'Grid/FromDepth': True,
"subscribe_odom_info": False,
"publish_tf_odom": False,
"frame_id": point_cloud_frame_id,
"subscribe_depth": False,
"subscribe_rgb": False,
"subscribe_rgbd": False,
"subscribe_scan": False,
"subscribe_scan_cloud": True,
"approx_sync": True,
"Rtabmap/DetectionRate": "1",
"RGBD/NeighborLinkRefining": "False",
"RGBD/ProximityBySpace": "True",
"RGBD/ProximityMaxGraphDepth": "0",
"RGBD/ProximityPathMaxNeighbors": "1",
"RGBD/AngularUpdate": "0.05",
"RGBD/LinearUpdate": "0.05",
"Mem/NotLinkedNodesKept": "False",
"Mem/STMSize": "30",
"Reg/Strategy": "1",
"Grid/CellSize": "0.1",
"Grid/RangeMax": "20",
"Grid/ClusterRadius": "1",
"Grid/GroundIsObstacle": "True",
"Optimizer/GravitySigma": "0.3",
"Icp/VoxelSize": "0.3",
"Icp/PointToPlaneK": "20",
"Icp/PointToPlaneRadius": "0",
"Icp/PointToPlane": "False",
"Icp/Iterations": "10",
"Icp/Epsilon": "0.001",
"Icp/MaxTranslation": "3",
"Icp/MaxCorrespondenceDistance": "1",
"Icp/PM": "True",
"Icp/PMOutlierRatio": "0.7",
"Icp/CorrespondenceRatio": "0.4",
}],
remappings=[("scan_cloud", point_cloud_topic_desk), ("imu", imu_topic),('odom', 'odom')],
arguments=["-d"], # parameter "-d": delete_db_on_start
namespace="")
# Create rtabmap_viz node
rtabmap_viz_node = Node(
package='rtabmap_viz', executable='rtabmap_viz', name="rtabmap_viz", output="screen",
parameters=[{
"frame_id": point_cloud_frame_id,
"odom_frame_id": "odom",
"subscribe_odom_info": False,
"subscribe_scan_cloud": True,
"approx_sync": False,
}],
remappings=[("scan_cloud", point_cloud_topic_desk)],
arguments=[],
namespace="rtabmap")
ld.add_action(sick_node)
if deskewing:
ld.add_action(deskewing_node)
# ld.add_action(rtabmap_odom_node)
ld.add_action(rtabmap_slam_node)
ld.add_action(rtabmap_viz_node)
return ld
That is the TF published by wheel odometry? Is it odom? If so, try for icp_odometry:
"frame_id": point_cloud_frame_id,
"odom_frame_id": "icp_odom",
"guess_frame_id": "odom",
...
#remaps:
("odom", "icp_odom")
If you do deskweing, use odom (wheel odometry) as fixed_frame_id. However, wheel odometry should be published faster than lidar and be pretty accurate under lidar full rotation time. Otherwise, you may enable deskewing inside icp_odometry node instead.
See also demos on this page: https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos
yes. i use tf odom -> base_footprint from the odometry of the wheel. i dont want to use icp_odometry because it has oscillation when the robot is stationary.