feat(slam): add rtabmap_ros

2025-07-14 11:34:38 +08:00
parent 3b6641c1fb
commit 943ce5b06f
1635 changed files with 603092 additions and 0 deletions
@@ -0,0 +1,93 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtabmap_msgs)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
+  # issues #1285 #1288
+  find_library(
+    rcutils_LIB NAMES rcutils
+    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+  )
+endif()
+
+##################
+## Dependencies ##
+##################
+find_package(rosidl_default_generators REQUIRED)
+
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+# declare the message files to generate code for
+set(msg_files
+   "msg/Info.msg"
+   "msg/KeyPoint.msg"
+   "msg/GlobalDescriptor.msg"
+   "msg/ScanDescriptor.msg"
+   "msg/MapData.msg"
+   "msg/MapGraph.msg"
+   "msg/Node.msg"
+   "msg/SensorData.msg"
+   "msg/Link.msg"
+   "msg/LandmarkDetection.msg"
+   "msg/LandmarkDetections.msg"
+   "msg/OdomInfo.msg"
+   "msg/Point2f.msg"
+   "msg/Point3f.msg"
+   "msg/Goal.msg"
+   "msg/RGBDImage.msg"
+   "msg/RGBDImages.msg"
+   "msg/UserData.msg"
+   "msg/GPS.msg"
+   "msg/Path.msg"
+   "msg/EnvSensor.msg"
+   "msg/CameraModel.msg"
+   "msg/CameraModels.msg"
+)
+
+# declare the service files to generate code for
+set(srv_files
+   "srv/GetMap.srv"
+   "srv/GetMap2.srv"
+   "srv/ListLabels.srv"
+   "srv/PublishMap.srv"
+   "srv/ResetPose.srv"
+   "srv/SetGoal.srv"
+   "srv/SetLabel.srv"
+   "srv/RemoveLabel.srv"
+   "srv/GetPlan.srv"
+   "srv/AddLink.srv"
+   "srv/GetNodeData.srv"
+   "srv/GetNodesInRadius.srv"
+   "srv/LoadDatabase.srv"
+   "srv/DetectMoreLoopClosures.srv"
+   "srv/GlobalBundleAdjustment.srv"
+   "srv/CleanupLocalGrids.srv"
+)
+
+## Generate messages and services
+rosidl_generate_interfaces(${PROJECT_NAME}
+   ${msg_files}
+   ${srv_files}
+   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs sensor_msgs std_srvs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()
@@ -0,0 +1,4 @@
+
+sensor_msgs/CameraInfo camera_info
+geometry_msgs/Transform local_transform
+
@@ -0,0 +1,3 @@
+
+CameraModel[] models
+
@@ -0,0 +1,6 @@
+
+std_msgs/Header header
+
+# EnvSensor
+int32 type
+float64 value
@@ -0,0 +1,7 @@
+
+float64 stamp      # in seconds
+float64 longitude  # DD format
+float64 latitude   # DD format
+float64 altitude   # in meters
+float64 error      # in meters
+float64 bearing    # North 0->360 deg
@@ -0,0 +1,8 @@
+
+std_msgs/Header header
+
+# compressed global descriptor
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+int32 type
+uint8[] info
+uint8[] data
@@ -0,0 +1,9 @@
+
+std_msgs/Header header
+
+# Set either node_id or node_label
+int32 node_id
+string node_label
+
+# optional: if not set, the base frame of the robot is used
+string frame_id
@@ -0,0 +1,50 @@
+
+########################################
+# RTAB-Map info with statistics 
+########################################
+
+std_msgs/Header header
+
+int32 ref_id
+int32 loop_closure_id
+int32 proximity_detection_id
+int32 landmark_id
+
+geometry_msgs/Transform loop_closure_transform
+
+####
+# For statistics...
+####
+# State (node IDs) of the current Working Memory (including STM)
+int32[] wm_state
+
+# std::map<int, float> posterior;
+int32[] posterior_keys
+float32[] posterior_values
+
+# std::map<int, float> likelihood;
+int32[] likelihood_keys
+float32[] likelihood_values
+
+# std::map<int, float> raw_likelihood;
+int32[] raw_likelihood_keys
+float32[] raw_likelihood_values
+
+# std::map<int, int> weights;
+int32[] weights_keys
+int32[] weights_values
+
+# std::map<int, std::string> labels;
+int32[] labels_keys
+string[] labels_values
+
+# std::map<std::string, float> stats
+string[] stats_keys
+float32[] stats_values
+
+# std::vector<int> local_path
+int32[] local_path
+int32 current_goal_id
+
+# std::vector<int> odomCache
+MapGraph odom_cache
@@ -0,0 +1,16 @@
+#class cv::KeyPoint
+#{
+#    Point2f pt;
+#    float size;
+#    float angle;
+#    float response;
+#    int octave;
+#    int class_id;
+#}
+
+Point2f pt
+float32 size
+float32 angle
+float32 response
+int32 octave
+int32 class_id
@@ -0,0 +1,21 @@
+
+# header.stamp: the timestamp of the detection (e.g. image timestamp)
+# header.frame_id: the base frame of pose (e.g., camera optical frame)
+std_msgs/Header header
+
+# Landmark's frame id
+string landmark_frame_id
+
+# Landmark's unique ID: should be >0
+int32 id
+
+# Size in meters of the landmark/tag (optional, set 0 to not use it).
+float32 size
+
+# Pose of the landmark in header.frame_id frame.
+# If covariance is unknown, keep it as null matrix.
+# rtabmap_slam/rtabmap's landmark_angular_variance and
+# landmark_linear_variance parameters can be used
+# for convenience if covariance is null.
+geometry_msgs/PoseWithCovariance pose
+
@@ -0,0 +1,7 @@
+
+# header.stamp: the timestamp of the detection (e.g. image timestamp)
+# header.frame_id: the base frame of pose (e.g., camera optical frame)
+std_msgs/Header header
+
+LandmarkDetection[] landmarks
+
@@ -0,0 +1,14 @@
+#class rtabmap::Link
+#{
+#   int from;
+#   int to;
+#   Type type;
+#   Transform transform;
+#   cv::Mat(6,6,CV_64FC1) information;
+#}
+
+int32 from_id
+int32 to_id
+int32 type
+geometry_msgs/Transform transform
+float64[36] information
@@ -0,0 +1,15 @@
+
+std_msgs/Header header
+
+##################
+# Optimized graph
+##################
+MapGraph graph
+
+##################
+# Graph data
+##################
+Node[] nodes
+
+
+
@@ -0,0 +1,18 @@
+
+std_msgs/Header header
+
+##
+# /map to /odom transform
+# Always identity when the graph is optimized from the latest pose.
+##
+geometry_msgs/Transform map_to_odom
+
+# The poses
+int32[] poses_id
+geometry_msgs/Pose[] poses
+
+# The links
+Link[] links
+
+
+
@@ -0,0 +1,23 @@
+#class rtabmap::Signature
+
+int32 id
+int32 map_id
+int32 weight
+float64 stamp
+string label
+
+# Pose from odometry not corrected
+geometry_msgs/Pose pose
+
+# std::multimap<wordId, index>
+# std::vector<cv::Keypoint>
+# std::vector<cv::Point3f>
+int32[] word_id_keys
+int32[] word_id_values
+KeyPoint[] word_kpts
+Point3f[] word_pts
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] word_descriptors
+
+SensorData data
@@ -0,0 +1,72 @@
+
+std_msgs/Header header
+
+bool lost
+int32 matches
+int32 inliers
+float32 icp_inliers_ratio
+float32 icp_rotation
+float32 icp_translation
+float32 icp_structural_complexity
+float32 icp_structural_distribution
+int32 icp_correspondences
+float64[36] covariance
+int32 features
+int32 local_map_size
+int32 local_scan_map_size
+int32 local_key_frames
+int32 local_bundle_outliers
+int32 local_bundle_constraints
+float32 local_bundle_time
+float32 local_bundle_avg_inlier_distance
+int32 local_bundle_max_key_frames_for_inlier
+bool key_frame_added
+float32 time_estimation
+float32 time_particle_filtering
+float32 stamp
+float32 interval
+float32 distance_travelled
+int32 memory_usage # MB
+
+float32 gravity_roll_error
+float32 gravity_pitch_error
+
+# Local bundle camera ids
+int32[] local_bundle_ids
+
+# Local bundle camera models
+CameraModels[] local_bundle_models
+
+# Local bundle camera poses
+geometry_msgs/Pose[] local_bundle_poses
+
+geometry_msgs/Transform transform
+geometry_msgs/Transform transform_filtered
+geometry_msgs/Transform transform_ground_truth
+geometry_msgs/Transform guess
+
+# 0=F2M, 1=F2F
+int32 type
+
+# F2M odometry
+#   std::multimap<int, cv::KeyPoint> words;
+#   std::vector<int> wordMatches;
+#   std::vector<int> wordInliers;
+int32[] words_keys
+KeyPoint[] words_values
+int32[] word_matches
+int32[] word_inliers
+int32[] local_map_keys
+Point3f[] local_map_values
+
+# local scan map data
+sensor_msgs/PointCloud2 local_scan_map
+
+# F2F odometry
+#   std::vector<cv::Point2f> ref_corners;
+#   std::vector<cv::Point2f> new_corners;
+#   std::vector<int> corner_inliers;
+Point2f[] ref_corners
+Point2f[] new_corners
+int32[] corner_inliers
+
@@ -0,0 +1,6 @@
+
+std_msgs/Header header
+
+int32[] node_ids
+geometry_msgs/Pose[] poses
+
@@ -0,0 +1,8 @@
+#class cv::Point2f
+#{
+#    float x;
+#    float y;
+#}
+
+float32 x
+float32 y
@@ -0,0 +1,10 @@
+#class cv::Point3f
+#{
+#    float x;
+#    float y;
+#    float z;
+#}
+
+float32 x
+float32 y
+float32 z
@@ -0,0 +1,25 @@
+
+std_msgs/Header header
+
+# For stereo, rgb corresponds to left camera, and depth the right camera.
+
+# camera info
+sensor_msgs/CameraInfo rgb_camera_info
+sensor_msgs/CameraInfo depth_camera_info
+
+# Raw
+sensor_msgs/Image rgb
+sensor_msgs/Image depth
+
+# Compressed
+sensor_msgs/CompressedImage rgb_compressed
+sensor_msgs/CompressedImage depth_compressed
+
+# Local features
+KeyPoint[] key_points
+Point3f[] points
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] descriptors
+
+GlobalDescriptor global_descriptor
@@ -0,0 +1,4 @@
+
+std_msgs/Header header
+
+RGBDImage[] rgbd_images
@@ -0,0 +1,8 @@
+
+std_msgs/Header header
+
+# scan or scan_cloud is set
+sensor_msgs/LaserScan scan
+sensor_msgs/PointCloud2 scan_cloud
+
+GlobalDescriptor global_descriptor
@@ -0,0 +1,66 @@
+#class rtabmap::SensorData
+
+std_msgs/Header header
+
+# For RGB-D, left corresponds to rgb camera, and right corresponds to depth camera.
+
+# Raw images
+sensor_msgs/Image left
+sensor_msgs/Image right
+
+# Compressed images
+# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
+uint8[] left_compressed
+uint8[] right_compressed
+
+# Camera info
+sensor_msgs/CameraInfo[] left_camera_info
+sensor_msgs/CameraInfo[] right_camera_info
+
+# Transform from base frame to camera frame
+geometry_msgs/Transform[] local_transform
+
+# raw 2d or 3D laser scan
+sensor_msgs/PointCloud2 laser_scan
+# compressed 2D or 3D laser scan
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] laser_scan_compressed
+int32 laser_scan_max_pts
+float32 laser_scan_max_range
+int32 laser_scan_format
+# local transform (base frame -> laser frame)
+geometry_msgs/Transform laser_scan_local_transform
+
+# compressed user data
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] user_data
+
+# compressed occupancy grid
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] grid_ground
+uint8[] grid_obstacles
+uint8[] grid_empty_cells
+float32 grid_cell_size
+Point3f grid_view_point
+
+# Local features
+KeyPoint[] key_points
+Point3f[] points
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] descriptors
+
+GlobalDescriptor[] global_descriptors
+
+EnvSensor[] env_sensors
+
+sensor_msgs/Imu imu
+geometry_msgs/Transform imu_local_transform
+
+LandmarkDetection[] landmarks
+
+# Ground truth
+geometry_msgs/Pose ground_truth_pose
+
+# GPS
+GPS gps
@@ -0,0 +1,12 @@
+
+std_msgs/Header header
+
+# OpenCV matrix containing the user data. A matrix of type CV_8UC1 
+# with 1 row is considered to be compressed (with rtabmap::compressData() method).
+# If you have one dimension unsigned 8 bits uncompressed data, make sure to transpose it
+# (to have multiple rows instead of multiple columns) in order to be detected as
+# not compressed.
+uint32 rows
+uint32 cols
+uint32 type
+uint8[] data
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtabmap_msgs</name>
+  <version>0.22.0</version>
+  <description>RTAB-Map's msgs package.</description>
+  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>ros_environment</build_depend>
+
+  <depend>builtin_interfaces</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>
@@ -0,0 +1,4 @@
+#request
+Link link
+---
+#response
@@ -0,0 +1,24 @@
+#  Cleanup local grids service
+# 
+#     Clear empty space from local occupancy grids 
+#     (and laser scans) based on the current optimized global 2d grid map.
+#     If the map needs to be regenerated in the future (e.g., when
+#     we re-use the map in SLAM mode), removed obstacles won't reappear.
+#     Use this with care and only when you know that the map doesn't have errors, 
+#     otherwise some real obstacles/walls may be cleared if there is too much 
+#     drift in the map.
+#
+
+# Radius in cells around empty cell without obstacles to clear underlying obstacles, default 1 cell if not set.
+int32 radius
+
+# Filter also the scans, default false if not set.
+# The filtered laser scans will be used for localization,
+# so if dynamic obstacles have been removed, localization won't try to
+# match them anymore. Filtering the laser scans cannot be reverted, 
+# but grids can (see DatabaseViewer->Edit menu).
+bool filter_scans
+
+---
+# return the number of grids or scans modified, -1 if there is an error
+int32 modified
@@ -0,0 +1,27 @@
+# Detect more loop closures service
+#
+#     Based on the current optimized graph, 
+#     this process will try to find more nodes corresponding with each 
+#     other, and thus finding more loop closures to add to graph.
+# 
+
+# Cluster radius (m),     default 1 m if not set
+float32 cluster_radius_max
+
+# Cluster radius min (m), default 0 m if not set
+float32 cluster_radius_min
+
+# Cluster angle (deg),    default 0 deg if not set
+float32 cluster_angle
+
+# Iterations,             default 1 if not set
+int32 iterations
+
+# Add only intra session loop closures
+bool intra_only
+
+# Add only inter session loop closures
+bool inter_only
+---
+# return the number of loop closures detected, or -1 if it failed.
+int32 detected
@@ -0,0 +1,7 @@
+#request
+bool global_map
+bool optimized
+bool graph_only
+---
+#response
+MapData data
@@ -0,0 +1,12 @@
+#request
+bool global_map
+bool optimized
+bool with_images
+bool with_scans
+bool with_user_data
+bool with_grids
+bool with_words
+bool with_global_descriptors
+---
+#response
+MapData data
@@ -0,0 +1,9 @@
+#request
+int32[] ids
+bool images
+bool scan
+bool grid
+bool user_data
+---
+#response
+Node[] data
@@ -0,0 +1,31 @@
+#request
+
+# In mapping mode (Mem/IncrementalMemory=true), if target pose 
+#   and node_id are all zeros, poses around the latest node 
+#   in the graph are returned.
+# In localization mode (Mem/IncrementalMemory=false), if target pose 
+#   and node_id are all zeros, poses around the latest localization 
+#   pose are returned.
+# If node_id is not zero, target pose is ignored.
+
+# Node id
+int32 node_id
+
+# Target pose:
+float32 x
+float32 y
+float32 z
+
+# Radius, <=0 means that RGBD/LocalRadius will be used 
+# if k is also 0. If k>0 and a radius of 0 means all nearest 
+# poses up to k.
+float32 radius
+
+# Maximum number of nearest poses
+int32 k
+
+---
+#response
+int32[] ids
+geometry_msgs/Pose[] poses
+float32[] dists_sqr
@@ -0,0 +1,11 @@
+# Get a plan from the current position to the goal node or pose 
+
+# The final node of the goal (set 0 to use pose instead)
+int32 goal_node
+# The final pose of the goal position (used only if goalNodeId=0)
+geometry_msgs/PoseStamped goal
+
+# How many meters from the map's graph we can plan (0=infinite)
+float32 tolerance
+---
+Path plan
@@ -0,0 +1,22 @@
+# Global Bundle Adjustment service
+#
+#       Perform global bundle adjustment. Note that as soon as the map 
+#       is modified again, the graph is re-optimized the standard way (without SBA).
+#       It then makes only sense to use this after a mapping run (and after a call 
+#       to /rtabmap/pause) when you know that the robot will restart in localization 
+#       mode the next time, or at the beginning of the localization session.
+#
+
+# Optimizer type (0=g2o, 1=CVSBA), default 0
+int32 type
+
+# Iterations,        default 0 (use Optimizer/Iterations already loaded in the node)
+int32 iterations
+
+# Pixel variance,    default 0 (use g2o/PixelVariance already loaded in the node)
+float32 pixel_variance
+
+# Use vocabulary matches, default false (rematch all features between frames)
+bool voc_matches
+---
+# return false if failure
@@ -0,0 +1,5 @@
+#request
+---
+#response 
+int32[] ids
+string[] labels
@@ -0,0 +1,11 @@
+#request
+
+# Local database path on which rtabmap is running.
+# If the path doesn't exist, a new database will be created.
+string database_path
+
+# If the database already exists, data will be cleared if true.
+bool clear
+
+---
+#response, return false on rtabmap initialization failure.
@@ -0,0 +1,6 @@
+#request
+bool global_map
+bool optimized
+bool graph_only
+---
+#response
@@ -0,0 +1,4 @@
+#request
+string label
+---
+#response 
@@ -0,0 +1,9 @@
+#request
+float32 x
+float32 y
+float32 z
+float32 roll
+float32 pitch
+float32 yaw
+---
+#response
@@ -0,0 +1,15 @@
+#request
+
+# Set either node_id or node_label
+int32 node_id
+string node_label
+
+# optional: if not set, the base frame of the robot is used
+string frame_id
+
+---
+
+#response 
+int32[] path_ids
+geometry_msgs/Pose[] path_poses
+float32 planning_time
@@ -0,0 +1,6 @@
+#request
+# Set node_id = 0 to set label to last node
+int32 node_id
+string node_label
+---
+#response