diff --git a/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml b/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml index 99364b285f737..43cfc7403184d 100644 --- a/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml +++ b/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp index 272a6ad5dab70..83b2ddef9215a 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include struct Ellipse @@ -35,7 +35,7 @@ struct Ellipse class LocalizationErrorMonitor : public rclcpp::Node { private: - rclcpp::Subscription::SharedPtr pose_with_cov_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::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(); diff --git a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml index dc7359bc90aac..eb2b5741b00fc 100644 --- a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml +++ b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 70f46272d2951..6ecb7bea2e189 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -16,7 +16,7 @@ diagnostic_msgs diagnostic_updater eigen - geometry_msgs + nav_msgs tf2 tf2_geometry_msgs visualization_msgs diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index fa9cff77ba6eb..b5647e46c5236 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/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( - "input/pose_with_cov", 1, - std::bind(&LocalizationErrorMonitor::onPoseWithCovariance, this, std::placeholders::_1)); + odom_sub_ = this->create_subscription( + "input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdom, this, std::placeholders::_1)); // QoS setup rclcpp::QoS durable_qos(1); @@ -101,8 +100,7 @@ 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); @@ -110,13 +108,13 @@ visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( 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