From eabd0f2c2708989f4b152b9a07d492275e65464c Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 2 Jul 2024 11:03:21 +0900 Subject: [PATCH 01/11] feat(control_evaluator): add lanelet info to the metrics (#7765) * add route handler Signed-off-by: Daniel Sanchez * add lanelet info to diagnostic Signed-off-by: Daniel Sanchez * add const Signed-off-by: Daniel Sanchez * add kinematic state info Signed-off-by: Daniel Sanchez * clean Signed-off-by: Daniel Sanchez * remove unusde subscriptions Signed-off-by: Daniel Sanchez * clean Signed-off-by: Daniel Sanchez * add shoulder lanelets Signed-off-by: Daniel Sanchez * fix includes Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../control_evaluator_node.hpp | 23 +++- .../launch/control_evaluator.launch.xml | 8 +- .../autoware_control_evaluator/package.xml | 1 + .../src/control_evaluator_node.cpp | 111 +++++++++++++++++- .../launch/control.launch.py | 5 +- 5 files changed, 139 insertions(+), 9 deletions(-) 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 279bada80e1b9..749ed2296313a 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 @@ -17,15 +17,17 @@ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" +#include #include #include +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include #include #include -#include #include -#include +#include #include #include #include @@ -39,6 +41,9 @@ using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; +using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletRoute; +using geometry_msgs::msg::AccelWithCovarianceStamped; /** * @brief Node for control evaluation @@ -62,6 +67,9 @@ class controlEvaluatorNode : public rclcpp::Node const DiagnosticArray & diag, const std::string & function_name); DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); + DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const; + DiagnosticStatus generateKinematicStateDiagnosticStatus( + const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); void onTimer(); @@ -74,11 +82,20 @@ class controlEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; + autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ + this, "/localization/acceleration"}; autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ + this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ + this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; rclcpp::Publisher::SharedPtr metrics_pub_; + // update Route Handler + void getRouteData(); + // Calculator // Metrics std::deque stamps_; @@ -87,7 +104,9 @@ class controlEvaluatorNode : public rclcpp::Node std::deque> diag_queue_; const std::vector target_functions_ = {"autonomous_emergency_braking"}; + autoware::route_handler::RouteHandler route_handler_; rclcpp::TimerBase::SharedPtr timer_; + std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace control_diagnostics diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index f977916541752..84e11208770b8 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -1,15 +1,21 @@ + + + + - + + + diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 49631c566eaf5..21f48b1c64485 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -16,6 +16,7 @@ autoware_motion_utils autoware_planning_msgs + autoware_route_handler autoware_universe_utils diagnostic_msgs pluginlib diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index ce749e6e8d5eb..1343137e54bb0 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -14,10 +14,11 @@ #include "autoware/control_evaluator/control_evaluator_node.hpp" -#include -#include -#include -#include +#include +#include + +#include +#include #include #include #include @@ -40,6 +41,29 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this)); } +void controlEvaluatorNode::getRouteData() +{ + // route + { + const auto msg = route_subscriber_.takeNewData(); + if (msg) { + if (msg->segments.empty()) { + RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); + } else { + route_handler_.setRoute(*msg); + } + } + } + + // map + { + const auto msg = vector_map_subscriber_.takeNewData(); + if (msg) { + route_handler_.setMap(*msg); + } + } +} + void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) { constexpr double KEEP_TIME = 1.0; @@ -102,9 +126,75 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos diagnostic_msgs::msg::KeyValue key_value; key_value.key = "decision"; const bool is_emergency_brake = (diag.level == DiagnosticStatus::ERROR); - key_value.value = (is_emergency_brake) ? "stop" : "none"; + key_value.value = (is_emergency_brake) ? "deceleration" : "none"; + status.values.push_back(key_value); + return status; +} + +DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const +{ + const auto current_lanelets = [&]() { + lanelet::ConstLanelet closest_route_lanelet; + route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); + const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(ego_pose); + lanelet::ConstLanelets closest_lanelets{closest_route_lanelet}; + closest_lanelets.insert( + closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end()); + return closest_lanelets; + }(); + const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanelets, ego_pose); + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); + + DiagnosticStatus status; + status.name = "ego_lane_info"; + status.level = status.OK; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "lane_id"; + key_value.value = std::to_string(current_lane.id()); status.values.push_back(key_value); + key_value.key = "s"; + key_value.value = std::to_string(arc_coordinates.length); + status.values.push_back(key_value); + key_value.key = "t"; + key_value.value = std::to_string(arc_coordinates.distance); + status.values.push_back(key_value); + return status; +} + +DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus( + const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped) +{ + DiagnosticStatus status; + status.name = "kinematic_state"; + status.level = status.OK; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "vel"; + key_value.value = std::to_string(odom.twist.twist.linear.x); + status.values.push_back(key_value); + key_value.key = "acc"; + const auto & acc = accel_stamped.accel.accel.linear.x; + key_value.value = std::to_string(acc); + status.values.push_back(key_value); + key_value.key = "jerk"; + const auto jerk = [&]() { + if (!prev_acc_stamped_.has_value()) { + prev_acc_stamped_ = accel_stamped; + return 0.0; + } + const auto t = static_cast(accel_stamped.header.stamp.sec) + + static_cast(accel_stamped.header.stamp.nanosec) * 1e-9; + const auto prev_t = static_cast(prev_acc_stamped_.value().header.stamp.sec) + + static_cast(prev_acc_stamped_.value().header.stamp.nanosec) * 1e-9; + const auto dt = t - prev_t; + if (dt < std::numeric_limits::epsilon()) return 0.0; + const auto prev_acc = prev_acc_stamped_.value().accel.accel.linear.x; + prev_acc_stamped_ = accel_stamped; + return (acc - prev_acc) / dt; + }(); + key_value.value = std::to_string(jerk); + status.values.push_back(key_value); return status; } @@ -145,6 +235,7 @@ void controlEvaluatorNode::onTimer() DiagnosticArray metrics_msg; const auto traj = traj_sub_.takeData(); const auto odom = odometry_sub_.takeData(); + const auto acc = accel_sub_.takeData(); // generate decision diagnostics from input diagnostics for (const auto & function : target_functions_) { @@ -171,6 +262,16 @@ void controlEvaluatorNode::onTimer() metrics_msg.status.push_back(generateYawDeviationDiagnosticStatus(*traj, ego_pose)); } + getRouteData(); + if (odom && route_handler_.isHandlerReady()) { + const Pose ego_pose = odom->pose.pose; + metrics_msg.status.push_back(generateLaneletDiagnosticStatus(ego_pose)); + } + + if (odom && acc) { + metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*odom, *acc)); + } + metrics_msg.header.stamp = now(); metrics_pub_->publish(metrics_msg); } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index c561a8c1b9018..69350e4a2cf5b 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -380,8 +380,11 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("~/input/diagnostics", "/diagnostics"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/acceleration", "/localization/acceleration"), ("~/input/trajectory", "/planning/scenario_planning/trajectory"), - ("~/metrics", "/control/control_evaluator/metrics"), + ("~/metrics", "/diagnostic/control_evaluator/metrics"), + ("~/input/vector_map", "/map/vector_map"), + ("~/input/route", "/planning/mission_planning/route"), ], ) From a8b0d479bcc08eb0f3926ccfddb410aa21337156 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 2 Jul 2024 18:30:09 +0900 Subject: [PATCH 02/11] refactor(control_evaluator): use class naming standard and use remapped param name (#7782) use class naming standard and use remapped param name Signed-off-by: Daniel Sanchez --- .../autoware_control_evaluator/CMakeLists.txt | 2 +- .../control_evaluator_node.hpp | 6 ++-- .../src/control_evaluator_node.cpp | 32 +++++++++---------- .../launch/control.launch.py | 2 +- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/evaluator/autoware_control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt index 189745349a592..2e6f9851cafce 100644 --- a/evaluator/autoware_control_evaluator/CMakeLists.txt +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -12,7 +12,7 @@ ament_auto_add_library(control_evaluator_node SHARED ) rclcpp_components_register_node(control_evaluator_node - PLUGIN "control_diagnostics::controlEvaluatorNode" + PLUGIN "control_diagnostics::ControlEvaluatorNode" EXECUTABLE control_evaluator ) 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 749ed2296313a..da60c820c45b1 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 @@ -48,10 +48,10 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; /** * @brief Node for control evaluation */ -class controlEvaluatorNode : public rclcpp::Node +class ControlEvaluatorNode : public rclcpp::Node { public: - explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options); + 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); @@ -83,7 +83,7 @@ class controlEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ - this, "/localization/acceleration"}; + this, "~/input/acceleration"}; autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 1343137e54bb0..5862522d582f7 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -25,12 +25,12 @@ namespace control_diagnostics { -controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options) +ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_options) : Node("control_evaluator", node_options) { using std::placeholders::_1; control_diag_sub_ = create_subscription( - "~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1)); + "~/input/diagnostics", 1, std::bind(&ControlEvaluatorNode::onDiagnostics, this, _1)); // Publisher metrics_pub_ = create_publisher("~/metrics", 1); @@ -38,10 +38,10 @@ controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_opti // Timer callback to publish evaluator diagnostics using namespace std::literals::chrono_literals; timer_ = - rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this)); + rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this)); } -void controlEvaluatorNode::getRouteData() +void ControlEvaluatorNode::getRouteData() { // route { @@ -64,7 +64,7 @@ void controlEvaluatorNode::getRouteData() } } -void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) +void ControlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) { constexpr double KEEP_TIME = 1.0; diag_queue_.erase( @@ -76,7 +76,7 @@ void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp) diag_queue_.end()); } -void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name) +void ControlEvaluatorNode::removeDiagnosticsByName(const std::string & name) { diag_queue_.erase( std::remove_if( @@ -87,13 +87,13 @@ void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name) diag_queue_.end()); } -void controlEvaluatorNode::addDiagnostic( +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( +void ControlEvaluatorNode::updateDiagnosticQueue( const DiagnosticArray & input_diagnostics, const std::string & function, const rclcpp::Time & stamp) { @@ -110,7 +110,7 @@ void controlEvaluatorNode::updateDiagnosticQueue( removeOldDiagnostics(stamp); } -void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) +void ControlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg) { // add target diagnostics to the queue and remove old ones for (const auto & function : target_functions_) { @@ -118,7 +118,7 @@ void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr d } } -DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) +DiagnosticStatus ControlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) { DiagnosticStatus status; status.level = status.OK; @@ -131,7 +131,7 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos return status; } -DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const +DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const { const auto current_lanelets = [&]() { lanelet::ConstLanelet closest_route_lanelet; @@ -162,7 +162,7 @@ DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos return status; } -DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus( +DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus( const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped) { DiagnosticStatus status; @@ -198,7 +198,7 @@ DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus( return status; } -DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus( +DiagnosticStatus ControlEvaluatorNode::generateLateralDeviationDiagnosticStatus( const Trajectory & traj, const Point & ego_point) { const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point); @@ -214,7 +214,7 @@ DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus( return status; } -DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus( +DiagnosticStatus ControlEvaluatorNode::generateYawDeviationDiagnosticStatus( const Trajectory & traj, const Pose & ego_pose) { const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); @@ -230,7 +230,7 @@ DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus( return status; } -void controlEvaluatorNode::onTimer() +void ControlEvaluatorNode::onTimer() { DiagnosticArray metrics_msg; const auto traj = traj_sub_.takeData(); @@ -278,4 +278,4 @@ void controlEvaluatorNode::onTimer() } // namespace control_diagnostics #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::ControlEvaluatorNode) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 69350e4a2cf5b..f2493061fd644 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -375,7 +375,7 @@ def launch_setup(context, *args, **kwargs): # control evaluator control_evaluator_component = ComposableNode( package="autoware_control_evaluator", - plugin="control_diagnostics::controlEvaluatorNode", + plugin="control_diagnostics::ControlEvaluatorNode", name="control_evaluator", remappings=[ ("~/input/diagnostics", "/diagnostics"), From 2111e76c13ead099b037791ed6cd8b15aa13d102 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 10 Jul 2024 10:29:13 +0900 Subject: [PATCH 03/11] 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 | 52 +++++- .../launch/planning_evaluator.launch.xml | 4 + .../autoware_planning_evaluator/package.xml | 1 + .../src/planning_evaluator_node.cpp | 176 +++++++++++++++++- 12 files changed, 385 insertions(+), 59 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 0121425c15d1d..755e240155b5f 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 @@ -27,13 +27,15 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" +#include +#include #include #include #include #include +#include #include - namespace planning_diagnostics { using autoware_perception_msgs::msg::PredictedObjects; @@ -82,15 +84,54 @@ class PlanningEvaluatorNode : public rclcpp::Node */ void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg); + /** + * @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 + */ + DiagnosticStatus generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr); + + /** + * @brief publish current ego kinematic state + */ + DiagnosticStatus generateKinematicStateDiagnosticStatus( + const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr); + private: static bool isFinite(const TrajectoryPoint & p); - void publishModifiedGoalDeviationMetrics(); + + /** + * @brief update route handler data + */ + void getRouteData(); + + /** + * @brief fetch data and publish diagnostics + */ + void onTimer(); + + /** + * @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 rclcpp::Subscription::SharedPtr traj_sub_; @@ -114,8 +155,11 @@ class PlanningEvaluatorNode : public rclcpp::Node std::deque stamps_; std::array>, static_cast(Metric::SIZE)> metric_stats_; - Odometry::ConstSharedPtr ego_state_ptr_; - PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_; + 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 b36e276c06944..3d8f0bbbde24a 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -4,6 +4,9 @@ + + + @@ -11,6 +14,7 @@ + diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 5bd500f200eac..19e0c455711d0 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 103e14c73f26d..c93f4bdd3f526 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -14,8 +14,16 @@ #include "autoware/planning_evaluator/planning_evaluator_node.hpp" +#include "autoware/evaluator_utils/evaluator_utils.hpp" + +#include +#include + +#include + #include "boost/lexical_cast.hpp" +#include #include #include #include @@ -63,6 +71,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 : @@ -105,6 +116,121 @@ 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 + { + const auto msg = route_subscriber_.takeNewData(); + if (msg) { + if (msg->segments.empty()) { + RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); + } else { + route_handler_.setRoute(*msg); + } + } + } + + // map + { + const auto msg = vector_map_subscriber_.takeNewData(); + if (msg) { + route_handler_.setMap(*msg); + } + } +} + +DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus( + const Odometry::ConstSharedPtr ego_state_ptr) +{ + const auto & ego_pose = ego_state_ptr->pose.pose; + const auto current_lanelets = [&]() { + lanelet::ConstLanelet closest_route_lanelet; + route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); + const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(ego_pose); + lanelet::ConstLanelets closest_lanelets{closest_route_lanelet}; + closest_lanelets.insert( + closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end()); + return closest_lanelets; + }(); + const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanelets, ego_pose); + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); + + DiagnosticStatus status; + status.name = "ego_lane_info"; + status.level = status.OK; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "lane_id"; + key_value.value = std::to_string(current_lane.id()); + status.values.push_back(key_value); + key_value.key = "s"; + key_value.value = std::to_string(arc_coordinates.length); + status.values.push_back(key_value); + key_value.key = "t"; + key_value.value = std::to_string(arc_coordinates.distance); + status.values.push_back(key_value); + return status; +} + +DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus( + const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr) +{ + DiagnosticStatus status; + status.name = "kinematic_state"; + status.level = status.OK; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "vel"; + key_value.value = std::to_string(ego_state_ptr->twist.twist.linear.x); + status.values.push_back(key_value); + key_value.key = "acc"; + const auto & acc = accel_stamped.accel.accel.linear.x; + key_value.value = std::to_string(acc); + status.values.push_back(key_value); + key_value.key = "jerk"; + const auto jerk = [&]() { + if (!prev_acc_stamped_.has_value()) { + prev_acc_stamped_ = accel_stamped; + return 0.0; + } + const auto t = static_cast(accel_stamped.header.stamp.sec) + + static_cast(accel_stamped.header.stamp.nanosec) * 1e-9; + const auto prev_t = static_cast(prev_acc_stamped_.value().header.stamp.sec) + + static_cast(prev_acc_stamped_.value().header.stamp.nanosec) * 1e-9; + const auto dt = t - prev_t; + if (dt < std::numeric_limits::epsilon()) return 0.0; + + const auto prev_acc = prev_acc_stamped_.value().accel.accel.linear.x; + prev_acc_stamped_ = accel_stamped; + return (acc - prev_acc) / dt; + }(); + key_value.value = std::to_string(jerk); + status.values.push_back(key_value); + return status; +} + DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( const Metric & metric, const Stat & metric_stat) const { @@ -126,7 +252,55 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg) { - if (!ego_state_ptr_) { + metrics_msg_.header.stamp = now(); + + const auto ego_state_ptr = odometry_sub_.takeData(); + onOdometry(ego_state_ptr); + { + const auto objects_msg = objects_sub_.takeData(); + onObjects(objects_msg); + } + + { + const auto ref_traj_msg = ref_sub_.takeData(); + onReferenceTrajectory(ref_traj_msg); + } + + { + const auto traj_msg = traj_sub_.takeData(); + onTrajectory(traj_msg, ego_state_ptr); + } + { + 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_); + } + metrics_msg_ = DiagnosticArray{}; +} + +void PlanningEvaluatorNode::onTrajectory( + const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr) +{ + if (!ego_state_ptr || !traj_msg) { return; } From 92ca5f94201f1583de2e1a20803cce8fa49a5591 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 29 Jul 2024 13:07:58 +0900 Subject: [PATCH 04/11] feat(evalautor): rename evaluator diag topics (#8152) * feat(evalautor): rename evaluator diag topics Signed-off-by: kosuke55 * perception Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../launch/control_evaluator.launch.xml | 2 +- .../launch/planning_evaluator.launch.xml | 2 ++ launch/tier4_simulator_launch/launch/simulator.launch.xml | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index 84e11208770b8..0070c07fe4aa7 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -13,7 +13,7 @@ - + diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 3d8f0bbbde24a..3737da55e17ca 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -18,6 +18,8 @@ + + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 3c5fab58212ed..28db60eecc0ab 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -147,7 +147,7 @@ - + From ef321d12d22f27e3733db2773f4766db50fe7d61 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 10 Sep 2024 18:21:20 +0900 Subject: [PATCH 05/11] docs(control_evaluator): update readme (#8829) * update readme Signed-off-by: Daniel Sanchez * add maintainer Signed-off-by: Daniel Sanchez * Update evaluator/autoware_control_evaluator/package.xml Add extra maintainer Co-authored-by: Tiankui Xian <1041084556@qq.com> --------- Signed-off-by: Daniel Sanchez Co-authored-by: Tiankui Xian <1041084556@qq.com> --- .github/CODEOWNERS | 4 +++- evaluator/autoware_control_evaluator/README.md | 14 +++++++++++++- evaluator/autoware_control_evaluator/package.xml | 2 ++ 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index df8673cd68724..dd44ba4d687b6 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -62,7 +62,9 @@ control/autoware_trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.m control/pure_pursuit/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp +evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp kosuke.takeuchi@tier4.jp +evaluator/autoware_evaluator_utils/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp +evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp diff --git a/evaluator/autoware_control_evaluator/README.md b/evaluator/autoware_control_evaluator/README.md index 71bd5c0142570..59c09015bd7b5 100644 --- a/evaluator/autoware_control_evaluator/README.md +++ b/evaluator/autoware_control_evaluator/README.md @@ -1,5 +1,17 @@ -# Planning Evaluator +# Control Evaluator ## Purpose This package provides nodes that generate metrics to evaluate the quality of control. + +It publishes diagnostic information about control modules' outputs as well as the ego vehicle's current kinematics and position. + +## Evaluated metrics + +The control evaluator uses the metrics defined in `include/autoware/control_evaluator/metrics/deviation_metrics.hpp`to calculate deviations in yaw and lateral distance from the ego's set-point. The control_evaluator can also be customized to offer metrics/evaluation about other control modules. Currently, the control_evaluator offers a simple diagnostic output based on the autonomous_emergency_braking node's output, but this functionality can be extended to evaluate other control modules' performance. + +## Kinematics output + +The control evaluator module also constantly publishes information regarding the ego vehicle's kinematics and position. It publishes the current ego lane id with the longitudinal `s` and lateral `t` arc coordinates. It also publishes the current ego speed, acceleration and jerk in its diagnostic messages. + +This information can be used by other nodes to establish automated evaluation using rosbags: by crosschecking the ego position and kinematics with the evaluated control module's output, it is possible to judge if the evaluated control modules reacted in a satisfactory way at certain interesting points of the rosbag reproduction. diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index d7300e6a3bfb4..52f5a04092713 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -6,6 +6,8 @@ ROS 2 node for evaluating control Daniel SANCHEZ takayuki MUROOKA + kosuke TAKEUCHI + Temkei Kem Apache License 2.0 Daniel SANCHEZ From 4ac4f03738813857e9b79756bb6c5e540c87e60f Mon Sep 17 00:00:00 2001 From: Tiankui Xian <1041084556@qq.com> Date: Fri, 18 Oct 2024 14:16:30 +0900 Subject: [PATCH 06/11] test(autoware_control_evaluator): add test for autoware_control_evaluator. (#9114) * init Signed-off-by: xtk8532704 <1041084556@qq.com> * tmp save. Signed-off-by: xtk8532704 <1041084556@qq.com> * save, there is a bug Signed-off-by: xtk8532704 <1041084556@qq.com> * update package.xml Signed-off-by: xtk8532704 <1041084556@qq.com> * coverage rate 64.5 Signed-off-by: xtk8532704 <1041084556@qq.com> * remove comments. Signed-off-by: xtk8532704 <1041084556@qq.com> --------- Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware_control_evaluator/CMakeLists.txt | 9 + .../control_evaluator_node.hpp | 2 - .../autoware_control_evaluator/package.xml | 5 +- .../test/test_control_evaluator_node.cpp | 197 ++++++++++++++++++ 4 files changed, 210 insertions(+), 3 deletions(-) create mode 100644 evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp diff --git a/evaluator/autoware_control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt index 2e6f9851cafce..5d6474de88015 100644 --- a/evaluator/autoware_control_evaluator/CMakeLists.txt +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -16,6 +16,15 @@ rclcpp_components_register_node(control_evaluator_node EXECUTABLE control_evaluator ) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_control_evaluator + test/test_control_evaluator_node.cpp + ) + target_link_libraries(test_control_evaluator + control_evaluator_node + ) +endif() + ament_auto_package( INSTALL_TO_SHARE 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 614f1d66b9e0d..e6fbed18e8008 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 @@ -56,8 +56,6 @@ class ControlEvaluatorNode : public rclcpp::Node const Trajectory & traj, const Point & ego_point); DiagnosticStatus generateYawDeviationDiagnosticStatus( const Trajectory & traj, const Pose & ego_pose); - std::optional generateStopDiagnosticStatus( - const DiagnosticArray & diag, const std::string & function_name); DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const; diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 52f5a04092713..1de3c0ecacae6 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -20,12 +20,15 @@ autoware_motion_utils autoware_planning_msgs autoware_route_handler + autoware_test_utils autoware_universe_utils diagnostic_msgs + nav_msgs pluginlib rclcpp rclcpp_components - + tf2 + tf2_ros ament_cmake_ros ament_lint_auto diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp new file mode 100644 index 0000000000000..ca51721fa1932 --- /dev/null +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -0,0 +1,197 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include + +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +#include "boost/lexical_cast.hpp" + +#include + +#include +#include +#include +#include + +using EvalNode = control_diagnostics::ControlEvaluatorNode; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; +using diagnostic_msgs::msg::DiagnosticArray; +using nav_msgs::msg::Odometry; + +constexpr double epsilon = 1e-6; + +class EvalTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_control_evaluator"); + + dummy_node = std::make_shared("control_evaluator_test_node"); + eval_node = std::make_shared(options); + // Enable all logging in the node + auto ret = rcutils_logging_set_logger_level( + dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + ret = rcutils_logging_set_logger_level( + eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + traj_pub_ = + rclcpp::create_publisher(dummy_node, "/control_evaluator/input/trajectory", 1); + odom_pub_ = + rclcpp::create_publisher(dummy_node, "/control_evaluator/input/odometry", 1); + tf_broadcaster_ = std::make_unique(dummy_node); + publishEgoPose(0.0, 0.0, 0.0); + } + + ~EvalTest() override { rclcpp::shutdown(); } + + void setTargetMetric(const std::string & metric_str) + { + const auto is_target_metric = [metric_str](const auto & status) { + return status.name == metric_str; + }; + metric_sub_ = rclcpp::create_subscription( + dummy_node, "/control_evaluator/metrics", 1, [=](const DiagnosticArray::ConstSharedPtr msg) { + const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); + if (it != msg->status.end()) { + metric_value_ = boost::lexical_cast(it->values[0].value); + metric_updated_ = true; + } + }); + } + + Trajectory makeTrajectory(const std::vector> & traj) + { + Trajectory t; + t.header.frame_id = "map"; + TrajectoryPoint p; + for (const std::pair & point : traj) { + p.pose.position.x = point.first; + p.pose.position.y = point.second; + t.points.push_back(p); + } + return t; + } + + void publishTrajectory(const Trajectory & traj) + { + traj_pub_->publish(traj); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + double publishTrajectoryAndGetMetric(const Trajectory & traj) + { + metric_updated_ = false; + traj_pub_->publish(traj); + while (!metric_updated_) { + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + return metric_value_; + } + + void publishEgoPose(const double x, const double y, const double yaw) + { + Odometry odom; + odom.header.frame_id = "map"; + odom.header.stamp = dummy_node->now(); + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + odom.pose.pose.orientation.x = q.x(); + odom.pose.pose.orientation.y = q.y(); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); + + odom_pub_->publish(odom); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + // Latest metric value + bool metric_updated_ = false; + double metric_value_; + // Node + rclcpp::Node::SharedPtr dummy_node; + EvalNode::SharedPtr eval_node; + // Trajectory publishers + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Subscription::SharedPtr metric_sub_; + // TF broadcaster + std::unique_ptr tf_broadcaster_; +}; + +TEST_F(EvalTest, TestYawDeviation) +{ + auto setYaw = [](geometry_msgs::msg::Quaternion & msg, const double yaw_rad) { + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw_rad); + msg.x = q.x(); + msg.y = q.y(); + msg.z = q.z(); + msg.w = q.w(); + }; + setTargetMetric("yaw_deviation"); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}}); + for (auto & p : t.points) { + setYaw(p.pose.orientation, M_PI); + } + + publishEgoPose(0.0, 0.0, M_PI); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon); + + publishEgoPose(0.0, 0.0, 0.0); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), M_PI, epsilon); + + publishEgoPose(0.0, 0.0, -M_PI); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon); +} + +TEST_F(EvalTest, TestLateralDeviation) +{ + setTargetMetric("lateral_deviation"); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}}); + + publishEgoPose(0.0, 0.0, 0.0); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon); + + publishEgoPose(1.0, 1.0, 0.0); + EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 1.0, epsilon); +} From 3d83ad1c3e3c1ed9e2e5da66c85093f2ba7af9c3 Mon Sep 17 00:00:00 2001 From: Kazunori-Nakajima <169115284+Kazunori-Nakajima@users.noreply.github.com> Date: Fri, 25 Oct 2024 16:21:17 +0900 Subject: [PATCH 07/11] feat(control_evaluator): add goal accuracy longitudinal, lateral, yaw (#9155) * feat(control_evaluator): add goal accuracy longitudinal, lateral, yaw Signed-off-by: Kasunori-Nakajima * style(pre-commit): autofix * fix: content of kosuke55-san comments Signed-off-by: Kasunori-Nakajima * fix: variable name Signed-off-by: Kasunori-Nakajima * fix: variable name Signed-off-by: Kasunori-Nakajima --------- Signed-off-by: Kasunori-Nakajima Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../control_evaluator_node.hpp | 3 ++ .../metrics/deviation_metrics.hpp | 24 +++++++++ .../src/control_evaluator_node.cpp | 51 +++++++++++++++++++ .../src/metrics/deviation_metrics.cpp | 15 ++++++ 4 files changed, 93 insertions(+) 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 e6fbed18e8008..2daf4a054ba98 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 @@ -56,6 +56,9 @@ class ControlEvaluatorNode : public rclcpp::Node const Trajectory & traj, const Point & ego_point); DiagnosticStatus generateYawDeviationDiagnosticStatus( const Trajectory & traj, const Pose & ego_pose); + DiagnosticStatus generateGoalLongitudinalDeviationDiagnosticStatus(const Pose & ego_pose); + DiagnosticStatus generateGoalLateralDeviationDiagnosticStatus(const Pose & ego_pose); + DiagnosticStatus generateGoalYawDeviationDiagnosticStatus(const Pose & ego_pose); DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const; diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp index 94fbd53532e7d..e0e04b0a65070 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp @@ -42,6 +42,30 @@ double calcLateralDeviation(const Trajectory & traj, const Point & point); */ double calcYawDeviation(const Trajectory & traj, const Pose & pose); +/** + * @brief calculate longitudinal deviation from target_point to base_pose + * @param [in] pose input base_pose + * @param [in] point input target_point + * @return longitudinal deviation from base_pose to target_point + */ +double calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); + +/** + * @brief calculate lateral deviation from target_point to base_pose + * @param [in] pose input base_pose + * @param [in] point input target_point + * @return lateral deviation from base_pose to target_point + */ +double calcLateralDeviation(const Pose & base_pose, const Point & target_point); + +/** + * @brief calculate yaw deviation from base_pose to target_pose + * @param [in] pose input base_pose + * @param [in] pose input target_pose + * @return yaw deviation from base_pose to target_pose + */ +double calcYawDeviation(const Pose & base_pose, const Pose & target_pose); + } // namespace metrics } // namespace control_diagnostics diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 47706ed56cd7a..85ae411311612 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -186,6 +186,53 @@ DiagnosticStatus ControlEvaluatorNode::generateYawDeviationDiagnosticStatus( return status; } +DiagnosticStatus ControlEvaluatorNode::generateGoalLongitudinalDeviationDiagnosticStatus( + const Pose & ego_pose) +{ + DiagnosticStatus status; + const double longitudinal_deviation = + metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position); + + status.level = status.OK; + status.name = "goal_longitudinal_deviation"; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metrics_value"; + key_value.value = std::to_string(longitudinal_deviation); + status.values.push_back(key_value); + return status; +} + +DiagnosticStatus ControlEvaluatorNode::generateGoalLateralDeviationDiagnosticStatus( + const Pose & ego_pose) +{ + DiagnosticStatus status; + const double lateral_deviation = + metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position); + + status.level = status.OK; + status.name = "goal_lateral_deviation"; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metrics_value"; + key_value.value = std::to_string(lateral_deviation); + status.values.push_back(key_value); + return status; +} + +DiagnosticStatus ControlEvaluatorNode::generateGoalYawDeviationDiagnosticStatus( + const Pose & ego_pose) +{ + DiagnosticStatus status; + const double yaw_deviation = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); + + status.level = status.OK; + status.name = "goal_yaw_deviation"; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metrics_value"; + key_value.value = std::to_string(yaw_deviation); + status.values.push_back(key_value); + return status; +} + void ControlEvaluatorNode::onTimer() { DiagnosticArray metrics_msg; @@ -222,6 +269,10 @@ void ControlEvaluatorNode::onTimer() if (odom && route_handler_.isHandlerReady()) { const Pose ego_pose = odom->pose.pose; metrics_msg.status.push_back(generateLaneletDiagnosticStatus(ego_pose)); + + metrics_msg.status.push_back(generateGoalLongitudinalDeviationDiagnosticStatus(ego_pose)); + metrics_msg.status.push_back(generateGoalLateralDeviationDiagnosticStatus(ego_pose)); + metrics_msg.status.push_back(generateGoalYawDeviationDiagnosticStatus(ego_pose)); } if (odom && acc) { diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp index 689291da09f6d..a851eeb410620 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -38,5 +38,20 @@ double calcYawDeviation(const Trajectory & traj, const Pose & pose) autoware::universe_utils::calcYawDeviation(traj.points[nearest_index].pose, pose)); } +double calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) +{ + return std::abs(autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point)); +} + +double calcLateralDeviation(const Pose & base_pose, const Point & target_point) +{ + return std::abs(autoware::universe_utils::calcLateralDeviation(base_pose, target_point)); +} + +double calcYawDeviation(const Pose & base_pose, const Pose & target_pose) +{ + return std::abs(autoware::universe_utils::calcYawDeviation(base_pose, target_pose)); +} + } // namespace metrics } // namespace control_diagnostics From 5eac8907d6e5010a1e754782ebc097949c328eff Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 8 Aug 2024 09:07:21 +0900 Subject: [PATCH 08/11] fix(autoware_planning_evaluator): fix unreadVariable (#8352) * fix:unreadVariable Signed-off-by: kobayu858 * fix:unreadVariable Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../autoware_planning_evaluator/src/metrics/metrics_utils.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index e17cfd98b27da..669afdd7229b0 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -30,9 +30,8 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons const TrajectoryPoint & curr_p = traj.points.at(curr_id); size_t target_id = curr_id; - double current_distance = 0.0; for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { - current_distance = calcDistance2d(traj.points.at(traj_id), curr_p); + double current_distance = calcDistance2d(traj.points.at(traj_id), curr_p); if (current_distance >= distance) { target_id = traj_id; break; From ed0787f74138f1ffeb79217aea5908891fb20439 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 2 Jul 2024 15:58:24 +0900 Subject: [PATCH 09/11] feat(planning_evaluator): add lanelet info to the planning evaluator (#7781) add lanelet info to the planning evaluator Signed-off-by: Daniel Sanchez --- .../planning_evaluator_node.hpp | 14 ++++++++++++++ .../launch/planning_evaluator.launch.xml | 2 ++ .../autoware_planning_evaluator/package.xml | 1 + .../src/planning_evaluator_node.cpp | 19 +++++++++++++++++++ 4 files changed, 36 insertions(+) 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 755e240155b5f..e63f24396bd6f 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 @@ -21,11 +21,15 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include +#include + #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include #include @@ -45,6 +49,9 @@ using autoware_planning_msgs::msg::TrajectoryPoint; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; +using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletRoute; +using geometry_msgs::msg::AccelWithCovarianceStamped; /** * @brief Node for planning evaluation */ @@ -139,10 +146,17 @@ class PlanningEvaluatorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr modified_goal_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ + this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ + this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ + this, "~/input/acceleration"}; rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; + autoware::route_handler::RouteHandler route_handler_; // Parameters std::string output_file_str_; diff --git a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index 3737da55e17ca..ec509993afc18 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -1,5 +1,6 @@ + @@ -13,6 +14,7 @@ + diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 19e0c455711d0..eb3161e0afb35 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -17,6 +17,7 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs + autoware_route_handler autoware_universe_utils diagnostic_msgs eigen diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index c93f4bdd3f526..e87d8e124f490 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -346,6 +346,25 @@ void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_m { ego_state_ptr_ = odometry_msg; metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + { + DiagnosticArray metrics_msg; + metrics_msg.header.stamp = now(); + + getRouteData(); + if (route_handler_.isHandlerReady() && ego_state_ptr_) { + metrics_msg.status.push_back(generateLaneletDiagnosticStatus()); + } + + const auto acc = accel_sub_.takeData(); + + if (acc && ego_state_ptr_) { + metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*acc)); + } + + if (!metrics_msg.status.empty()) { + metrics_pub_->publish(metrics_msg); + } + } if (modified_goal_ptr_) { publishModifiedGoalDeviationMetrics(); From 1a3caa5cfd94d0993a005331c46076ce145874bc Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 28 Oct 2024 15:36:25 +0900 Subject: [PATCH 10/11] fix planning evaluator Signed-off-by: Daniel Sanchez --- .../src/control_evaluator_node.cpp | 4 +- .../planning_evaluator_node.hpp | 66 +----- .../launch/planning_evaluator.launch.xml | 8 - .../autoware_planning_evaluator/package.xml | 2 - .../src/metrics/metrics_utils.cpp | 3 +- .../src/planning_evaluator_node.cpp | 195 +----------------- 6 files changed, 9 insertions(+), 269 deletions(-) diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 85ae411311612..d07ed22fc12dd 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -16,8 +16,8 @@ #include "autoware/evaluator_utils/evaluator_utils.hpp" -#include -#include +#include +#include #include #include 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 e63f24396bd6f..0121425c15d1d 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 @@ -21,25 +21,19 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include -#include - #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" -#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; @@ -49,9 +43,6 @@ using autoware_planning_msgs::msg::TrajectoryPoint; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; -using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; -using autoware_planning_msgs::msg::LaneletRoute; -using geometry_msgs::msg::AccelWithCovarianceStamped; /** * @brief Node for planning evaluation */ @@ -91,54 +82,15 @@ class PlanningEvaluatorNode : public rclcpp::Node */ void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg); - /** - * @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 - */ - DiagnosticStatus generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr); - - /** - * @brief publish current ego kinematic state - */ - DiagnosticStatus generateKinematicStateDiagnosticStatus( - const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr); - private: static bool isFinite(const TrajectoryPoint & p); - - /** - * @brief update route handler data - */ - void getRouteData(); - - /** - * @brief fetch data and publish diagnostics - */ - void onTimer(); - - /** - * @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_; + void publishModifiedGoalDeviationMetrics(); // ROS rclcpp::Subscription::SharedPtr traj_sub_; @@ -146,17 +98,10 @@ class PlanningEvaluatorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr modified_goal_sub_; rclcpp::Subscription::SharedPtr odom_sub_; - autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ - this, "~/input/route", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ - this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ - this, "~/input/acceleration"}; rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; - autoware::route_handler::RouteHandler route_handler_; // Parameters std::string output_file_str_; @@ -169,11 +114,8 @@ class PlanningEvaluatorNode : public rclcpp::Node std::deque stamps_; 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}; + Odometry::ConstSharedPtr ego_state_ptr_; + PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_; }; } // 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 ec509993afc18..b36e276c06944 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -1,27 +1,19 @@ - - - - - - - - diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index eb3161e0afb35..5bd500f200eac 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -13,11 +13,9 @@ ament_cmake_auto autoware_cmake - autoware_evaluator_utils autoware_motion_utils autoware_perception_msgs autoware_planning_msgs - autoware_route_handler autoware_universe_utils diagnostic_msgs eigen diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 669afdd7229b0..e17cfd98b27da 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -30,8 +30,9 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons const TrajectoryPoint & curr_p = traj.points.at(curr_id); size_t target_id = curr_id; + double current_distance = 0.0; for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { - double current_distance = calcDistance2d(traj.points.at(traj_id), curr_p); + current_distance = calcDistance2d(traj.points.at(traj_id), curr_p); if (current_distance >= distance) { target_id = traj_id; break; diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index e87d8e124f490..103e14c73f26d 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -14,16 +14,8 @@ #include "autoware/planning_evaluator/planning_evaluator_node.hpp" -#include "autoware/evaluator_utils/evaluator_utils.hpp" - -#include -#include - -#include - #include "boost/lexical_cast.hpp" -#include #include #include #include @@ -71,9 +63,6 @@ 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 : @@ -116,121 +105,6 @@ 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 - { - const auto msg = route_subscriber_.takeNewData(); - if (msg) { - if (msg->segments.empty()) { - RCLCPP_ERROR(get_logger(), "input route is empty. ignored"); - } else { - route_handler_.setRoute(*msg); - } - } - } - - // map - { - const auto msg = vector_map_subscriber_.takeNewData(); - if (msg) { - route_handler_.setMap(*msg); - } - } -} - -DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus( - const Odometry::ConstSharedPtr ego_state_ptr) -{ - const auto & ego_pose = ego_state_ptr->pose.pose; - const auto current_lanelets = [&]() { - lanelet::ConstLanelet closest_route_lanelet; - route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); - const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(ego_pose); - lanelet::ConstLanelets closest_lanelets{closest_route_lanelet}; - closest_lanelets.insert( - closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end()); - return closest_lanelets; - }(); - const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanelets, ego_pose); - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); - - DiagnosticStatus status; - status.name = "ego_lane_info"; - status.level = status.OK; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "lane_id"; - key_value.value = std::to_string(current_lane.id()); - status.values.push_back(key_value); - key_value.key = "s"; - key_value.value = std::to_string(arc_coordinates.length); - status.values.push_back(key_value); - key_value.key = "t"; - key_value.value = std::to_string(arc_coordinates.distance); - status.values.push_back(key_value); - return status; -} - -DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus( - const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr) -{ - DiagnosticStatus status; - status.name = "kinematic_state"; - status.level = status.OK; - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "vel"; - key_value.value = std::to_string(ego_state_ptr->twist.twist.linear.x); - status.values.push_back(key_value); - key_value.key = "acc"; - const auto & acc = accel_stamped.accel.accel.linear.x; - key_value.value = std::to_string(acc); - status.values.push_back(key_value); - key_value.key = "jerk"; - const auto jerk = [&]() { - if (!prev_acc_stamped_.has_value()) { - prev_acc_stamped_ = accel_stamped; - return 0.0; - } - const auto t = static_cast(accel_stamped.header.stamp.sec) + - static_cast(accel_stamped.header.stamp.nanosec) * 1e-9; - const auto prev_t = static_cast(prev_acc_stamped_.value().header.stamp.sec) + - static_cast(prev_acc_stamped_.value().header.stamp.nanosec) * 1e-9; - const auto dt = t - prev_t; - if (dt < std::numeric_limits::epsilon()) return 0.0; - - const auto prev_acc = prev_acc_stamped_.value().accel.accel.linear.x; - prev_acc_stamped_ = accel_stamped; - return (acc - prev_acc) / dt; - }(); - key_value.value = std::to_string(jerk); - status.values.push_back(key_value); - return status; -} - DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( const Metric & metric, const Stat & metric_stat) const { @@ -252,55 +126,7 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg) { - metrics_msg_.header.stamp = now(); - - const auto ego_state_ptr = odometry_sub_.takeData(); - onOdometry(ego_state_ptr); - { - const auto objects_msg = objects_sub_.takeData(); - onObjects(objects_msg); - } - - { - const auto ref_traj_msg = ref_sub_.takeData(); - onReferenceTrajectory(ref_traj_msg); - } - - { - const auto traj_msg = traj_sub_.takeData(); - onTrajectory(traj_msg, ego_state_ptr); - } - { - 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_); - } - metrics_msg_ = DiagnosticArray{}; -} - -void PlanningEvaluatorNode::onTrajectory( - const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr) -{ - if (!ego_state_ptr || !traj_msg) { + if (!ego_state_ptr_) { return; } @@ -346,25 +172,6 @@ void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_m { ego_state_ptr_ = odometry_msg; metrics_calculator_.setEgoPose(odometry_msg->pose.pose); - { - DiagnosticArray metrics_msg; - metrics_msg.header.stamp = now(); - - getRouteData(); - if (route_handler_.isHandlerReady() && ego_state_ptr_) { - metrics_msg.status.push_back(generateLaneletDiagnosticStatus()); - } - - const auto acc = accel_sub_.takeData(); - - if (acc && ego_state_ptr_) { - metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*acc)); - } - - if (!metrics_msg.status.empty()) { - metrics_pub_->publish(metrics_msg); - } - } if (modified_goal_ptr_) { publishModifiedGoalDeviationMetrics(); From 9a64647449ead1e35bf56e10e1dcd4276c12c7fe Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 28 Oct 2024 15:44:41 +0900 Subject: [PATCH 11/11] fix metrics topic name Signed-off-by: Daniel Sanchez --- launch/tier4_control_launch/launch/control.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index f2493061fd644..995c99cf62c1f 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -382,7 +382,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/odometry", "/localization/kinematic_state"), ("~/input/acceleration", "/localization/acceleration"), ("~/input/trajectory", "/planning/scenario_planning/trajectory"), - ("~/metrics", "/diagnostic/control_evaluator/metrics"), + ("~/metrics", "/control/control_evaluator/metrics"), ("~/input/vector_map", "/map/vector_map"), ("~/input/route", "/planning/mission_planning/route"), ],