From 6dc805e4f1b411ce0d5b37a07bf3607342b1c9cf Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 10 Jul 2024 10:29:13 +0900 Subject: [PATCH] feat(planning_evaluator,control_evaluator, evaluator utils): add diagnostics subscriber to planning eval (#7849) * add utils and diagnostics subscription to planning_evaluator Signed-off-by: Daniel Sanchez * add diagnostics eval Signed-off-by: Daniel Sanchez * fix input diag in launch Signed-off-by: kosuke55 --------- Signed-off-by: Daniel Sanchez Signed-off-by: kosuke55 Co-authored-by: kosuke55 --- .../control_evaluator_node.hpp | 7 -- .../autoware_control_evaluator/package.xml | 1 + .../src/control_evaluator_node.cpp | 50 +------------- .../autoware_evaluator_utils/CMakeLists.txt | 13 ++++ evaluator/autoware_evaluator_utils/README.md | 5 ++ .../evaluator_utils/evaluator_utils.hpp | 45 +++++++++++++ .../autoware_evaluator_utils/package.xml | 24 +++++++ .../src/evaluator_utils.cpp | 66 +++++++++++++++++++ .../planning_evaluator_node.hpp | 20 +++++- .../launch/planning_evaluator.launch.xml | 2 + .../autoware_planning_evaluator/package.xml | 1 + .../src/planning_evaluator_node.cpp | 46 +++++++++++++ 12 files changed, 225 insertions(+), 55 deletions(-) create mode 100644 evaluator/autoware_evaluator_utils/CMakeLists.txt create mode 100644 evaluator/autoware_evaluator_utils/README.md create mode 100644 evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp create mode 100644 evaluator/autoware_evaluator_utils/package.xml create mode 100644 evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index da60c820c45b1..614f1d66b9e0d 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -52,13 +52,6 @@ class ControlEvaluatorNode : public rclcpp::Node { public: explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); - void removeOldDiagnostics(const rclcpp::Time & stamp); - void removeDiagnosticsByName(const std::string & name); - void addDiagnostic(const DiagnosticStatus & diag, const rclcpp::Time & stamp); - void updateDiagnosticQueue( - const DiagnosticArray & input_diagnostics, const std::string & function, - const rclcpp::Time & stamp); - DiagnosticStatus generateLateralDeviationDiagnosticStatus( const Trajectory & traj, const Point & ego_point); DiagnosticStatus generateYawDeviationDiagnosticStatus( diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 21f48b1c64485..d7300e6a3bfb4 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_evaluator_utils autoware_motion_utils autoware_planning_msgs autoware_route_handler diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 5862522d582f7..47706ed56cd7a 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -14,6 +14,8 @@ #include "autoware/control_evaluator/control_evaluator_node.hpp" +#include "autoware/evaluator_utils/evaluator_utils.hpp" + #include #include @@ -64,57 +66,11 @@ void ControlEvaluatorNode::getRouteData() } } -void ControlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) -{ - constexpr double KEEP_TIME = 1.0; - diag_queue_.erase( - std::remove_if( - diag_queue_.begin(), diag_queue_.end(), - [stamp](const std::pair & p) { - return (stamp - p.second).seconds() > KEEP_TIME; - }), - diag_queue_.end()); -} - -void ControlEvaluatorNode::removeDiagnosticsByName(const std::string & name) -{ - diag_queue_.erase( - std::remove_if( - diag_queue_.begin(), diag_queue_.end(), - [&name](const std::pair & p) { - return p.first.name.find(name) != std::string::npos; - }), - diag_queue_.end()); -} - -void ControlEvaluatorNode::addDiagnostic( - const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp) -{ - diag_queue_.push_back(std::make_pair(diag, stamp)); -} - -void ControlEvaluatorNode::updateDiagnosticQueue( - const DiagnosticArray & input_diagnostics, const std::string & function, - const rclcpp::Time & stamp) -{ - const auto it = std::find_if( - input_diagnostics.status.begin(), input_diagnostics.status.end(), - [&function](const diagnostic_msgs::msg::DiagnosticStatus & diag) { - return diag.name.find(function) != std::string::npos; - }); - if (it != input_diagnostics.status.end()) { - removeDiagnosticsByName(it->name); - addDiagnostic(*it, input_diagnostics.header.stamp); - } - - removeOldDiagnostics(stamp); -} - void ControlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) { // add target diagnostics to the queue and remove old ones for (const auto & function : target_functions_) { - updateDiagnosticQueue(*diag_msg, function, now()); + autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); } } diff --git a/evaluator/autoware_evaluator_utils/CMakeLists.txt b/evaluator/autoware_evaluator_utils/CMakeLists.txt new file mode 100644 index 0000000000000..d75ed43a4e3cb --- /dev/null +++ b/evaluator/autoware_evaluator_utils/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_evaluator_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(evaluator_utils SHARED + src/evaluator_utils.cpp +) + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/evaluator/autoware_evaluator_utils/README.md b/evaluator/autoware_evaluator_utils/README.md new file mode 100644 index 0000000000000..b0db86f86b5c0 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/README.md @@ -0,0 +1,5 @@ +# Evaluator Utils + +## Purpose + +This package provides utils functions for other evaluator packages diff --git a/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp b/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp new file mode 100644 index 0000000000000..3b91c9d7605e0 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ +#define AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::evaluator_utils +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using DiagnosticQueue = std::deque>; + +void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); +void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue); +void addDiagnostic( + const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); +void updateDiagnosticQueue( + const DiagnosticArray & input_diagnostics, const std::string & function, + const rclcpp::Time & stamp, DiagnosticQueue & diag_queue); + +} // namespace autoware::evaluator_utils + +#endif // AUTOWARE__EVALUATOR_UTILS__EVALUATOR_UTILS_HPP_ diff --git a/evaluator/autoware_evaluator_utils/package.xml b/evaluator/autoware_evaluator_utils/package.xml new file mode 100644 index 0000000000000..b8566e33a32a3 --- /dev/null +++ b/evaluator/autoware_evaluator_utils/package.xml @@ -0,0 +1,24 @@ + + + + autoware_evaluator_utils + 0.1.0 + ROS 2 node for evaluating control + Daniel SANCHEZ + Takayuki MUROOKA + Apache License 2.0 + + Daniel SANCHEZ + Takayuki MUROOKA + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + rclcpp + rclcpp_components + + + ament_cmake + + diff --git a/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp b/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp new file mode 100644 index 0000000000000..293db21f50e7f --- /dev/null +++ b/evaluator/autoware_evaluator_utils/src/evaluator_utils.cpp @@ -0,0 +1,66 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/evaluator_utils/evaluator_utils.hpp" + +#include + +namespace autoware::evaluator_utils +{ +void removeOldDiagnostics(const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + constexpr double KEEP_TIME = 1.0; + diag_queue.erase( + std::remove_if( + diag_queue.begin(), diag_queue.end(), + [stamp](const std::pair & p) { + return (stamp - p.second).seconds() > KEEP_TIME; + }), + diag_queue.end()); +} + +void removeDiagnosticsByName(const std::string & name, DiagnosticQueue & diag_queue) +{ + diag_queue.erase( + std::remove_if( + diag_queue.begin(), diag_queue.end(), + [&name](const std::pair & p) { + return p.first.name.find(name) != std::string::npos; + }), + diag_queue.end()); +} + +void addDiagnostic( + const DiagnosticStatus & diag, const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + diag_queue.push_back(std::make_pair(diag, stamp)); +} + +void updateDiagnosticQueue( + const DiagnosticArray & input_diagnostics, const std::string & function, + const rclcpp::Time & stamp, DiagnosticQueue & diag_queue) +{ + const auto it = std::find_if( + input_diagnostics.status.begin(), input_diagnostics.status.end(), + [&function](const DiagnosticStatus & diag) { + return diag.name.find(function) != std::string::npos; + }); + if (it != input_diagnostics.status.end()) { + removeDiagnosticsByName(it->name, diag_queue); + addDiagnostic(*it, input_diagnostics.header.stamp, diag_queue); + } + + removeOldDiagnostics(stamp, diag_queue); +} +} // namespace autoware::evaluator_utils diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 91dc0386ae6c6..6925cb15e8fad 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -32,13 +32,14 @@ #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include +#include #include #include #include #include +#include #include - namespace planning_diagnostics { using autoware_perception_msgs::msg::PredictedObjects; @@ -93,12 +94,22 @@ class PlanningEvaluatorNode : public rclcpp::Node const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg, const Odometry::ConstSharedPtr ego_state_ptr); + /** + * @brief obtain diagnostics information + */ + void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); + /** * @brief publish the given metric statistic */ DiagnosticStatus generateDiagnosticStatus( const Metric & metric, const Stat & metric_stat) const; + /** + * @brief publish current ego lane info + */ + DiagnosticStatus generateDiagnosticEvaluationStatus(const DiagnosticStatus & diag); + /** * @brief publish current ego lane info */ @@ -127,6 +138,10 @@ class PlanningEvaluatorNode : public rclcpp::Node * @brief fetch topic data */ void fetchData(); + // The diagnostics cycle is faster than timer, and each node publishes diagnostic separately. + // takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with + // onDiagnostics(). + rclcpp::Subscription::SharedPtr planning_diag_sub_; // ROS autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ @@ -164,6 +179,9 @@ class PlanningEvaluatorNode : public rclcpp::Node std::array>, static_cast(Metric::SIZE)> metric_stats_; rclcpp::TimerBase::SharedPtr timer_; + // queue for diagnostics and time stamp + std::deque> diag_queue_; + const std::vector target_functions_ = {"obstacle_cruise_planner"}; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 70301ebe6275f..c9517404b63a9 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -7,6 +7,7 @@ + @@ -15,6 +16,7 @@ + diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 3c3d4bd8bb595..eb3161e0afb35 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + autoware_evaluator_utils autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 9b65dbbc0b89e..ae3416b9cba8f 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -14,6 +14,8 @@ #include "autoware/planning_evaluator/planning_evaluator_node.hpp" +#include "autoware/evaluator_utils/evaluator_utils.hpp" + #include #include @@ -21,6 +23,7 @@ #include "boost/lexical_cast.hpp" +#include #include #include #include @@ -55,6 +58,9 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op output_file_str_ = declare_parameter("output_file"); ego_frame_str_ = declare_parameter("ego_frame"); + planning_diag_sub_ = create_subscription( + "~/input/diagnostics", 1, std::bind(&PlanningEvaluatorNode::onDiagnostics, this, _1)); + // List of metrics to calculate and publish metrics_pub_ = create_publisher("~/metrics", 1); for (const std::string & selected_metric : @@ -97,6 +103,29 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode() } } +void PlanningEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) +{ + // add target diagnostics to the queue and remove old ones + for (const auto & function : target_functions_) { + autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); + } +} + +DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticEvaluationStatus( + const DiagnosticStatus & diag) +{ + DiagnosticStatus status; + status.name = diag.name; + + const auto it = std::find_if(diag.values.begin(), diag.values.end(), [](const auto & key_value) { + return key_value.key.find("decision") != std::string::npos; + }); + const bool found = it != diag.values.end(); + status.level = (found) ? status.OK : status.ERROR; + status.values.push_back((found) ? *it : diagnostic_msgs::msg::KeyValue{}); + return status; +} + void PlanningEvaluatorNode::getRouteData() { // route @@ -232,6 +261,23 @@ void PlanningEvaluatorNode::onTimer() const auto modified_goal_msg = modified_goal_sub_.takeData(); onModifiedGoal(modified_goal_msg, ego_state_ptr); } + + { + // generate decision diagnostics from input diagnostics + for (const auto & function : target_functions_) { + const auto it = std::find_if( + diag_queue_.begin(), diag_queue_.end(), + [&function](const std::pair & p) { + return p.first.name.find(function) != std::string::npos; + }); + if (it == diag_queue_.end()) { + continue; + } + // generate each decision diagnostics + metrics_msg_.status.push_back(generateDiagnosticEvaluationStatus(it->first)); + } + } + if (!metrics_msg_.status.empty()) { metrics_pub_->publish(metrics_msg_); }