wip

DCO 1.1 Signed-off-by: Tatsuya Ishihara <[email protected]>
CMU-cabot · Nov 18, 2024 · 1545f4c · 1545f4c
1 parent 5adfc96
commit 1545f4c
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 0 deletions.
diff --git a/cabot_navigation2/src/clip_ground_filter_node.cpp b/cabot_navigation2/src/clip_ground_filter_node.cpp
@@ -61,6 +61,7 @@ void ClipGroundFilterNode::filterGround(
   if (publish_debug_ground_) {
     visualization_msgs::msg::Marker plane_marker;
     plane_marker.header.frame_id = target_frame_;
+    plane_marker.header.stamp = time;
     plane_marker.ns = "clip_plane";
     plane_marker.id = 0;
     plane_marker.type = visualization_msgs::msg::Marker::CUBE;

diff --git a/cabot_navigation2/src/grid_map_ground_filter_node.cpp b/cabot_navigation2/src/grid_map_ground_filter_node.cpp
@@ -639,9 +639,11 @@ void GridMapGroundFilterNode::filterGround(
     sensor_msgs::msg::PointCloud2 outlier_points_msg;
     pcl::toROSMsg(*outlier_points, outlier_points_msg);
     outlier_points_msg.header.frame_id = "odom";
+    outlier_points_msg.header.stamp = time;
     debug_outlier_pointcloud_pub_->publish(outlier_points_msg);
 
     auto grid_map_msg = grid_map::GridMapRosConverter::toMessage(*grid_map_ptr_);
+    grid_map_msg->header.stamp = time;
     debug_grid_map_pub_->publish(std::move(grid_map_msg));
   }
 }

diff --git a/cabot_navigation2/src/ransac_ground_filter_node.cpp b/cabot_navigation2/src/ransac_ground_filter_node.cpp
@@ -116,6 +116,7 @@ void RansacGroundFilterNode::filterGround(
   if (publish_debug_ground_) {
     visualization_msgs::msg::Marker plane_marker;
     plane_marker.header.frame_id = target_frame_;
+    plane_marker.header.stamp = time;
     plane_marker.ns = "ransac_plane";
     plane_marker.id = 0;
     plane_marker.type = visualization_msgs::msg::Marker::CUBE;