From e88467540523600350da22cd5c305eb12b9abf40 Mon Sep 17 00:00:00 2001 From: FieldDiTian Date: Wed, 1 Jul 2026 21:20:34 -0700 Subject: [PATCH] Add GICP RViz trajectory overlays --- gicp_localization/README.md | 16 ++- .../include/gicp_localization/localization.h | 11 ++ gicp_localization/launch/localization.rviz | 41 ++++++ .../launch/localization_with_tf.launch.py | 12 ++ gicp_localization/src/localization.cc | 134 +++++++++++++++++- 5 files changed, 210 insertions(+), 4 deletions(-) diff --git a/gicp_localization/README.md b/gicp_localization/README.md index d0b1f421..5be37bc5 100644 --- a/gicp_localization/README.md +++ b/gicp_localization/README.md @@ -279,12 +279,26 @@ correct negative offset. |---|---|---| | `localized_pose` (`gicp/localization/pose`) | `geometry_msgs/PoseStamped` | Localized pose (scan rate). | | `localized_odom` (`gicp/localization/odom`) | `nav_msgs/Odometry` | Localized odom propagated at IMU rate (~100 Hz). | -| `localized_path` (`gicp/localization/path`) | `nav_msgs/Path` | Trajectory history. | +| `localized_path` (`gicp/localization/path`) | `nav_msgs/Path` | Final adopted localization trajectory history. | +| `gt_ins` (`/gt_ins`) | `nav_msgs/Path` | RViz/debug GT/INS reference path, sampled at the same LiDAR scan stamps as `localized_path`. | +| `gicp/localization/gicp_only_path` | `nav_msgs/Path` | RViz/debug current accepted-GICP-only segment; clears when the node falls back to IMU/GT recovery. | +| `gicp/localization/gicp_only_segments` | `visualization_msgs/MarkerArray` | RViz/debug accepted-GICP-only history as broken orange line segments, preserving gaps across fallback/recovery. | | `gicp/localization/pose_utm` / `odom_utm` / `path_utm` | (same types) | Optional legacy UTM-frame mirrors, only when `utm_transform_path` is set (the primary `map`-frame outputs above are already local ENU with the adapter). | | `aligned_cloud` (`gicp/localization/aligned_cloud`) | `sensor_msgs/PointCloud2` | Aligned scan in `map`. | | `map` (`gicp/localization/map`) | `sensor_msgs/PointCloud2` | Downsampled visualization map. | | TF: `map → base_frame` | | Published when `publish_tf=true`. | +### RViz path colors + +The default `launch/localization.rviz` config uses fixed colors for the path +overlays: + +| Color | Display | Topic | Meaning | +|---|---|---|---| +| Green | Trajectory Path | `/gicp/localization/path` | Final adopted localization trajectory, including accepted GICP, IMU dead-reckoning fallback, and GT recovery snaps. | +| Orange | GICP-only Segments | `/gicp/localization/gicp_only_segments` | Accepted-GICP-only trajectory segments. The line breaks while the node is on IMU fallback or GT recovery. | +| Blue | GT INS Reference | `/gt_ins` | GT/INS reference path sampled at the same LiDAR scan timestamps as the final path. | + ### Debug topics (require `localization/debug/enable_pub: true`) Per-scan scalar metrics on `gicp/localization/debug/*`: diff --git a/gicp_localization/include/gicp_localization/localization.h b/gicp_localization/include/gicp_localization/localization.h index fcc251b1..309d27bb 100644 --- a/gicp_localization/include/gicp_localization/localization.h +++ b/gicp_localization/include/gicp_localization/localization.h @@ -246,6 +246,9 @@ class LocalizationNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr pose_pub; rclcpp::Publisher::SharedPtr localized_odom_pub; rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr gt_ins_path_pub; + rclcpp::Publisher::SharedPtr gicp_only_path_pub; + rclcpp::Publisher::SharedPtr gicp_only_segments_pub; rclcpp::Publisher::SharedPtr utm_pose_pub; rclcpp::Publisher::SharedPtr utm_odom_pub; rclcpp::Publisher::SharedPtr utm_path_pub; @@ -310,6 +313,7 @@ class LocalizationNode : public rclcpp::Node { Eigen::Matrix4f last_gicp_pose_; rclcpp::Time last_gicp_stamp_; bool last_gicp_valid_; + bool last_scan_gicp_accepted_{false}; double last_fitness_score_{-1.0}; // -1 = no scan yet double last_accepted_scan_stamp_{-1.0}; // s — stamp of last accepted GICP scan (P3 dead-reckon cov) @@ -318,6 +322,13 @@ class LocalizationNode : public rclcpp::Node { // subscribers, so we don't pay an O(N) DDS serialize on every scan. nav_msgs::msg::Path path_msg; std::deque path_buffer_; + nav_msgs::msg::Path gt_ins_path_msg_; + std::deque gt_ins_path_buffer_; + nav_msgs::msg::Path gicp_only_path_msg_; + std::deque gicp_only_path_buffer_; + visualization_msgs::msg::MarkerArray gicp_only_segments_msg_; + bool gicp_only_segment_active_{false}; + int gicp_only_segment_next_id_{0}; // IMU data structures boost::circular_buffer imu_buffer; diff --git a/gicp_localization/launch/localization.rviz b/gicp_localization/launch/localization.rviz index 3ed63f08..c463bab8 100644 --- a/gicp_localization/launch/localization.rviz +++ b/gicp_localization/launch/localization.rviz @@ -251,6 +251,47 @@ Visualization Manager: Value: /gicp/localization/path Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 170; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: GT INS Reference + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 0; 170; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /gt_ins + Value: true + + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: GICP-only Segments + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /gicp/localization/gicp_only_segments + Value: true + - Alpha: 1 Autocompute Value Bounds: Max Value: 10 diff --git a/gicp_localization/launch/localization_with_tf.launch.py b/gicp_localization/launch/localization_with_tf.launch.py index 100f681c..e9fa2538 100644 --- a/gicp_localization/launch/localization_with_tf.launch.py +++ b/gicp_localization/launch/localization_with_tf.launch.py @@ -44,6 +44,8 @@ def generate_launch_description(): odom_topic = LaunchConfiguration('odom_topic', default='/odom') gt_odom_topic = LaunchConfiguration('gt_odom_topic', default='/gps_p1/filtered_odom') imu_only = LaunchConfiguration('imu_only', default='false') + debug_pub = LaunchConfiguration('debug_pub', default='false') + verbose_scan_log = LaunchConfiguration('verbose_scan_log', default='false') urdf_path = LaunchConfiguration( 'urdf_path', default='') @@ -75,6 +77,12 @@ def generate_launch_description(): declare_imu_only_arg = DeclareLaunchArgument( 'imu_only', default_value=imu_only, description='If true, disable GICP and run IMU-only propagation') + declare_debug_pub_arg = DeclareLaunchArgument( + 'debug_pub', default_value=debug_pub, + description='If true, publish debug topics including localized_path and RViz comparison paths') + declare_verbose_scan_log_arg = DeclareLaunchArgument( + 'verbose_scan_log', default_value=verbose_scan_log, + description='If true, log per-scan GICP accept/reject diagnostics') declare_urdf_path_arg = DeclareLaunchArgument( 'urdf_path', default_value=urdf_path, description='Absolute path to the vehicle URDF used by robot_state_publisher ' @@ -140,6 +148,8 @@ def make_localization_node(context): localization_yaml_path, {'localization/lidar_frame': child_frame_value}, {'localization/imu_only': LaunchConfiguration('imu_only')}, + {'localization/debug/enable_pub': LaunchConfiguration('debug_pub')}, + {'localization/debug/verbose_scan_log': LaunchConfiguration('verbose_scan_log')}, {'localization/lidar_concat/urdf_path': urdf_file}, ] if map_path_value: @@ -200,6 +210,8 @@ def make_rviz_node(context): declare_odom_topic_arg, declare_gt_odom_topic_arg, declare_imu_only_arg, + declare_debug_pub_arg, + declare_verbose_scan_log_arg, declare_urdf_path_arg, declare_parent_frame_arg, declare_child_frame_arg, diff --git a/gicp_localization/src/localization.cc b/gicp_localization/src/localization.cc index 9d769c3b..56946d90 100644 --- a/gicp_localization/src/localization.cc +++ b/gicp_localization/src/localization.cc @@ -984,6 +984,11 @@ gicp_localization::LocalizationNode::LocalizationNode() : Node("gicp_localizatio if (this->debug_pub_enabled_) { this->path_pub = this->create_publisher("localized_path", 10); + this->gt_ins_path_pub = this->create_publisher("/gt_ins", 10); + this->gicp_only_path_pub = + this->create_publisher("gicp/localization/gicp_only_path", 10); + this->gicp_only_segments_pub = + this->create_publisher("gicp/localization/gicp_only_segments", 10); this->gt_snap_pub = this->create_publisher("gicp/localization/gt_snap", 10); this->dbg_initial_guess_pose_pub = this->create_publisher("gicp/localization/debug/initial_guess_pose", 10); @@ -1725,6 +1730,19 @@ void gicp_localization::LocalizationNode::applyInitialPoseFromParams() { this->path_msg.poses.clear(); this->path_msg.header.frame_id = this->map_frame; this->path_msg.header.stamp = this->now(); + this->path_buffer_.clear(); + this->gt_ins_path_msg_.poses.clear(); + this->gt_ins_path_msg_.header.frame_id = this->map_frame; + this->gt_ins_path_msg_.header.stamp = this->path_msg.header.stamp; + this->gt_ins_path_buffer_.clear(); + this->gicp_only_path_msg_.poses.clear(); + this->gicp_only_path_msg_.header.frame_id = this->map_frame; + this->gicp_only_path_msg_.header.stamp = this->path_msg.header.stamp; + this->gicp_only_path_buffer_.clear(); + this->gicp_only_segments_msg_.markers.clear(); + this->gicp_only_segment_active_ = false; + this->gicp_only_segment_next_id_ = 0; + this->last_scan_gicp_accepted_ = false; RCLCPP_INFO(this->get_logger(), "Initial pose loaded from parameters at [%.2f, %.2f, %.2f] m with RPY [%.2f, %.2f, %.2f] rad", @@ -1780,6 +1798,19 @@ void gicp_localization::LocalizationNode::applyInitialPose(const Eigen::Vector3f this->path_msg.poses.clear(); this->path_msg.header.frame_id = this->map_frame; this->path_msg.header.stamp = stamp.nanoseconds() > 0 ? stamp : this->now(); + this->path_buffer_.clear(); + this->gt_ins_path_msg_.poses.clear(); + this->gt_ins_path_msg_.header.frame_id = this->map_frame; + this->gt_ins_path_msg_.header.stamp = this->path_msg.header.stamp; + this->gt_ins_path_buffer_.clear(); + this->gicp_only_path_msg_.poses.clear(); + this->gicp_only_path_msg_.header.frame_id = this->map_frame; + this->gicp_only_path_msg_.header.stamp = this->path_msg.header.stamp; + this->gicp_only_path_buffer_.clear(); + this->gicp_only_segments_msg_.markers.clear(); + this->gicp_only_segment_active_ = false; + this->gicp_only_segment_next_id_ = 0; + this->last_scan_gicp_accepted_ = false; RCLCPP_INFO(this->get_logger(), "Received initial pose (%s) at [%.2f, %.2f, %.2f]", source.c_str(), p.x(), p.y(), p.z()); @@ -2802,6 +2833,7 @@ void gicp_localization::LocalizationNode::performLocalization() { RCLCPP_DEBUG(this->get_logger(), "performLocalization: Acquiring mutex lock..."); std::lock_guard lock(this->pose_mutex); RCLCPP_DEBUG(this->get_logger(), "performLocalization: Mutex acquired"); + this->last_scan_gicp_accepted_ = false; // Set source cloud RCLCPP_DEBUG(this->get_logger(), "performLocalization: Setting input source (%lu points)...", @@ -2820,9 +2852,10 @@ void gicp_localization::LocalizationNode::performLocalization() { // in map<-lidar, then convert the optimizer output back to map<-base. const Eigen::Matrix4f T_base_lidar = this->extrinsics.baselink2lidar_T; const Eigen::Matrix4f T_lidar_base = T_base_lidar.inverse(); - Eigen::Matrix4f initial_guess = this->deskew_ - ? Eigen::Matrix4f::Identity() - : (this->T_prior * T_base_lidar); + Eigen::Matrix4f initial_guess = Eigen::Matrix4f::Identity(); + if (!this->deskew_) { + initial_guess = this->T_prior * T_base_lidar; + } Eigen::Matrix4f guess_pose_map = this->T_prior; double guess_from_last_trans = 0.0; @@ -3152,6 +3185,7 @@ void gicp_localization::LocalizationNode::performLocalization() { if (gicp_accepted) { this->current_pose = candidate_pose; + this->last_scan_gicp_accepted_ = true; // Update lidar pose for next iteration Eigen::Vector3f new_p = this->current_pose.block<3, 1>(0, 3); @@ -3314,6 +3348,100 @@ void gicp_localization::LocalizationNode::publishPose() { this->path_pub->publish(this->path_msg); } + // Publish a scan-time-aligned INS/GT reference path. Each point is sampled at + // the same scan_stamp used by localized_path, so RViz playback compares both + // paths on the same timeline. + if (this->gt_ins_path_pub && this->gt_odom_enabled_ && this->gt_odom_received_.load()) { + GtSample gt; + if (this->getGtPoseAt(this->scan_stamp.seconds(), gt)) { + Eigen::Vector3f gt_position; + Eigen::Quaternionf gt_orientation; + if (!this->composeGtPoseInBase(gt, gt_position, gt_orientation)) { + gt_position = gt.p; + gt_orientation = gt.q; + } + gt_orientation.normalize(); + + geometry_msgs::msg::PoseStamped gt_pose_msg; + gt_pose_msg.header.stamp = this->scan_stamp; + gt_pose_msg.header.frame_id = this->map_frame; + gt_pose_msg.pose.position.x = gt_position.x(); + gt_pose_msg.pose.position.y = gt_position.y(); + gt_pose_msg.pose.position.z = gt_position.z(); + gt_pose_msg.pose.orientation.w = gt_orientation.w(); + gt_pose_msg.pose.orientation.x = gt_orientation.x(); + gt_pose_msg.pose.orientation.y = gt_orientation.y(); + gt_pose_msg.pose.orientation.z = gt_orientation.z(); + + if (this->gt_ins_path_buffer_.size() >= 10000) this->gt_ins_path_buffer_.pop_front(); + this->gt_ins_path_buffer_.push_back(gt_pose_msg); + if (this->gt_ins_path_pub->get_subscription_count() > 0) { + this->gt_ins_path_msg_.header.stamp = this->scan_stamp; + this->gt_ins_path_msg_.header.frame_id = this->map_frame; + this->gt_ins_path_msg_.poses.assign(this->gt_ins_path_buffer_.begin(), this->gt_ins_path_buffer_.end()); + this->gt_ins_path_pub->publish(this->gt_ins_path_msg_); + } + } + } + + auto publish_gicp_only_path = [&]() { + if (this->gicp_only_path_pub && this->gicp_only_path_pub->get_subscription_count() > 0) { + this->gicp_only_path_msg_.header.stamp = this->scan_stamp; + this->gicp_only_path_msg_.header.frame_id = this->map_frame; + this->gicp_only_path_msg_.poses.assign( + this->gicp_only_path_buffer_.begin(), this->gicp_only_path_buffer_.end()); + this->gicp_only_path_pub->publish(this->gicp_only_path_msg_); + } + }; + + auto publish_gicp_only_segments = [&]() { + if (this->gicp_only_segments_pub && this->gicp_only_segments_pub->get_subscription_count() > 0) { + for (auto& marker : this->gicp_only_segments_msg_.markers) { + marker.header.stamp = this->scan_stamp; + } + this->gicp_only_segments_pub->publish(this->gicp_only_segments_msg_); + } + }; + + if (this->last_scan_gicp_accepted_) { + if (!this->gicp_only_segment_active_) { + this->gicp_only_path_buffer_.clear(); + + visualization_msgs::msg::Marker segment; + segment.header.stamp = this->scan_stamp; + segment.header.frame_id = this->map_frame; + segment.ns = "gicp_only_segments"; + segment.id = this->gicp_only_segment_next_id_++; + segment.type = visualization_msgs::msg::Marker::LINE_STRIP; + segment.action = visualization_msgs::msg::Marker::ADD; + segment.pose.orientation.w = 1.0; + segment.scale.x = 0.18; + segment.color.a = 1.0f; + segment.color.r = 1.0f; + segment.color.g = 0.55f; + segment.color.b = 0.0f; + this->gicp_only_segments_msg_.markers.push_back(segment); + this->gicp_only_segment_active_ = true; + } + + if (this->gicp_only_path_buffer_.size() >= 10000) this->gicp_only_path_buffer_.pop_front(); + this->gicp_only_path_buffer_.push_back(pose_msg); + publish_gicp_only_path(); + + if (!this->gicp_only_segments_msg_.markers.empty()) { + geometry_msgs::msg::Point point; + point.x = pose_msg.pose.position.x; + point.y = pose_msg.pose.position.y; + point.z = pose_msg.pose.position.z; + this->gicp_only_segments_msg_.markers.back().points.push_back(point); + } + publish_gicp_only_segments(); + } else if (this->gicp_only_segment_active_ || !this->gicp_only_path_buffer_.empty()) { + this->gicp_only_segment_active_ = false; + this->gicp_only_path_buffer_.clear(); + publish_gicp_only_path(); + } + // Publish UTM-frame pose/path if (this->utm_enabled_) { Eigen::Matrix4f T_utm_base = this->T_utm_map_ * this->current_pose;