add debug/pose_with_covariance

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>

Author: YamatoAndo

localization/landmark_based_localizer/lidar_marker_localizer/README.md

@@ -19,6 +19,7 @@
 | Name                            | Type                                            | Description                                                        |
 | :------------------------------ | :---------------------------------------------- | :----------------------------------------------------------------- |
 | `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose                                                     |
+| `~/debug/pose_with_covariance`  | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] Estimated Pose                                       |
 | `~/debug/result`                | `sensor_msgs::msg::Image`                       | [debug topic]                                                      |
 | `~/debug/marker`                | `visualization_msgs::msg::MarkerArray`          | [debug topic] Loaded landmarks to visualize in Rviz as thin boards |
 | `/diagnostics`                  | `diagnostic_msgs::msg::DiagnosticArray`         | Diagnostics outputs                                                |

localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.cpp

@@ -90,6 +90,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer()
   qos_marker.reliable();
   pub_marker_mapped_ = this->create_publisher<MarkerArray>("~/debug/marker_mapped", qos_marker);
   pub_marker_detected_ = this->create_publisher<PoseArray>("~/debug/marker_detected", 10);
+  pub_debug_pose_with_covariance_ = this->create_publisher<PoseWithCovarianceStamped>("~/debug/pose_with_covariance", 10);
 
   service_trigger_node_ = this->create_service<SetBool>(
     "~/service/trigger_node_srv",
@@ -284,6 +285,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin
   result.pose.covariance = rotate_covariance(param_.base_covariance, map_to_base_link_rotation);
 
   pub_base_link_pose_with_covariance_on_map_->publish(result);
+  pub_debug_pose_with_covariance_->publish(result);
 }
 
 void LidarMarkerLocalizer::service_trigger_node(

localization/landmark_based_localizer/lidar_marker_localizer/src/lidar_marker_localizer.hpp

@@ -112,6 +112,7 @@ class LidarMarkerLocalizer : public rclcpp::Node
   rclcpp::Service<SetBool>::SharedPtr service_trigger_node_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_mapped_;
   rclcpp::Publisher<PoseArray>::SharedPtr pub_marker_detected_;
+  rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_debug_pose_with_covariance_;
 
   std::shared_ptr<DiagnosticsModule> diagnostics_module_;