[codex] Add GICP RViz trajectory overlays#5
Open
FieldDiTian wants to merge 1 commit into
Open
Conversation
There was a problem hiding this comment.
Pull request overview
Adds RViz/debug trajectory overlays to distinguish “final adopted localization” from “accepted-by-GICP-only” motion, and to provide a scan-time-aligned GT/INS reference path for visual comparison during playback and demos.
Changes:
- Publish a scan-time-aligned
/gt_insnav_msgs/Pathsampled at the same LiDAR scan timestamps as the localized path. - Publish accepted-GICP-only overlays as a “current segment”
Pathand a persistent “broken segments”MarkerArrayhistory. - Expose
debug_pubandverbose_scan_logas launch arguments and document overlay topic semantics / default RViz colors.
Reviewed changes
Copilot reviewed 5 out of 5 changed files in this pull request and generated 2 comments.
Show a summary per file
| File | Description |
|---|---|
| gicp_localization/src/localization.cc | Adds publishers, buffers, and publish logic for /gt_ins and accepted-GICP-only overlays. |
| gicp_localization/include/gicp_localization/localization.h | Declares new publishers/state and buffers for the new overlays. |
| gicp_localization/launch/localization_with_tf.launch.py | Wires new launch args into node parameters to enable debug/RViz publishers and scan logging. |
| gicp_localization/launch/localization.rviz | Adds RViz displays for /gt_ins and gicp_only_segments. |
| gicp_localization/README.md | Documents new topics plus default RViz overlay color semantics. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+3406
to
+3425
| 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; | ||
| } |
Comment on lines
+3431
to
+3438
| 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(); |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
/gt_inspath for RViz/debug comparison against the final localization path.gicp/localization/gicp_only_pathsegment and persistent orangegicp/localization/gicp_only_segmentsmarker history.debug_pubandverbose_scan_loglaunch arguments intogicp_localizationparameters so the debug/RViz publishers can actually be enabled from the normal launch flow.gicp_localization/README.md.Why
/gicp/localization/pathis the final adopted localization result, so it includes accepted GICP poses as well as IMU fallback and GT recovery snaps. That made RViz demos ambiguous: a strange point on the final path did not directly show whether GICP accepted that scan or whether the final output came from fallback/recovery. The added overlays make that distinction visible on the same replay timeline.Impact
Trajectory Pathremains/gicp/localization/path, the final adopted localization trajectory.GICP-only Segmentsonly advances on scans accepted by GICP and breaks during IMU fallback or GT recovery.GT INS Referenceis sampled at the same LiDAR scan timestamps as the final path for time-aligned visual comparison.Validation
git diff --check origin/art-jazzy...HEADsource /opt/ros/jazzy/setup.bash && source /home/roar/Documents/race_common/install/setup.bash && colcon build --packages-select gicp_localization --symlink-install/gicp/localization/path,/gicp/localization/gicp_only_segments, and/gt_ins.