Add odom frame and ICP-based relocalization

src/slam/arise_slam_mid360/CMakeLists.txt

@@ -182,4 +182,9 @@ install(DIRECTORY config launch
         DESTINATION share/${PROJECT_NAME}/
 )
 
+install(PROGRAMS
+  scripts/global_localization.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_package()

src/slam/arise_slam_mid360/RELOCALIZATION.md

@@ -0,0 +1,60 @@
+# Relocalization
+
+ICP-based relocalization against a prior PCD map. Publishes a `map -> odom` TF correction so the full TF tree is:
+
+```
+map -> odom -> sensor
+```
+
+In mapping mode (no prior map), `map -> odom` is identity.
+
+## How it works
+
+The `global_localization` node loads a prior PCD map, subscribes to the SLAM's registered cloud and odometry, and periodically runs ICP to align the current scan against the map. The resulting correction is published on `/map_to_odom`, which the `imu_preintegration_node` picks up and broadcasts as the `map -> odom` TF.
+
+Relocalization is triggered by publishing an initial pose estimate (e.g. from RViz "2D Pose Estimate"), then refines continuously at `freq_localization` Hz.
+
+## Usage
+
+### Automatic (recommended)
+
+Set the `MAP_PATH` environment variable before launching. The global localization node starts automatically:
+
+```bash
+export MAP_PATH=/path/to/map       # expects map.pcd at this path + .pcd
+ros2 launch arise_slam_mid360 arize_slam.launch.py
+```
+
+This sets `local_mode:=true` and `relocalization_map_path:=/path/to/map.pcd` automatically.
+
+Then in RViz, use **2D Pose Estimate** to set the initial pose and trigger the first ICP alignment.
+
+### Manual
+
+Launch SLAM and relocalization separately:
+
+```bash
+# Terminal 1: SLAM
+ros2 launch arise_slam_mid360 arize_slam.launch.py
+
+# Terminal 2: Relocalization
+ros2 launch arise_slam_mid360 relocalization.launch.py pcd_map_path:=/path/to/map.pcd
+```
+
+### Parameters
+
+| Parameter | Default | Description |
+|---|---|---|
+| `pcd_map_path` | (required) | Path to the `.pcd` map file |
+| `cloud_topic` | `/registered_scan` | Registered point cloud from SLAM |
+| `odom_topic` | `/state_estimation` | Odometry from SLAM |
+| `map_voxel_size` | `0.4` | Voxel size for downsampling global map (m) |
+| `scan_voxel_size` | `0.1` | Voxel size for downsampling current scan (m) |
+| `freq_localization` | `0.5` | ICP refinement frequency (Hz) |
+| `localization_threshold` | `0.8` | Min ICP fitness score to accept alignment |
+| `fov` | `6.28` | Field of view for map cropping (rad) |
+| `fov_far` | `300.0` | Max range for map cropping (m) |
+
+### Dependencies
+
+Python: `open3d`, `ros2_numpy`, `tf_transformations`, `numpy`

src/slam/arise_slam_mid360/config/livox_mid360.yaml

@@ -13,6 +13,7 @@ feature_extraction_node:
         depthdown_topic: "/rs_down/depth/cloud_filtered"
         world_frame: "map"
         world_frame_rot: "map_rot"
+        odom_frame: "odom"
         sensor_frame: "sensor"
         sensor_frame_rot: "sensor_rot"
 
@@ -58,6 +59,7 @@ laser_mapping_node:
         depthdown_topic: "/rs_down/depth/cloud_filtered"
         world_frame: "map"
         world_frame_rot: "map_rot"
+        odom_frame: "odom"
         sensor_frame: "laser"
         sensor_frame_rot: "sensor_rot"
 
@@ -97,6 +99,7 @@ imu_preintegration_node:
         depthdown_topic: "/rs_down/depth/cloud_filtered"
         world_frame: "map"
         world_frame_rot: "map_rot"
+        odom_frame: "odom"
         sensor_frame: "sensor"
         sensor_frame_rot: "sensor_rot"
 

src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h

@@ -5,6 +5,7 @@
 #ifndef IMUPREINTEGRATION_H
 #define IMUPREINTEGRATION_H
 
+#include <mutex>
 #include "rclcpp/rclcpp.hpp"
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -88,6 +89,9 @@ namespace arise_slam {
         void
         imuHandler(const sensor_msgs::msg::Imu::SharedPtr imu_raw);
 
+        void
+        mapToOdomHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg);
+
         void
         initial_system(double currentCorrectionTime, gtsam::Pose3 lidarPose);
 
@@ -155,6 +159,7 @@ namespace arise_slam {
         rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subImu;
         rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subLaserOdometry;
         rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subVisualOdometry;
+        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subMapToOdom;
 
         // Publishers
         rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubImuOdometry;
@@ -199,9 +204,10 @@ namespace arise_slam {
         gtsam::Pose3 lidar2Imu;
 
     public:
-        tf2::Transform map_to_odom;                     // map -> odom
+        tf2::Transform map_to_odom_;                     // map -> odom
+        std::mutex map_to_odom_mutex_;
         std::shared_ptr<tf2_ros::TransformBroadcaster> tfMap2Odom;
-        std::shared_ptr<tf2_ros::TransformBroadcaster> tfOdom2BaseLink;      // odom -> base_link
+        std::shared_ptr<tf2_ros::TransformBroadcaster> tfOdom2BaseLink;      // odom -> sensor
 
     public:
         MapRingBuffer<Imu::Ptr> imuBuf;

src/slam/arise_slam_mid360/include/arise_slam_mid360/config/parameter.h

@@ -48,6 +48,7 @@ extern std::string ProjectName;
 
 extern std::string WORLD_FRAME;
 extern std::string WORLD_FRAME_ROT;
+extern std::string ODOM_FRAME;
 extern std::string SENSOR_FRAME;
 extern std::string SENSOR_FRAME_ROT;
 extern SensorType sensor;

src/slam/arise_slam_mid360/launch/arize_slam.launch.py

@@ -3,6 +3,7 @@
 from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PythonExpression
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -59,6 +60,10 @@ def generate_launch_description():
         "sensor_frame_rot",
         default_value="sensor_rot",
     )
+    odom_frame_arg = DeclareLaunchArgument(
+        "odom_frame",
+        default_value="odom",
+    )
 
     # Localization arguments
     local_mode_arg = DeclareLaunchArgument(
@@ -139,6 +144,20 @@ def generate_launch_description():
     )
 
 
+    # Global localization node — only launched when local_mode is true
+    global_localization_node = Node(
+        package="arise_slam_mid360",
+        executable="global_localization.py",
+        name="global_localization",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("local_mode")),
+        parameters=[{
+            "pcd_map_path": LaunchConfiguration("relocalization_map_path"),
+            "cloud_topic": "/registered_scan",
+            "odom_topic": "/state_estimation",
+        }],
+    )
+
     return LaunchDescription([
         launch_ros.actions.SetParameter(name='use_sim_time', value='false'),
         config_path_arg,
@@ -149,6 +168,7 @@ def generate_launch_description():
         world_frame_rot_arg,
         sensor_frame_arg,
         sensor_frame_rot_arg,
+        odom_frame_arg,
         local_mode_arg,
         relocalization_map_path_arg,
         init_x_arg,
@@ -160,4 +180,5 @@ def generate_launch_description():
         feature_extraction_node,
         laser_mapping_node,
         imu_preintegration_node,
+        global_localization_node,
     ])

src/slam/arise_slam_mid360/launch/relocalization.launch.py

@@ -0,0 +1,81 @@
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pcd_map_path_arg = DeclareLaunchArgument(
+        "pcd_map_path",
+        default_value="",
+        description="Path to the PCD map file for relocalization",
+    )
+    cloud_topic_arg = DeclareLaunchArgument(
+        "cloud_topic",
+        default_value="/registered_scan",
+        description="Registered point cloud topic from SLAM",
+    )
+    odom_topic_arg = DeclareLaunchArgument(
+        "odom_topic",
+        default_value="/state_estimation",
+        description="Odometry topic from SLAM (odom->sensor)",
+    )
+    map_voxel_size_arg = DeclareLaunchArgument(
+        "map_voxel_size",
+        default_value="0.4",
+    )
+    scan_voxel_size_arg = DeclareLaunchArgument(
+        "scan_voxel_size",
+        default_value="0.1",
+    )
+    freq_localization_arg = DeclareLaunchArgument(
+        "freq_localization",
+        default_value="0.5",
+    )
+    localization_threshold_arg = DeclareLaunchArgument(
+        "localization_threshold",
+        default_value="0.8",
+    )
+    fov_arg = DeclareLaunchArgument(
+        "fov",
+        default_value="6.28319",
+        description="Field of view in radians (default 360 deg)",
+    )
+    fov_far_arg = DeclareLaunchArgument(
+        "fov_far",
+        default_value="300.0",
+        description="Maximum range for FOV cropping (meters)",
+    )
+
+    global_localization_node = Node(
+        package="arise_slam_mid360",
+        executable="global_localization.py",
+        name="global_localization",
+        output="screen",
+        parameters=[{
+            "pcd_map_path": LaunchConfiguration("pcd_map_path"),
+            "cloud_topic": LaunchConfiguration("cloud_topic"),
+            "odom_topic": LaunchConfiguration("odom_topic"),
+            "map_voxel_size": LaunchConfiguration("map_voxel_size"),
+            "scan_voxel_size": LaunchConfiguration("scan_voxel_size"),
+            "freq_localization": LaunchConfiguration("freq_localization"),
+            "localization_threshold": LaunchConfiguration("localization_threshold"),
+            "fov": LaunchConfiguration("fov"),
+            "fov_far": LaunchConfiguration("fov_far"),
+        }],
+    )
+
+    return LaunchDescription([
+        pcd_map_path_arg,
+        cloud_topic_arg,
+        odom_topic_arg,
+        map_voxel_size_arg,
+        scan_voxel_size_arg,
+        freq_localization_arg,
+        localization_threshold_arg,
+        fov_arg,
+        fov_far_arg,
+        global_localization_node,
+    ])