From 0602f5e784f2c78146a9f6ff95520761ab292696 Mon Sep 17 00:00:00 2001 From: Bona Date: Fri, 3 Apr 2026 11:00:30 -0700 Subject: [PATCH 1/2] Add odom frame and ICP-based relocalization Split the TF tree from map->sensor to map->odom->sensor following standard ROS conventions. The imu_preintegration node now publishes both odom->sensor (continuous) and map->odom (identity by default, updated by relocalization). Added global_localization.py (adapted from FAST_LIO_LOCALIZATION2) which performs ICP against a prior PCD map and publishes corrections on /map_to_odom. The node auto-launches when MAP_PATH env is set (local_mode=true). --- src/slam/arise_slam_mid360/CMakeLists.txt | 5 + src/slam/arise_slam_mid360/RELOCALIZATION.md | 60 ++++ .../config/livox_mid360.yaml | 3 + .../ImuPreintegration/imuPreintegration.h | 10 +- .../arise_slam_mid360/config/parameter.h | 1 + .../launch/arize_slam.launch.py | 21 ++ .../launch/relocalization.launch.py | 81 ++++++ .../scripts/global_localization.py | 275 ++++++++++++++++++ .../ImuPreintegration/imuPreintegration.cpp | 70 ++++- .../src/parameter/parameter.cpp | 4 + 10 files changed, 513 insertions(+), 17 deletions(-) create mode 100644 src/slam/arise_slam_mid360/RELOCALIZATION.md create mode 100644 src/slam/arise_slam_mid360/launch/relocalization.launch.py create mode 100755 src/slam/arise_slam_mid360/scripts/global_localization.py diff --git a/src/slam/arise_slam_mid360/CMakeLists.txt b/src/slam/arise_slam_mid360/CMakeLists.txt index d4a45d29..639b7813 100755 --- a/src/slam/arise_slam_mid360/CMakeLists.txt +++ b/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() diff --git a/src/slam/arise_slam_mid360/RELOCALIZATION.md b/src/slam/arise_slam_mid360/RELOCALIZATION.md new file mode 100644 index 00000000..22349495 --- /dev/null +++ b/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` diff --git a/src/slam/arise_slam_mid360/config/livox_mid360.yaml b/src/slam/arise_slam_mid360/config/livox_mid360.yaml index a2fbfd56..a3dfb043 100644 --- a/src/slam/arise_slam_mid360/config/livox_mid360.yaml +++ b/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" diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h index b2f5de3b..d1307d70 100755 --- a/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h +++ b/src/slam/arise_slam_mid360/include/arise_slam_mid360/ImuPreintegration/imuPreintegration.h @@ -5,6 +5,7 @@ #ifndef IMUPREINTEGRATION_H #define IMUPREINTEGRATION_H +#include #include "rclcpp/rclcpp.hpp" #include #include @@ -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::SharedPtr subImu; rclcpp::Subscription::SharedPtr subLaserOdometry; rclcpp::Subscription::SharedPtr subVisualOdometry; + rclcpp::Subscription::SharedPtr subMapToOdom; // Publishers rclcpp::Publisher::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 tfMap2Odom; - std::shared_ptr tfOdom2BaseLink; // odom -> base_link + std::shared_ptr tfOdom2BaseLink; // odom -> sensor public: MapRingBuffer imuBuf; diff --git a/src/slam/arise_slam_mid360/include/arise_slam_mid360/config/parameter.h b/src/slam/arise_slam_mid360/include/arise_slam_mid360/config/parameter.h index 6a43b53d..0dc4d2e7 100755 --- a/src/slam/arise_slam_mid360/include/arise_slam_mid360/config/parameter.h +++ b/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; diff --git a/src/slam/arise_slam_mid360/launch/arize_slam.launch.py b/src/slam/arise_slam_mid360/launch/arize_slam.launch.py index b8014b12..29d3202b 100644 --- a/src/slam/arise_slam_mid360/launch/arize_slam.launch.py +++ b/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, ]) diff --git a/src/slam/arise_slam_mid360/launch/relocalization.launch.py b/src/slam/arise_slam_mid360/launch/relocalization.launch.py new file mode 100644 index 00000000..db116112 --- /dev/null +++ b/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, + ]) diff --git a/src/slam/arise_slam_mid360/scripts/global_localization.py b/src/slam/arise_slam_mid360/scripts/global_localization.py new file mode 100755 index 00000000..8071c012 --- /dev/null +++ b/src/slam/arise_slam_mid360/scripts/global_localization.py @@ -0,0 +1,275 @@ +#!/usr/bin/env python3 + +import copy +import threading +import time +import logging + +# Suppress ros2_numpy warnings about missing rgb/intensity fields +logging.getLogger("ros2_numpy").setLevel(logging.ERROR) + +import open3d as o3d +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion +from nav_msgs.msg import Odometry +from sensor_msgs.msg import PointCloud2 +from std_msgs.msg import Header +import numpy as np +import tf2_ros +import tf_transformations +import ros2_numpy + + +class GlobalLocalization(Node): + def __init__(self): + super().__init__("global_localization") + self.global_map = None + self.T_map_to_odom = np.eye(4) + self.cur_odom = None + self.cur_scan = None + self.initialized = False + self.localization_lock = threading.Lock() + + self.declare_parameters( + namespace="", + parameters=[ + ("map_voxel_size", 0.4), + ("scan_voxel_size", 0.1), + ("freq_localization", 0.5), + ("localization_threshold", 0.8), + ("fov", 6.28319), + ("fov_far", 300.0), + ("pcd_map_path", ""), + ("cloud_topic", "/registered_scan"), + ("odom_topic", "/state_estimation"), + ], + ) + + self.callback_group = ReentrantCallbackGroup() + + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + qos_profile = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=10 + ) + + self.pub_pc_in_map = self.create_publisher(PointCloud2, "/cur_scan_in_map", qos_profile) + self.pub_submap = self.create_publisher(PointCloud2, "/submap", qos_profile) + self.pub_map_to_odom = self.create_publisher(Odometry, "/map_to_odom", qos_profile) + + self.get_logger().info("Loading global map...") + self.initialize_global_map() + self.get_logger().info("Global map loaded.") + + cloud_topic = self.get_parameter("cloud_topic").value + odom_topic = self.get_parameter("odom_topic").value + + self.create_subscription(PointCloud2, cloud_topic, self.cb_save_cur_scan, qos_profile, callback_group=self.callback_group) + self.create_subscription(Odometry, odom_topic, self.cb_save_cur_odom, qos_profile, callback_group=self.callback_group) + self.create_subscription(PoseWithCovarianceStamped, "/initialpose", self.cb_initialize_pose, qos_profile, callback_group=self.callback_group) + + self.get_logger().info(f"Subscribed to cloud: {cloud_topic}, odom: {odom_topic}") + + self.timer_localisation = self.create_timer( + 1.0 / self.get_parameter("freq_localization").value, + self.localisation_timer_callback, + callback_group=self.callback_group) + + def pose_to_mat(self, pose): + trans = np.eye(4) + trans[:3, 3] = [pose.position.x, pose.position.y, pose.position.z] + quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] + trans[:3, :3] = tf_transformations.quaternion_matrix(quat)[:3, :3] + return trans + + def msg_to_array(self, pc_msg): + pc_array = ros2_numpy.numpify(pc_msg) + return pc_array["xyz"] + + def registration_at_scale(self, scan, map_pcd, initial, scale): + result_icp = o3d.pipelines.registration.registration_icp( + self.voxel_down_sample(scan, self.get_parameter("scan_voxel_size").value * scale), + self.voxel_down_sample(map_pcd, self.get_parameter("map_voxel_size").value * scale), + 1.0 * scale, + initial, + o3d.pipelines.registration.TransformationEstimationPointToPoint(), + o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=20), + ) + return result_icp.transformation, result_icp.fitness + + def inverse_se3(self, trans): + trans_inverse = np.eye(4) + trans_inverse[:3, :3] = trans[:3, :3].T + trans_inverse[:3, 3] = -np.matmul(trans[:3, :3].T, trans[:3, 3]) + return trans_inverse + + def publish_point_cloud(self, publisher, header, pc): + data = dict() + data["xyz"] = pc[:, :3].astype(np.float32) + + if pc.shape[1] >= 4: + data["intensity"] = pc[:, 3].astype(np.float32) + + msg = ros2_numpy.msgify(PointCloud2, data) + msg.header = header + + # Fix ros2_numpy bug: point_step not set when only xyz provided + if msg.point_step == 0: + msg.point_step = 12 # 3 floats x 4 bytes + msg.row_step = msg.point_step * msg.width + + publisher.publish(msg) + + def crop_global_map_in_FOV(self, pose_estimation): + with self.localization_lock: + if self.cur_odom is None: + return None + T_odom_to_base_link = self.pose_to_mat(self.cur_odom.pose.pose) + header = self.cur_odom.header + + T_map_to_base_link = np.matmul(pose_estimation, T_odom_to_base_link) + + robot_pos_in_map = T_map_to_base_link[:3, 3] + global_map_points = np.asarray(self.global_map.points) + + fov_far = self.get_parameter("fov_far").value + distances_sq = np.sum((global_map_points - robot_pos_in_map)**2, axis=1) + close_indices = np.where(distances_sq < fov_far**2)[0] + + if len(close_indices) == 0: + return o3d.geometry.PointCloud() + + close_map_points = global_map_points[close_indices] + + T_base_link_to_map = self.inverse_se3(T_map_to_base_link) + close_map_points_h = np.column_stack([close_map_points, np.ones(len(close_map_points))]) + points_in_base_link = np.matmul(T_base_link_to_map, close_map_points_h.T).T + + fov_rad = self.get_parameter("fov").value + if fov_rad > 3.14: + indices = np.where( + (points_in_base_link[:, 0] < fov_far) + & (np.abs(np.arctan2(points_in_base_link[:, 1], points_in_base_link[:, 0])) < fov_rad / 2.0) + ) + else: + indices = np.where( + (points_in_base_link[:, 0] > 0) + & (points_in_base_link[:, 0] < fov_far) + & (np.abs(np.arctan2(points_in_base_link[:, 1], points_in_base_link[:, 0])) < fov_rad / 2.0) + ) + + final_points_in_map = close_map_points[indices] + + global_map_in_FOV = o3d.geometry.PointCloud() + global_map_in_FOV.points = o3d.utility.Vector3dVector(final_points_in_map) + + header.frame_id = "map" + self.publish_point_cloud(self.pub_submap, header, final_points_in_map[::10]) + + return global_map_in_FOV + + def global_localization(self, pose_estimation): + with self.localization_lock: + if self.cur_scan is None: + return + scan_tobe_mapped = copy.copy(self.cur_scan) + + global_map_in_FOV = self.crop_global_map_in_FOV(pose_estimation) + if global_map_in_FOV is None: + return + + transformation, fitness = self.registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=pose_estimation, scale=1) + + if fitness > self.get_parameter("localization_threshold").value: + with self.localization_lock: + self.T_map_to_odom = transformation + self.publish_odom(transformation) + self.get_logger().info(f"Localization updated (fitness: {fitness:.3f})", throttle_duration_sec=5.0) + else: + self.get_logger().warn(f"Fitness score {fitness:.3f} below threshold {self.get_parameter('localization_threshold').value}") + + def voxel_down_sample(self, pcd, voxel_size): + try: + pcd_down = pcd.voxel_down_sample(voxel_size) + except Exception: + pcd_down = o3d.geometry.voxel_down_sample(pcd, voxel_size) + return pcd_down + + def cb_save_cur_odom(self, msg): + with self.localization_lock: + self.cur_odom = msg + + def cb_save_cur_scan(self, msg): + pc = self.msg_to_array(msg) + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(pc) + with self.localization_lock: + self.cur_scan = pcd + self.publish_point_cloud(self.pub_pc_in_map, msg.header, pc) + + def initialize_global_map(self): + pcd_path = self.get_parameter("pcd_map_path").value + if not pcd_path: + self.get_logger().error("pcd_map_path parameter is empty!") + raise RuntimeError("pcd_map_path not set") + self.global_map = o3d.io.read_point_cloud(pcd_path) + self.global_map = self.voxel_down_sample(self.global_map, self.get_parameter("map_voxel_size").value) + self.get_logger().info(f"Global map loaded: {len(self.global_map.points)} points from {pcd_path}") + + def cb_initialize_pose(self, msg): + initial_pose = self.pose_to_mat(msg.pose.pose) + with self.localization_lock: + self.initialized = True + cur_scan_exists = self.cur_scan is not None + + self.get_logger().info("Initial pose received.") + + if cur_scan_exists: + self.global_localization(initial_pose) + + def publish_odom(self, transform): + odom_msg = Odometry() + xyz = transform[:3, 3] + quat = tf_transformations.quaternion_from_matrix(transform) + odom_msg.pose.pose = Pose( + position=Point(x=xyz[0], y=xyz[1], z=xyz[2]), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) + ) + odom_msg.header.stamp = self.get_clock().now().to_msg() + odom_msg.header.frame_id = "map" + self.pub_map_to_odom.publish(odom_msg) + + def localisation_timer_callback(self): + with self.localization_lock: + if not self.initialized or self.cur_scan is None: + self.get_logger().info("Waiting for initial pose and current scan...", throttle_duration_sec=5.0) + return + current_transform = copy.copy(self.T_map_to_odom) + + self.global_localization(current_transform) + + +def main(args=None): + rclpy.init(args=args) + node = GlobalLocalization() + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp b/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp index f715b34e..d84117f0 100644 --- a/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp +++ b/src/slam/arise_slam_mid360/src/ImuPreintegration/imuPreintegration.cpp @@ -71,7 +71,19 @@ namespace arise_slam { tfMap2Odom = std::make_shared(this); tfOdom2BaseLink = std::make_shared(this); - + + // Initialize map_to_odom as identity (no relocalization correction) + map_to_odom_.setIdentity(); + + // Subscribe to relocalization corrections from global_localization node + rclcpp::QoS map_to_odom_qos(1); + map_to_odom_qos.best_effort(); + map_to_odom_qos.keep_last(1); + subMapToOdom = this->create_subscription( + "/map_to_odom", map_to_odom_qos, + std::bind(&imuPreintegration::mapToOdomHandler, this, + std::placeholders::_1), sub_options); + // set relevant parameter std::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU(config_.imuGravity); @@ -1037,7 +1049,7 @@ namespace arise_slam { nav_msgs::msg::Odometry odometry2; odometry2.header.stamp = thisImu.header.stamp; - odometry2.header.frame_id = WORLD_FRAME; + odometry2.header.frame_id = ODOM_FRAME; odometry2.child_frame_id = SENSOR_FRAME; Eigen::Quaterniond q_w_lidar(lidarPoseOpt.rotation().toQuaternion().w(), lidarPoseOpt.rotation().toQuaternion().x(), @@ -1079,26 +1091,38 @@ namespace arise_slam { health_status_msg.data = health_status; pubHealthStatus->publish(health_status_msg); - tf2_ros::TransformBroadcaster br(this); - // tf2::Transform transform; - geometry_msgs::msg::TransformStamped transform_stamped_; - tf2::Transform transform; - transform_stamped_.header.stamp = thisImu.header.stamp; - transform_stamped_.header.frame_id = WORLD_FRAME; - transform_stamped_.child_frame_id = SENSOR_FRAME; - + // Publish odom -> sensor TF + geometry_msgs::msg::TransformStamped odom_to_sensor; + tf2::Transform tf_odom_sensor; + odom_to_sensor.header.stamp = thisImu.header.stamp; + odom_to_sensor.header.frame_id = ODOM_FRAME; + odom_to_sensor.child_frame_id = SENSOR_FRAME; + tf2::Quaternion q; - transform.setOrigin(tf2::Vector3(odometry2.pose.pose.position.x, + tf_odom_sensor.setOrigin(tf2::Vector3(odometry2.pose.pose.position.x, odometry2.pose.pose.position.y, odometry2.pose.pose.position.z)); q.setW(q_w_lidar.w()); q.setX(q_w_lidar.x()); q.setY(q_w_lidar.y()); q.setZ(q_w_lidar.z()); - transform.setRotation(q); - transform_stamped_.transform = tf2::toMsg(transform); - if(frame_count%4==0) - br.sendTransform(transform_stamped_); + tf_odom_sensor.setRotation(q); + odom_to_sensor.transform = tf2::toMsg(tf_odom_sensor); + + // Publish map -> odom TF (identity unless updated by relocalization) + geometry_msgs::msg::TransformStamped map_to_odom_tf; + map_to_odom_tf.header.stamp = thisImu.header.stamp; + map_to_odom_tf.header.frame_id = WORLD_FRAME; + map_to_odom_tf.child_frame_id = ODOM_FRAME; + { + std::lock_guard lock(map_to_odom_mutex_); + map_to_odom_tf.transform = tf2::toMsg(map_to_odom_); + } + + if(frame_count%4==0) { + tfOdom2BaseLink->sendTransform(odom_to_sensor); + tfMap2Odom->sendTransform(map_to_odom_tf); + } // publish IMU path static nav_msgs::msg::Path imuPath; @@ -1129,4 +1153,20 @@ namespace arise_slam { } + void imuPreintegration::mapToOdomHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg) { + std::lock_guard lock(map_to_odom_mutex_); + tf2::Quaternion q( + odomMsg->pose.pose.orientation.x, + odomMsg->pose.pose.orientation.y, + odomMsg->pose.pose.orientation.z, + odomMsg->pose.pose.orientation.w); + tf2::Vector3 t( + odomMsg->pose.pose.position.x, + odomMsg->pose.pose.position.y, + odomMsg->pose.pose.position.z); + map_to_odom_.setRotation(q); + map_to_odom_.setOrigin(t); + RCLCPP_INFO_ONCE(this->get_logger(), "Received map_to_odom relocalization correction"); + } + } // end namespace arise_slam diff --git a/src/slam/arise_slam_mid360/src/parameter/parameter.cpp b/src/slam/arise_slam_mid360/src/parameter/parameter.cpp index e2b5763c..65104127 100755 --- a/src/slam/arise_slam_mid360/src/parameter/parameter.cpp +++ b/src/slam/arise_slam_mid360/src/parameter/parameter.cpp @@ -27,6 +27,7 @@ std::string ProjectName; std::string WORLD_FRAME; std::string WORLD_FRAME_ROT; +std::string ODOM_FRAME; std::string SENSOR_FRAME; std::string SENSOR_FRAME_ROT; SensorType sensor; @@ -299,6 +300,7 @@ bool readGlobalparam(rclcpp::Node::SharedPtr node) node->declare_parameter("depthdown_topic","/rs_down/depth/cloud_filtered"); node->declare_parameter("world_frame", "sensor_init"); node->declare_parameter("world_frame_rot", "sensor_init_rot"); + node->declare_parameter("odom_frame", "odom"); node->declare_parameter("sensor_frame", "sensor"); node->declare_parameter("sensor_frame_rot", "sensor_rot"); node->declare_parameter("PROJECT_NAME", "arise_slam"); @@ -310,6 +312,7 @@ bool readGlobalparam(rclcpp::Node::SharedPtr node) DepthDown_TOPIC = node->get_parameter("depthdown_topic").as_string(); WORLD_FRAME = node->get_parameter("world_frame").as_string(); WORLD_FRAME_ROT = node->get_parameter("world_frame_rot").as_string(); + ODOM_FRAME = node->get_parameter("odom_frame").as_string(); SENSOR_FRAME = node->get_parameter("sensor_frame").as_string(); SENSOR_FRAME_ROT = node->get_parameter("sensor_frame_rot").as_string(); ProjectName = node->get_parameter("PROJECT_NAME").as_string(); @@ -321,6 +324,7 @@ bool readGlobalparam(rclcpp::Node::SharedPtr node) RCLCPP_DEBUG(node->get_logger(), "DepthDown_TOPIC %s", DepthDown_TOPIC.c_str()); RCLCPP_DEBUG(node->get_logger(), "WORLD_FRAME %s", WORLD_FRAME.c_str()); RCLCPP_DEBUG(node->get_logger(), "WORLD_FRAME_ROT %s", WORLD_FRAME_ROT.c_str()); + RCLCPP_DEBUG(node->get_logger(), "ODOM_FRAME %s", ODOM_FRAME.c_str()); RCLCPP_DEBUG(node->get_logger(), "SENSOR_FRAME %s", SENSOR_FRAME.c_str()); RCLCPP_DEBUG(node->get_logger(), "SENSOR_FRAME_ROT %s", SENSOR_FRAME_ROT.c_str()); RCLCPP_DEBUG(node->get_logger(), "ProjectName %s", ProjectName.c_str()); From 0f8171084686a089c0aed5bb9d355fc42bd98617 Mon Sep 17 00:00:00 2001 From: Bona Date: Fri, 3 Apr 2026 11:36:32 -0700 Subject: [PATCH 2/2] Move global_localization to standalone package Addresses PR feedback: relocalization is SLAM-agnostic so it belongs in its own package. Moved global_localization.py and relocalization.launch.py into src/slam/global_localization/ as a proper ament_python package. Also default pcd_map_path from MAP_PATH env variable in the relocalization launch. --- src/slam/arise_slam_mid360/CMakeLists.txt | 5 ---- .../launch/arize_slam.launch.py | 4 +-- .../README.md} | 8 +++--- .../global_localization/__init__.py | 0 .../global_localization.py | 0 .../launch/relocalization.launch.py | 10 ++++--- src/slam/global_localization/package.xml | 27 +++++++++++++++++++ .../resource/global_localization | 0 src/slam/global_localization/setup.cfg | 4 +++ src/slam/global_localization/setup.py | 23 ++++++++++++++++ 10 files changed, 66 insertions(+), 15 deletions(-) rename src/slam/{arise_slam_mid360/RELOCALIZATION.md => global_localization/README.md} (74%) create mode 100644 src/slam/global_localization/global_localization/__init__.py rename src/slam/{arise_slam_mid360/scripts => global_localization/global_localization}/global_localization.py (100%) rename src/slam/{arise_slam_mid360 => global_localization}/launch/relocalization.launch.py (91%) create mode 100644 src/slam/global_localization/package.xml create mode 100644 src/slam/global_localization/resource/global_localization create mode 100644 src/slam/global_localization/setup.cfg create mode 100644 src/slam/global_localization/setup.py diff --git a/src/slam/arise_slam_mid360/CMakeLists.txt b/src/slam/arise_slam_mid360/CMakeLists.txt index 639b7813..d4a45d29 100755 --- a/src/slam/arise_slam_mid360/CMakeLists.txt +++ b/src/slam/arise_slam_mid360/CMakeLists.txt @@ -182,9 +182,4 @@ install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}/ ) -install(PROGRAMS - scripts/global_localization.py - DESTINATION lib/${PROJECT_NAME} -) - ament_package() diff --git a/src/slam/arise_slam_mid360/launch/arize_slam.launch.py b/src/slam/arise_slam_mid360/launch/arize_slam.launch.py index 29d3202b..ae2458ff 100644 --- a/src/slam/arise_slam_mid360/launch/arize_slam.launch.py +++ b/src/slam/arise_slam_mid360/launch/arize_slam.launch.py @@ -146,8 +146,8 @@ 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", + package="global_localization", + executable="global_localization", name="global_localization", output="screen", condition=IfCondition(LaunchConfiguration("local_mode")), diff --git a/src/slam/arise_slam_mid360/RELOCALIZATION.md b/src/slam/global_localization/README.md similarity index 74% rename from src/slam/arise_slam_mid360/RELOCALIZATION.md rename to src/slam/global_localization/README.md index 22349495..75baec1d 100644 --- a/src/slam/arise_slam_mid360/RELOCALIZATION.md +++ b/src/slam/global_localization/README.md @@ -1,6 +1,6 @@ -# Relocalization +# global_localization -ICP-based relocalization against a prior PCD map. Publishes a `map -> odom` TF correction so the full TF tree is: +SLAM-agnostic ICP-based relocalization against a prior PCD map. Publishes a `map -> odom` correction on `/map_to_odom` so any SLAM backend that subscribes can broadcast the full TF tree: ``` map -> odom -> sensor @@ -10,7 +10,7 @@ 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. +The `global_localization` node loads a prior PCD map, subscribes to any SLAM's registered cloud and odometry topics (configurable), and periodically runs ICP to align the current scan against the map. The resulting correction is published on `/map_to_odom`. The SLAM backend (e.g. arise_slam's `imu_preintegration_node`) subscribes and broadcasts it 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. @@ -38,7 +38,7 @@ Launch SLAM and relocalization separately: 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 +ros2 launch global_localization relocalization.launch.py pcd_map_path:=/path/to/map.pcd ``` ### Parameters diff --git a/src/slam/global_localization/global_localization/__init__.py b/src/slam/global_localization/global_localization/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/slam/arise_slam_mid360/scripts/global_localization.py b/src/slam/global_localization/global_localization/global_localization.py similarity index 100% rename from src/slam/arise_slam_mid360/scripts/global_localization.py rename to src/slam/global_localization/global_localization/global_localization.py diff --git a/src/slam/arise_slam_mid360/launch/relocalization.launch.py b/src/slam/global_localization/launch/relocalization.launch.py similarity index 91% rename from src/slam/arise_slam_mid360/launch/relocalization.launch.py rename to src/slam/global_localization/launch/relocalization.launch.py index db116112..6c5a043d 100644 --- a/src/slam/arise_slam_mid360/launch/relocalization.launch.py +++ b/src/slam/global_localization/launch/relocalization.launch.py @@ -7,10 +7,12 @@ def generate_launch_description(): + map_path_env = os.environ.get('MAP_PATH', '') + pcd_map_path_arg = DeclareLaunchArgument( "pcd_map_path", - default_value="", - description="Path to the PCD map file for relocalization", + default_value=map_path_env + '.pcd' if map_path_env else '', + description="Path to the PCD map file for relocalization. Auto-set from MAP_PATH env variable.", ) cloud_topic_arg = DeclareLaunchArgument( "cloud_topic", @@ -50,8 +52,8 @@ def generate_launch_description(): ) global_localization_node = Node( - package="arise_slam_mid360", - executable="global_localization.py", + package="global_localization", + executable="global_localization", name="global_localization", output="screen", parameters=[{ diff --git a/src/slam/global_localization/package.xml b/src/slam/global_localization/package.xml new file mode 100644 index 00000000..b8434ff9 --- /dev/null +++ b/src/slam/global_localization/package.xml @@ -0,0 +1,27 @@ + + + + global_localization + 0.1.0 + + ICP-based relocalization against a prior PCD map. + SLAM-agnostic: works with any backend that publishes a registered + point cloud and odometry. + + + iServe Robotics + GPLv3 + + ament_python + + rclpy + nav_msgs + sensor_msgs + geometry_msgs + std_msgs + tf2_ros + + + ament_python + + diff --git a/src/slam/global_localization/resource/global_localization b/src/slam/global_localization/resource/global_localization new file mode 100644 index 00000000..e69de29b diff --git a/src/slam/global_localization/setup.cfg b/src/slam/global_localization/setup.cfg new file mode 100644 index 00000000..d976e1f5 --- /dev/null +++ b/src/slam/global_localization/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/global_localization +[install] +install_scripts=$base/lib/global_localization diff --git a/src/slam/global_localization/setup.py b/src/slam/global_localization/setup.py new file mode 100644 index 00000000..bbdf348f --- /dev/null +++ b/src/slam/global_localization/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'global_localization' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + entry_points={ + 'console_scripts': [ + 'global_localization = global_localization.global_localization:main', + ], + }, +)