feat(localization_error_monitor): change subscribing topic type (tier…

…4#1532)

* feat(localization_error_monitor): change subscribing topic type

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml

@@ -2,7 +2,7 @@
 <launch>
   <group>
     <include file="$(find-pkg-share localization_error_monitor)/launch/localization_error_monitor.launch.xml">
-      <arg name="input/pose_with_cov" value="/localization/pose_with_covariance"/>
+      <arg name="input/odom" value="/localization/kinematic_state"/>
       <arg name="param_file" value="$(find-pkg-share tier4_localization_launch)/config/localization_error_monitor.param.yaml"/>
     </include>
   </group>

localization/localization_error_monitor/include/localization_error_monitor/node.hpp

@@ -20,7 +20,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 struct Ellipse
@@ -35,7 +35,7 @@ struct Ellipse
 class LocalizationErrorMonitor : public rclcpp::Node
 {
 private:
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_cov_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr ellipse_marker_pub_;
 
   rclcpp::TimerBase::SharedPtr timer_;
@@ -50,11 +50,9 @@ class LocalizationErrorMonitor : public rclcpp::Node
   void checkLocalizationAccuracy(diagnostic_updater::DiagnosticStatusWrapper & stat);
   void checkLocalizationAccuracyLateralDirection(
     diagnostic_updater::DiagnosticStatusWrapper & stat);
-  void onPoseWithCovariance(
-    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr input_msg);
+  void onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg);
   visualization_msgs::msg::Marker createEllipseMarker(
-    const Ellipse & ellipse,
-    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_with_cov);
+    const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom);
   double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta);
   void onTimer();
 

localization/localization_error_monitor/launch/localization_error_monitor.launch.xml

@@ -1,9 +1,9 @@
 <launch>
-  <arg name="input/pose_with_cov" default="/localization/pose_with_covariance"/>
+  <arg name="input/odom" default="/localization/kinematic_state"/>
   <arg name="param_file" default="$(find-pkg-share localization_error_monitor)/config/localization_error_monitor.param.yaml"/>
 
   <node name="localization_error_monitor" exec="localization_error_monitor" pkg="localization_error_monitor" output="screen">
-    <remap from="input/pose_with_cov" to="$(var input/pose_with_cov)"/>
+    <remap from="input/odom" to="$(var input/odom)"/>
     <param from="$(var param_file)"/>
   </node>
 </launch>

localization/localization_error_monitor/package.xml

@@ -16,7 +16,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>eigen</depend>
-  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>visualization_msgs</depend>

localization/localization_error_monitor/src/node.cpp

@@ -42,9 +42,8 @@ LocalizationErrorMonitor::LocalizationErrorMonitor()
   warn_ellipse_size_lateral_direction_ =
     this->declare_parameter("warn_ellipse_size_lateral_direction", 0.2);
 
-  pose_with_cov_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    "input/pose_with_cov", 1,
-    std::bind(&LocalizationErrorMonitor::onPoseWithCovariance, this, std::placeholders::_1));
+  odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+    "input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdom, this, std::placeholders::_1));
 
   // QoS setup
   rclcpp::QoS durable_qos(1);
@@ -101,22 +100,21 @@ void LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection(
 }
 
 visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker(
-  const Ellipse & ellipse,
-  geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_with_cov)
+  const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom)
 {
   tf2::Quaternion quat;
   quat.setEuler(0, 0, ellipse.yaw);
 
   const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0);
   const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0);
   visualization_msgs::msg::Marker marker;
-  marker.header = pose_with_cov->header;
+  marker.header = odom->header;
   marker.header.stamp = this->now();
   marker.ns = "error_ellipse";
   marker.id = 0;
   marker.type = visualization_msgs::msg::Marker::SPHERE;
   marker.action = visualization_msgs::msg::Marker::ADD;
-  marker.pose = pose_with_cov->pose.pose;
+  marker.pose = odom->pose.pose;
   marker.pose.orientation = tf2::toMsg(quat);
   marker.scale.x = ellipse_long_radius * 2;
   marker.scale.y = ellipse_short_radius * 2;
@@ -128,8 +126,7 @@ visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker(
   return marker;
 }
 
-void LocalizationErrorMonitor::onPoseWithCovariance(
-  geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr input_msg)
+void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg)
 {
   // create xy covariance (2x2 matrix)
   // input geometry_msgs::PoseWithCovariance contain 6x6 matrix