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/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt index 189745349a592..5d6474de88015 100644 --- a/evaluator/autoware_control_evaluator/CMakeLists.txt +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -12,10 +12,19 @@ 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 ) +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/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/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 279bada80e1b9..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 @@ -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,29 +41,29 @@ 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 */ -class controlEvaluatorNode : public rclcpp::Node +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); - + 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 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; + DiagnosticStatus generateKinematicStateDiagnosticStatus( + const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); void onTimer(); @@ -74,11 +76,20 @@ 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 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 +98,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/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/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index f977916541752..0070c07fe4aa7 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..1de3c0ecacae6 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 @@ -14,14 +16,19 @@ ament_cmake_auto autoware_cmake + autoware_evaluator_utils 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/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index ce749e6e8d5eb..d07ed22fc12dd 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -14,22 +14,25 @@ #include "autoware/control_evaluator/control_evaluator_node.hpp" -#include -#include -#include -#include +#include "autoware/evaluator_utils/evaluator_utils.hpp" + +#include +#include + +#include +#include #include #include #include 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); @@ -37,64 +40,41 @@ 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)); -} - -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()); + rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this)); } -void controlEvaluatorNode::addDiagnostic( - const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp) +void ControlEvaluatorNode::getRouteData() { - 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); + // 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); + } + } } - removeOldDiagnostics(stamp); + // map + { + const auto msg = vector_map_subscriber_.takeNewData(); + if (msg) { + route_handler_.setMap(*msg); + } + } } -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_) { - updateDiagnosticQueue(*diag_msg, function, now()); + autoware::evaluator_utils::updateDiagnosticQueue(*diag_msg, function, now(), diag_queue_); } } -DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) +DiagnosticStatus ControlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag) { DiagnosticStatus status; status.level = status.OK; @@ -102,13 +82,79 @@ 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::generateLateralDeviationDiagnosticStatus( +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); @@ -124,7 +170,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); @@ -140,11 +186,59 @@ DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus( return status; } -void controlEvaluatorNode::onTimer() +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; 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,10 +265,24 @@ 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)); + + 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) { + 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) +RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::ControlEvaluatorNode) 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 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); +} 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/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index c561a8c1b9018..995c99cf62c1f 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -375,13 +375,16 @@ 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"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/acceleration", "/localization/acceleration"), ("~/input/trajectory", "/planning/scenario_planning/trajectory"), ("~/metrics", "/control/control_evaluator/metrics"), + ("~/input/vector_map", "/map/vector_map"), + ("~/input/route", "/planning/mission_planning/route"), ], ) 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 @@ - +