From c33cf12ecfdd2d4f3c8a0c77478cbc05f754665e Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Tue, 6 Aug 2024 16:08:26 +0900 Subject: [PATCH 1/4] WIP add control evaluator Signed-off-by: Daniel Sanchez --- .../autoware_control_evaluator/CMakeLists.txt | 24 ++ control/autoware_control_evaluator/README.md | 5 + .../control_evaluator_node.hpp | 108 ++++++++ .../metrics/deviation_metrics.hpp | 49 ++++ .../launch/control_evaluator.launch.xml | 21 ++ .../autoware_control_evaluator/package.xml | 35 +++ .../param/control_evaluator.defaults.yaml | 2 + .../src/control_evaluator_node.cpp | 237 ++++++++++++++++++ .../src/metrics/deviation_metrics.cpp | 41 +++ .../autoware_evaluator_utils/CMakeLists.txt | 13 + control/autoware_evaluator_utils/README.md | 5 + .../evaluator_utils/evaluator_utils.hpp | 45 ++++ control/autoware_evaluator_utils/package.xml | 24 ++ .../src/evaluator_utils.cpp | 66 +++++ 14 files changed, 675 insertions(+) create mode 100644 control/autoware_control_evaluator/CMakeLists.txt create mode 100644 control/autoware_control_evaluator/README.md create mode 100644 control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp create mode 100644 control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp create mode 100644 control/autoware_control_evaluator/launch/control_evaluator.launch.xml create mode 100644 control/autoware_control_evaluator/package.xml create mode 100644 control/autoware_control_evaluator/param/control_evaluator.defaults.yaml create mode 100644 control/autoware_control_evaluator/src/control_evaluator_node.cpp create mode 100644 control/autoware_control_evaluator/src/metrics/deviation_metrics.cpp create mode 100644 control/autoware_evaluator_utils/CMakeLists.txt create mode 100644 control/autoware_evaluator_utils/README.md create mode 100644 control/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp create mode 100644 control/autoware_evaluator_utils/package.xml create mode 100644 control/autoware_evaluator_utils/src/evaluator_utils.cpp diff --git a/control/autoware_control_evaluator/CMakeLists.txt b/control/autoware_control_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..2e6f9851cafce --- /dev/null +++ b/control/autoware_control_evaluator/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_control_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(pluginlib REQUIRED) + +ament_auto_add_library(control_evaluator_node SHARED + src/control_evaluator_node.cpp + src/metrics/deviation_metrics.cpp +) + +rclcpp_components_register_node(control_evaluator_node + PLUGIN "control_diagnostics::ControlEvaluatorNode" + EXECUTABLE control_evaluator +) + + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/control/autoware_control_evaluator/README.md b/control/autoware_control_evaluator/README.md new file mode 100644 index 0000000000000..71bd5c0142570 --- /dev/null +++ b/control/autoware_control_evaluator/README.md @@ -0,0 +1,5 @@ +# Planning Evaluator + +## Purpose + +This package provides nodes that generate metrics to evaluate the quality of control. diff --git a/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp new file mode 100644 index 0000000000000..a12f6465adc42 --- /dev/null +++ b/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -0,0 +1,108 @@ +// 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__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ + +#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 + +namespace control_diagnostics +{ + +using autoware_auto_planning_msgs::msg::Trajectory; +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; +using LaneletRoute = autoware_auto_planning_msgs::msg::HADMapRoute; +using geometry_msgs::msg::AccelWithCovarianceStamped; + +/** + * @brief Node for control evaluation + */ +class ControlEvaluatorNode : public rclcpp::Node +{ +public: + explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); + DiagnosticStatus generateLateralDeviationDiagnosticStatus( + 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; + DiagnosticStatus generateKinematicStateDiagnosticStatus( + const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); + + void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); + void onTimer(); + +private: + // 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 control_diag_sub_; + + autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ + this, "~/input/odometry"}; + autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ + this, "~/input/acceleration"}; + autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ + this, "~/input/trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletRoute, autoware::universe_utils::polling_policy::Newest> + route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber< + LaneletMapBin, autoware::universe_utils::polling_policy::Newest> + 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_; + + // queue for diagnostics and time stamp + std::deque> diag_queue_; + const std::vector target_functions_ = {"autonomous_emergency_braking"}; + + route_handler::RouteHandler route_handler_; + rclcpp::TimerBase::SharedPtr timer_; + std::optional prev_acc_stamped_{std::nullopt}; +}; +} // namespace control_diagnostics + +#endif // AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ diff --git a/control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp b/control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp new file mode 100644 index 0000000000000..1e994e56f56b1 --- /dev/null +++ b/control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp @@ -0,0 +1,49 @@ +// 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__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ + +#include + +#include <#include "autoware_auto_planning_msgs/msg/trajectory.hpp"> + +namespace control_diagnostics +{ +namespace metrics +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; + +/** + * @brief calculate lateral deviation of the given trajectory from the reference trajectory + * @param [in] ref reference trajectory + * @param [in] point input point + * @return lateral deviation + */ +double calcLateralDeviation(const Trajectory & traj, const Point & point); + +/** + * @brief calculate yaw deviation of the given trajectory from the reference trajectory + * @param [in] traj input trajectory + * @param [in] pose input pose + * @return yaw deviation + */ +double calcYawDeviation(const Trajectory & traj, const Pose & pose); + +} // namespace metrics +} // namespace control_diagnostics + +#endif // AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/control/autoware_control_evaluator/launch/control_evaluator.launch.xml b/control/autoware_control_evaluator/launch/control_evaluator.launch.xml new file mode 100644 index 0000000000000..0070c07fe4aa7 --- /dev/null +++ b/control/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_control_evaluator/package.xml b/control/autoware_control_evaluator/package.xml new file mode 100644 index 0000000000000..d7300e6a3bfb4 --- /dev/null +++ b/control/autoware_control_evaluator/package.xml @@ -0,0 +1,35 @@ + + + + autoware_control_evaluator + 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 + + autoware_evaluator_utils + autoware_motion_utils + autoware_planning_msgs + autoware_route_handler + autoware_universe_utils + diagnostic_msgs + pluginlib + rclcpp + rclcpp_components + + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autoware_control_evaluator/param/control_evaluator.defaults.yaml b/control/autoware_control_evaluator/param/control_evaluator.defaults.yaml new file mode 100644 index 0000000000000..a20dbd7ffd3d9 --- /dev/null +++ b/control/autoware_control_evaluator/param/control_evaluator.defaults.yaml @@ -0,0 +1,2 @@ +/**: + ros__parameters: diff --git a/control/autoware_control_evaluator/src/control_evaluator_node.cpp b/control/autoware_control_evaluator/src/control_evaluator_node.cpp new file mode 100644 index 0000000000000..01ee2c7bd9450 --- /dev/null +++ b/control/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -0,0 +1,237 @@ +// 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/control_evaluator/control_evaluator_node.hpp" + +#include "autoware/evaluator_utils/evaluator_utils.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace control_diagnostics +{ +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)); + + // Publisher + metrics_pub_ = create_publisher("~/metrics", 1); + + // 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)); +} + +void ControlEvaluatorNode::getRouteData() +{ + // route + { + const auto msg = route_subscriber_.takeData(); + 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_.takeData(); + if (msg) { + route_handler_.setMap(*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_) { + autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); + } +} + +DiagnosticStatus ControlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) +{ + DiagnosticStatus status; + status.level = status.OK; + status.name = diag.name; + 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) ? "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; +} + +DiagnosticStatus ControlEvaluatorNode::generateLateralDeviationDiagnosticStatus( + const Trajectory & traj, const Point & ego_point) +{ + const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point); + + DiagnosticStatus status; + status.level = status.OK; + status.name = "lateral_deviation"; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(lateral_deviation); + status.values.push_back(key_value); + + return status; +} + +DiagnosticStatus ControlEvaluatorNode::generateYawDeviationDiagnosticStatus( + const Trajectory & traj, const Pose & ego_pose) +{ + const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); + + DiagnosticStatus status; + status.level = status.OK; + status.name = "yaw_deviation"; + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(yaw_deviation); + status.values.push_back(key_value); + + return status; +} + +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_) { + 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 + // - AEB decision + if (it->first.name.find("autonomous_emergency_braking") != std::string::npos) { + metrics_msg.status.push_back(generateAEBDiagnosticStatus(it->first)); + } + } + + // calculate deviation metrics + if (odom && traj && !traj->points.empty()) { + const Pose ego_pose = odom->pose.pose; + metrics_msg.status.push_back( + generateLateralDeviationDiagnosticStatus(*traj, ego_pose.position)); + 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); +} +} // namespace control_diagnostics + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::ControlEvaluatorNode) diff --git a/control/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/control/autoware_control_evaluator/src/metrics/deviation_metrics.cpp new file mode 100644 index 0000000000000..6144a4fb33678 --- /dev/null +++ b/control/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -0,0 +1,41 @@ +// 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/control_evaluator/metrics/deviation_metrics.hpp" + +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" + +namespace control_diagnostics +{ +namespace metrics +{ +using autoware_planning_msgs::msg::Trajectory; + +double calcLateralDeviation(const Trajectory & traj, const Point & point) +{ + const size_t nearest_index = tier4_autoware_utils::findNearestIndex(traj.points, point); + return std::abs( + tier4_autoware_utils::calcLateralDeviation(traj.points[nearest_index].pose, point)); +} + +double calcYawDeviation(const Trajectory & traj, const Pose & pose) +{ + const size_t nearest_index = tier4_autoware_utils::findNearestIndex(traj.points, pose.position); + return std::abs(tier4_autoware_utils::calcYawDeviation(traj.points[nearest_index].pose, pose)); +} + +} // namespace metrics +} // namespace control_diagnostics diff --git a/control/autoware_evaluator_utils/CMakeLists.txt b/control/autoware_evaluator_utils/CMakeLists.txt new file mode 100644 index 0000000000000..d75ed43a4e3cb --- /dev/null +++ b/control/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/control/autoware_evaluator_utils/README.md b/control/autoware_evaluator_utils/README.md new file mode 100644 index 0000000000000..b0db86f86b5c0 --- /dev/null +++ b/control/autoware_evaluator_utils/README.md @@ -0,0 +1,5 @@ +# Evaluator Utils + +## Purpose + +This package provides utils functions for other evaluator packages diff --git a/control/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp b/control/autoware_evaluator_utils/include/autoware/evaluator_utils/evaluator_utils.hpp new file mode 100644 index 0000000000000..3b91c9d7605e0 --- /dev/null +++ b/control/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/control/autoware_evaluator_utils/package.xml b/control/autoware_evaluator_utils/package.xml new file mode 100644 index 0000000000000..b8566e33a32a3 --- /dev/null +++ b/control/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/control/autoware_evaluator_utils/src/evaluator_utils.cpp b/control/autoware_evaluator_utils/src/evaluator_utils.cpp new file mode 100644 index 0000000000000..293db21f50e7f --- /dev/null +++ b/control/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 From 7f11ec6e0146a25dce405b9bec0c0f56fe7210d1 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 7 Aug 2024 18:44:35 +0900 Subject: [PATCH 2/4] change for control_evaluator's build success Signed-off-by: Kyoichi Sugahara --- .../autoware/control_evaluator/control_evaluator_node.hpp | 2 +- .../control_evaluator/metrics/deviation_metrics.hpp | 5 ++--- control/autoware_control_evaluator/package.xml | 4 +++- .../src/control_evaluator_node.cpp | 2 +- .../src/metrics/deviation_metrics.cpp | 6 +++--- 5 files changed, 10 insertions(+), 9 deletions(-) diff --git a/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index a12f6465adc42..5f7d08e2acf2b 100644 --- a/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -22,7 +22,7 @@ #include #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include +#include #include #include diff --git a/control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp b/control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp index 1e994e56f56b1..0602d76cb6268 100644 --- a/control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp +++ b/control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp @@ -15,9 +15,8 @@ #ifndef AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ #define AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#include - -#include <#include "autoware_auto_planning_msgs/msg/trajectory.hpp"> +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" namespace control_diagnostics { diff --git a/control/autoware_control_evaluator/package.xml b/control/autoware_control_evaluator/package.xml index d7300e6a3bfb4..6c44eb62cce77 100644 --- a/control/autoware_control_evaluator/package.xml +++ b/control/autoware_control_evaluator/package.xml @@ -16,9 +16,11 @@ autoware_evaluator_utils autoware_motion_utils - autoware_planning_msgs + tier4_autoware_utils + autoware_auto_planning_msgs autoware_route_handler autoware_universe_utils + route_handler diagnostic_msgs pluginlib rclcpp diff --git a/control/autoware_control_evaluator/src/control_evaluator_node.cpp b/control/autoware_control_evaluator/src/control_evaluator_node.cpp index 01ee2c7bd9450..daf5cfa9f9da0 100644 --- a/control/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/control/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -92,7 +92,7 @@ DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos 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); + const auto shoulder_lanelets = route_handler_.getShoulderLanelets(); lanelet::ConstLanelets closest_lanelets{closest_route_lanelet}; closest_lanelets.insert( closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end()); diff --git a/control/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/control/autoware_control_evaluator/src/metrics/deviation_metrics.cpp index 6144a4fb33678..210f1dff01c17 100644 --- a/control/autoware_control_evaluator/src/metrics/deviation_metrics.cpp +++ b/control/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -14,15 +14,15 @@ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/trajectory/trajectory.hpp" namespace control_diagnostics { namespace metrics { -using autoware_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::Trajectory; double calcLateralDeviation(const Trajectory & traj, const Point & point) { From cc9969ccbfb0d892db992c51de86bbbc7903f41a Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 8 Aug 2024 18:52:26 +0900 Subject: [PATCH 3/4] chore: fix package.xml to avoid rosdep error Signed-off-by: kyoichi-sugahara --- control/autonomous_emergency_braking/package.xml | 4 ++++ control/autoware_control_evaluator/package.xml | 3 --- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index f68c16798ca79..44c8da5301fed 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -8,6 +8,10 @@ Apache License 2.0 ament_cmake + autoware_cmake + ament_cmake_ros + ament_lint_auto + autoware_lint_common autoware_cmake diff --git a/control/autoware_control_evaluator/package.xml b/control/autoware_control_evaluator/package.xml index 6c44eb62cce77..78422ca66bb3d 100644 --- a/control/autoware_control_evaluator/package.xml +++ b/control/autoware_control_evaluator/package.xml @@ -15,11 +15,8 @@ autoware_cmake autoware_evaluator_utils - autoware_motion_utils tier4_autoware_utils autoware_auto_planning_msgs - autoware_route_handler - autoware_universe_utils route_handler diagnostic_msgs pluginlib From 27ad565674d9a72f3fa48b547b28a5e0e698b1a1 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 9 Aug 2024 13:44:32 +0900 Subject: [PATCH 4/4] feat: add IMU subscriber to control evaluator node Signed-off-by: Kyoichi Sugahara --- .../control_evaluator_node.hpp | 13 ++++++--- .../src/control_evaluator_node.cpp | 27 ++++++++++--------- 2 files changed, 23 insertions(+), 17 deletions(-) diff --git a/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 5f7d08e2acf2b..4ff4765d48c08 100644 --- a/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -44,6 +45,8 @@ using nav_msgs::msg::Odometry; using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using LaneletRoute = autoware_auto_planning_msgs::msg::HADMapRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; +using sensor_msgs::msg::Imu; + /** * @brief Node for control evaluation @@ -62,7 +65,7 @@ class ControlEvaluatorNode : public rclcpp::Node DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const; DiagnosticStatus generateKinematicStateDiagnosticStatus( - const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); + const Odometry & odom, const Imu & imu); void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); void onTimer(); @@ -75,8 +78,10 @@ class ControlEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ - this, "~/input/acceleration"}; + // autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ + // this, "~/input/acceleration"}; + autoware::universe_utils::InterProcessPollingSubscriber imu_sub_{ + this, "~/input/imu"}; autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; autoware::universe_utils::InterProcessPollingSubscriber< @@ -101,7 +106,7 @@ class ControlEvaluatorNode : public rclcpp::Node route_handler::RouteHandler route_handler_; rclcpp::TimerBase::SharedPtr timer_; - std::optional prev_acc_stamped_{std::nullopt}; + std::optional prev_imu_{std::nullopt}; }; } // namespace control_diagnostics diff --git a/control/autoware_control_evaluator/src/control_evaluator_node.cpp b/control/autoware_control_evaluator/src/control_evaluator_node.cpp index daf5cfa9f9da0..aa939d4e32791 100644 --- a/control/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/control/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -119,7 +119,7 @@ DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos } DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus( - const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped) + const Odometry & odom, const Imu & imu) { DiagnosticStatus status; status.name = "kinematic_state"; @@ -129,24 +129,24 @@ DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus( 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; + const auto & acc = imu.linear_acceleration.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; + if (!prev_imu_.has_value()) { + prev_imu_ = imu; 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 t = static_cast(imu.header.stamp.sec) + + static_cast(imu.header.stamp.nanosec) * 1e-9; + const auto prev_t = static_cast(prev_imu_.value().header.stamp.sec) + + static_cast(prev_imu_.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; + const auto prev_acc = prev_imu_.value().linear_acceleration.x; + prev_imu_ = imu; return (acc - prev_acc) / dt; }(); key_value.value = std::to_string(jerk); @@ -191,7 +191,8 @@ void ControlEvaluatorNode::onTimer() DiagnosticArray metrics_msg; const auto traj = traj_sub_.takeData(); const auto odom = odometry_sub_.takeData(); - const auto acc = accel_sub_.takeData(); + // const auto acc = accel_sub_.takeData(); + const auto imu = imu_sub_.takeData(); // generate decision diagnostics from input diagnostics for (const auto & function : target_functions_) { @@ -224,8 +225,8 @@ void ControlEvaluatorNode::onTimer() metrics_msg.status.push_back(generateLaneletDiagnosticStatus(ego_pose)); } - if (odom && acc) { - metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*odom, *acc)); + if (odom && imu) { + metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*odom, *imu)); } metrics_msg.header.stamp = now();