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..ae2458ff 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="global_localization", + executable="global_localization", + 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/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()); diff --git a/src/slam/global_localization/README.md b/src/slam/global_localization/README.md new file mode 100644 index 00000000..75baec1d --- /dev/null +++ b/src/slam/global_localization/README.md @@ -0,0 +1,60 @@ +# global_localization + +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 +``` + +In mapping mode (no prior map), `map -> odom` is identity. + +## How it works + +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. + +## 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 global_localization 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/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/global_localization/global_localization/global_localization.py b/src/slam/global_localization/global_localization/global_localization.py new file mode 100755 index 00000000..8071c012 --- /dev/null +++ b/src/slam/global_localization/global_localization/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/global_localization/launch/relocalization.launch.py b/src/slam/global_localization/launch/relocalization.launch.py new file mode 100644 index 00000000..6c5a043d --- /dev/null +++ b/src/slam/global_localization/launch/relocalization.launch.py @@ -0,0 +1,83 @@ +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(): + map_path_env = os.environ.get('MAP_PATH', '') + + pcd_map_path_arg = DeclareLaunchArgument( + "pcd_map_path", + 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", + 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="global_localization", + executable="global_localization", + 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/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', + ], + }, +)