From f2ae1af058d28edf1f856332070a6749f15b3395 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 24 May 2024 16:16:32 +0900 Subject: [PATCH 01/10] feat(pose2twist): componentize Pose2Twist (#7113) * remove unusing main func file Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change log output from log to both Signed-off-by: a-maumau * style(pre-commit): autofix --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/pose2twist/CMakeLists.txt | 9 ++++-- .../include/pose2twist/pose2twist_core.hpp | 2 +- .../pose2twist/launch/pose2twist.launch.xml | 2 +- localization/pose2twist/package.xml | 1 + .../pose2twist/src/pose2twist_core.cpp | 5 +++- .../pose2twist/src/pose2twist_node.cpp | 29 ------------------- 6 files changed, 14 insertions(+), 34 deletions(-) delete mode 100644 localization/pose2twist/src/pose2twist_node.cpp diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index a2fbbddf7d120..2a586aa9cd049 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -4,11 +4,16 @@ project(pose2twist) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose2twist - src/pose2twist_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose2twist_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Pose2Twist" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index c2a39f7e2ed3d..459a62ea5cd13 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -24,7 +24,7 @@ class Pose2Twist : public rclcpp::Node { public: - Pose2Twist(); + explicit Pose2Twist(const rclcpp::NodeOptions & options); ~Pose2Twist() = default; private: diff --git a/localization/pose2twist/launch/pose2twist.launch.xml b/localization/pose2twist/launch/pose2twist.launch.xml index a0fae57a163e6..57a41dbfcf017 100644 --- a/localization/pose2twist/launch/pose2twist.launch.xml +++ b/localization/pose2twist/launch/pose2twist.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 16c49bb51c218..07e445c72978c 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -19,6 +19,7 @@ geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tier4_debug_msgs diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index a321a06122816..4b98ec6c81ad4 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -24,7 +24,7 @@ #include #include -Pose2Twist::Pose2Twist() : Node("pose2twist_core") +Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose2twist", options) { using std::placeholders::_1; @@ -129,3 +129,6 @@ void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_ms angular_z_msg.data = twist_msg.twist.angular.z; angular_z_pub_->publish(angular_z_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Pose2Twist) diff --git a/localization/pose2twist/src/pose2twist_node.cpp b/localization/pose2twist/src/pose2twist_node.cpp deleted file mode 100644 index 81f98461ecffd..0000000000000 --- a/localization/pose2twist/src/pose2twist_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 "pose2twist/pose2twist_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} From 27e1aa1dc32abe7a81ca54fabf1e67a544b5f412 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 24 May 2024 16:30:41 +0900 Subject: [PATCH 02/10] fix(perception_online_evaluator): fix range resolution (#7115) Signed-off-by: Fumiya Watanabe --- .../param/perception_online_evaluator.defaults.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml index 807f247807d98..16d4b6fe458cd 100644 --- a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -13,7 +13,7 @@ prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] stopped_velocity_threshold: 1.0 - detection_radius_list: [50.0, 100.0, 150.0, 200.0] + detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0] detection_height_list: [10.0] detection_count_purge_seconds: 36000.0 objects_count_window_seconds: 1.0 From 6d77841dbc0844f778bfa68139867f60ea2ddbd8 Mon Sep 17 00:00:00 2001 From: Motz <83898149+Motsu-san@users.noreply.github.com> Date: Fri, 24 May 2024 16:34:26 +0900 Subject: [PATCH 03/10] refactor(gyro_odometer): componentize node in gyro_odometer (#7090) * refactor: install glog to gyro_odometer Signed-off-by: Motsu-san * style(pre-commit): autofix * feat: change glog error setting --------- Signed-off-by: Motsu-san Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/gyro_odometer/CMakeLists.txt | 14 ++-- .../gyro_odometer/gyro_odometer_core.hpp | 11 ++- .../launch/gyro_odometer.launch.xml | 2 +- localization/gyro_odometer/package.xml | 5 +- .../gyro_odometer/src/gyro_odometer_core.cpp | 81 +++++++++++-------- .../gyro_odometer/src/gyro_odometer_node.cpp | 30 ------- .../test/test_gyro_odometer_pubsub.cpp | 6 +- 7 files changed, 68 insertions(+), 81 deletions(-) delete mode 100644 localization/gyro_odometer/src/gyro_odometer_node.cpp diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 57589837dd529..a832383ff4761 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -4,17 +4,12 @@ project(gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/gyro_odometer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp ) target_link_libraries(${PROJECT_NAME} fmt) -ament_auto_add_library(gyro_odometer_node SHARED - src/gyro_odometer_core.cpp -) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_gyro_odometer test/test_main.cpp @@ -25,10 +20,15 @@ if(BUILD_TESTING) rclcpp ) target_link_libraries(test_gyro_odometer - gyro_odometer_node + ${PROJECT_NAME} ) endif() +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::gyro_odometer::GyroOdometerNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package(INSTALL_TO_SHARE launch diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 3a6358e62c21a..2926589bd2da9 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -36,14 +36,17 @@ #include #include -class GyroOdometer : public rclcpp::Node +namespace autoware::gyro_odometer +{ + +class GyroOdometerNode : public rclcpp::Node { private: using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - explicit GyroOdometer(const rclcpp::NodeOptions & options); - ~GyroOdometer(); + explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); + ~GyroOdometerNode(); private: void callbackVehicleTwist( @@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node std::deque gyro_queue_; }; +} // namespace autoware::gyro_odometer + #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index 21aff3e10d26c..aed6050858fe1 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index a0a982ddedc16..3c979eb69ac8a 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -19,14 +19,13 @@ fmt geometry_msgs + rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs tier4_autoware_utils - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 5de0ecd7cdc0a..bda7af74b8489 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -14,6 +14,8 @@ #include "gyro_odometer/gyro_odometer_core.hpp" +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -25,6 +27,42 @@ #include #include +namespace autoware::gyro_odometer +{ + +GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) +: Node("gyro_odometer", node_options), + output_frame_(declare_parameter("output_frame")), + message_timeout_sec_(declare_parameter("message_timeout_sec")), + vehicle_twist_arrived_(false), + imu_arrived_(false) +{ + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); + + vehicle_twist_sub_ = create_subscription( + "vehicle/twist_with_covariance", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + + imu_sub_ = create_subscription( + "imu", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + + twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); + twist_with_covariance_raw_pub_ = create_publisher( + "twist_with_covariance_raw", rclcpp::QoS{10}); + + twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); + twist_with_covariance_pub_ = create_publisher( + "twist_with_covariance", rclcpp::QoS{10}); + + // TODO(YamatoAndo) createTimer +} + +GyroOdometerNode::~GyroOdometerNode() +{ +} + std::array transformCovariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( return twist_with_cov; } -GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) -: Node("gyro_odometer", options), - output_frame_(declare_parameter("output_frame")), - message_timeout_sec_(declare_parameter("message_timeout_sec")), - vehicle_twist_arrived_(false), - imu_arrived_(false) -{ - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); - - vehicle_twist_sub_ = create_subscription( - "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1)); - - imu_sub_ = create_subscription( - "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); - - twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); - twist_with_covariance_raw_pub_ = create_publisher( - "twist_with_covariance_raw", rclcpp::QoS{10}); - - twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); - twist_with_covariance_pub_ = create_publisher( - "twist_with_covariance", rclcpp::QoS{10}); - - // TODO(YamatoAndo) createTimer -} - -GyroOdometer::~GyroOdometer() -{ -} - -void GyroOdometer::callbackVehicleTwist( +void GyroOdometerNode::callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { vehicle_twist_arrived_ = true; @@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist( gyro_queue_.clear(); } -void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { imu_arrived_ = true; if (!vehicle_twist_arrived_) { @@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m gyro_queue_.clear(); } -void GyroOdometer::publishData( +void GyroOdometerNode::publishData( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; @@ -269,3 +275,8 @@ void GyroOdometer::publishData( twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } + +} // namespace autoware::gyro_odometer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode) diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp deleted file mode 100644 index 5bb6241506fbe..0000000000000 --- a/localization/gyro_odometer/src/gyro_odometer_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 "gyro_odometer/gyro_odometer_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto node = std::make_shared(options); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index 7f6416fbdda16..b7849ef03bfc5 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -108,7 +108,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -135,7 +136,8 @@ TEST(GyroOdometer, TestGyroOdometerImuOnly) { Imu input_imu = generateSampleImu(); - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); From fe3083303429d24ff259b860e362152255122caf Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 24 May 2024 16:47:50 +0900 Subject: [PATCH 04/10] feat(perception_online_evaluator): add metric_value not only stat (#7100) Signed-off-by: kosuke55 --- .../metrics/metric.hpp | 5 +++ .../metrics_calculator.hpp | 4 +- .../perception_online_evaluator_node.hpp | 2 + .../src/metrics_calculator.cpp | 16 ++++---- .../src/perception_online_evaluator_node.cpp | 39 +++++++++++++++---- .../test_perception_online_evaluator_node.cpp | 14 ++++++- 6 files changed, 61 insertions(+), 19 deletions(-) diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 926fbb7435f3a..eaa07f2317940 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace perception_diagnostics @@ -36,7 +37,11 @@ enum class Metric { SIZE, }; +// Each metric has a different return type. (statistic or just a one value etc). +// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; +using MetricValueMap = std::unordered_map; +using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index 96b1c01ee2d16..a04819d4844ce 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricStatMap calcObjectsCountMetrics() const; + MetricValueMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index e5c41ff28c4da..070d938b8bd0a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; + DiagnosticStatus generateDiagnosticStatus( + const std::string metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 9c1e0667e4fef..cc455445ca9f8 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -26,7 +26,7 @@ namespace perception_diagnostics using object_recognition_utils::convertLabelToString; using tier4_autoware_utils::inverseTransformPoint; -std::optional MetricsCalculator::calculate(const Metric & metric) const +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const +MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricStatMap metric_stat_map; + MetricValueMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } // calculate the average number of objects in the detection area in the past @@ -472,8 +471,8 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] - .add(count); + metric_stat_map + ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } @@ -481,8 +480,7 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 0fcdd77f4d515..be9ac2332f62c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); - if (!metric_stat_map.has_value()) { + const auto metric_result = metrics_calculator_.calculate(Metric(metric)); + if (!metric_result.has_value()) { continue; } - for (const auto & [metric, stat] : metric_stat_map.value()) { - if (stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); - } - } + std::visit( + [&metrics_msg, this](auto && arg) { + using T = std::decay_t; + for (const auto & [metric, value] : arg) { + if constexpr (std::is_same_v) { + if (value.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); + } + } else if constexpr (std::is_same_v) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); + } + } + }, + metric_result.value()); } // publish metrics @@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string metric, const double value) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(value); + status.values.push_back(key_value); + + return status; +} + void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 95d6f07cca5d9..53ef0fb99c8e3 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test [=](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[2].value); + const auto mean_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "mean"; }); + if (mean_it != it->values.end()) { + metric_value_ = boost::lexical_cast(mean_it->value); + } else { + const auto metric_value_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "metric_value"; }); + if (metric_value_it != it->values.end()) { + metric_value_ = boost::lexical_cast(metric_value_it->value); + } + } metric_updated_ = true; } }); From 98cc9f272e229a58fff4641223e53ca8859d5b14 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Fri, 24 May 2024 17:06:48 +0900 Subject: [PATCH 05/10] feat(pose_instability_detector): componentize PoseInstabilityDetector (#7114) * remove unusing main func file Signed-off-by: a-maumau * mod to componentize and use glog Signed-off-by: a-maumau * change log output from log to both Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- .../pose_instability_detector/CMakeLists.txt | 15 +++++++---- .../pose_instability_detector.launch.xml | 2 +- .../pose_instability_detector/package.xml | 1 + .../pose_instability_detector/src/main.cpp | 26 ------------------- .../src/pose_instability_detector.cpp | 5 +++- 5 files changed, 16 insertions(+), 33 deletions(-) delete mode 100644 localization/pose_instability_detector/src/main.cpp diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt index 5270df636d791..c6f94ab7df16e 100644 --- a/localization/pose_instability_detector/CMakeLists.txt +++ b/localization/pose_instability_detector/CMakeLists.txt @@ -4,14 +4,19 @@ project(pose_instability_detector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose_instability_detector - src/main.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_instability_detector.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "PoseInstabilityDetector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_pose_instability_detector + ament_auto_add_gtest(test_${PROJECT_NAME} test/test.cpp src/pose_instability_detector.cpp ) @@ -19,6 +24,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE - launch - config + launch + config ) diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml index 4a390ee2854d7..5ebe7d7e429e0 100644 --- a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml +++ b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index bf57e5589b747..d658d1c2d437f 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -22,6 +22,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tf2_geometry_msgs tier4_autoware_utils diff --git a/localization/pose_instability_detector/src/main.cpp b/localization/pose_instability_detector/src/main.cpp deleted file mode 100644 index 34e679e86730f..0000000000000 --- a/localization/pose_instability_detector/src/main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// 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 "pose_instability_detector.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto pose_instability_detector = std::make_shared(); - rclcpp::spin(pose_instability_detector); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index afb7d6e007db2..c2bce6a3db288 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -23,7 +23,7 @@ #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) -: Node("pose_instability_detector", options), +: rclcpp::Node("pose_instability_detector", options), threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), @@ -174,3 +174,6 @@ void PoseInstabilityDetector::callback_timer() prev_odometry_ = latest_odometry_; twist_buffer_.clear(); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInstabilityDetector) From 02775ebd5305094058e31910195204dbdb131396 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 24 May 2024 18:26:43 +0900 Subject: [PATCH 06/10] fix(avoidance): improve avoidance safety check logic (#6974) * fix(avoidance): don't ignore stopped object unless it's a parked vehilce Signed-off-by: satoshi-ota * fix(avoidance): rebuild prepare distance calculation/validation logic Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/helper.hpp | 17 +++++++++++++---- .../src/scene.cpp | 9 +++++---- .../src/shift_line_generator.cpp | 8 ++++---- .../src/utils.cpp | 2 +- 4 files changed, 23 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 8de39d00af130..55d14e424b7f6 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -97,16 +97,18 @@ class AvoidanceHelper return std::max(getEgoSpeed(), values.at(idx)); } - double getMinimumPrepareDistance() const + double getNominalPrepareDistance(const double velocity) const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance); + const auto & values = p->velocity_map; + const auto idx = getConstraintsMapIndex(velocity, values); + return std::max(values.at(idx) * p->max_prepare_time, p->min_prepare_distance); } double getNominalPrepareDistance() const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); + return std::max(getAvoidanceEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); } double getNominalAvoidanceDistance(const double shift_length) const @@ -298,6 +300,13 @@ class AvoidanceHelper return std::numeric_limits::max(); } + bool isEnoughPrepareDistance(const double prepare_distance) const + { + const auto & p = parameters_; + return prepare_distance > + std::max(getEgoSpeed() * p->min_prepare_distance, p->min_prepare_distance); + } + bool isComfortable(const AvoidLineArray & shift_lines) const { const auto JERK_BUFFER = 0.1; // [m/sss] @@ -328,7 +337,7 @@ class AvoidanceHelper const auto desire_shift_length = getShiftLength(object, is_object_on_right, object.avoid_margin.value()); - const auto prepare_distance = getMinimumPrepareDistance(); + const auto prepare_distance = getNominalPrepareDistance(0.0); const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 15b3e970df4b1..503438f9f7feb 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -409,9 +409,9 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const auto registered_lines = path_shifter_.getShiftLines(); if (!registered_lines.empty()) { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); - const auto to_shift_start_point = motion_utils::calcSignedArcLength( + const auto prepare_distance = motion_utils::calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); - if (to_shift_start_point < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(prepare_distance)) { RCLCPP_DEBUG( getLogger(), "Distance to shift start point is less than minimum prepare distance. The distance is not " @@ -1412,10 +1412,11 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto avoidance_distance = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto constant_distance = helper_->getFrontConstantDistance(object); + const auto prepare_distance = helper_->getNominalPrepareDistance(0.0); return object.longitudinal - std::min( - avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer, + avoidance_distance + constant_distance + prepare_distance + p->stop_buffer, p->stop_max_distance); } @@ -1444,7 +1445,7 @@ void AvoidanceModule::insertReturnDeadLine( const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); const auto min_return_distance = - helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); + helper_->getMinAvoidanceDistance(shift_length) + helper_->getNominalPrepareDistance(0.0); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 84b2b9b61a702..fc3b9e24ff9f6 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -929,7 +929,7 @@ void ShiftLineGenerator::applySmallShiftFilter( continue; } - if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(s.start_longitudinal)) { continue; } @@ -1173,13 +1173,13 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( std::max(nominal_prepare_distance - last_sl_distance, 0.0); double prepare_distance_scaled = std::max( - helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); + helper_->getNominalPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); prepare_distance_scaled = std::max( - helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); + helper_->getNominalPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1292,7 +1292,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(candidate.start_longitudinal)) { break; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 3559981fed38d..089571bc8392e 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1916,7 +1916,7 @@ std::vector getSafetyCheckTargetObjects( std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](const auto & object) { if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) { // check only moving objects - if (filtering_utils::isMovingObject(object, parameters)) { + if (filtering_utils::isMovingObject(object, parameters) || !object.is_parked) { ret.objects.push_back(object.object); } } From d5e691ec9af579ceb7a156eff4c5ea5e2e86c92e Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 24 May 2024 18:45:26 +0900 Subject: [PATCH 07/10] refactor(obstacle_cruise): refactor a function checkConsistency() (#7105) refactor Signed-off-by: Yuki Takagi --- planning/obstacle_cruise_planner/src/node.cpp | 44 +++++-------------- 1 file changed, 11 insertions(+), 33 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index f637911dae903..21e3ecb7bf758 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1378,7 +1378,6 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto current_closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - // If previous closest obstacle ptr is not set if (!prev_closest_stop_obstacle_ptr_) { if (current_closest_stop_obstacle) { prev_closest_stop_obstacle_ptr_ = @@ -1387,44 +1386,23 @@ void ObstacleCruisePlannerNode::checkConsistency( return; } - // Put previous closest target obstacle if necessary const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&](PredictedObject predicted_object) { return tier4_autoware_utils::toHexString(predicted_object.object_id) == prev_closest_stop_obstacle_ptr_->uuid; }); - - // If previous closest obstacle is not in the current perception lists - // just return the current target obstacles + // If previous closest obstacle disappear from the perception result, do nothing anymore. if (predicted_object_itr == predicted_objects.objects.end()) { return; } - // Previous closest obstacle is in the perception lists - const auto obstacle_itr = std::find_if( + const auto is_disappeared_from_stop_obstacle = std::none_of( stop_obstacles.begin(), stop_obstacles.end(), [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; }); - - // Previous closest obstacle is both in the perception lists and target obstacles - if (obstacle_itr != stop_obstacles.end()) { - if (current_closest_stop_obstacle) { - if ((current_closest_stop_obstacle->uuid == prev_closest_stop_obstacle_ptr_->uuid)) { - // prev_closest_obstacle is current_closest_stop_obstacle just return the target - // obstacles(in target obstacles) - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - // New obstacle becomes new stop obstacle - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } - } else { - // Previous closest stop obstacle becomes cruise obstacle - prev_closest_stop_obstacle_ptr_ = nullptr; - } - } else { - // prev obstacle is not in the target obstacles, but in the perception list + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); if ( predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < @@ -1433,13 +1411,13 @@ void ObstacleCruisePlannerNode::checkConsistency( stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); return; } + } - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - prev_closest_stop_obstacle_ptr_ = nullptr; - } + if (current_closest_stop_obstacle) { + prev_closest_stop_obstacle_ptr_ = + std::make_shared(*current_closest_stop_obstacle); + } else { + prev_closest_stop_obstacle_ptr_ = nullptr; } } From bf575b6ef7ee2b23ca90fe35f46dae3bfe974848 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 24 May 2024 20:41:21 +0900 Subject: [PATCH 08/10] feat(default_ad_api): use diagnostic graph (#7043) Signed-off-by: Takagi, Isamu --- .../launch/system.launch.xml | 74 ++++++++++--------- system/default_ad_api/src/operation_mode.cpp | 40 +++------- system/default_ad_api/src/operation_mode.hpp | 8 +- .../system_diagnostic_monitor.launch.xml | 2 + 4 files changed, 53 insertions(+), 71 deletions(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index a1bbfc883de7c..e5f7aa4e18b33 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -25,11 +25,12 @@ - - - - - + + + + + + @@ -57,6 +58,13 @@ + + + + + + + @@ -70,8 +78,20 @@ + + + + + + + + + + + + - + @@ -85,52 +105,34 @@ - + + - - + + + + + + - - - - - - - - - - + + + - + - - - - - - - - - - - - - - - - diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 829585ed4b8b4..d4b430ecd0579 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -39,18 +39,15 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) adaptor.init_cli(cli_mode_, group_cli_); adaptor.init_cli(cli_control_, group_cli_); - const std::vector module_names = { - "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + const auto name = "/system/operation_mode/availability"; + const auto qos = rclcpp::QoS(1); + const auto callback = [this](const OperationModeAvailability::ConstSharedPtr msg) { + mode_available_[OperationModeState::Message::STOP] = msg->stop; + mode_available_[OperationModeState::Message::AUTONOMOUS] = msg->autonomous; + mode_available_[OperationModeState::Message::LOCAL] = msg->local; + mode_available_[OperationModeState::Message::REMOTE] = msg->remote; }; - - for (size_t i = 0; i < module_names.size(); ++i) { - const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; - const auto qos = rclcpp::QoS(1).transient_local(); - const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) { - module_states_[module_names[i]] = msg->available; - }; - sub_module_states_.push_back(create_subscription(name, qos, callback)); - } + sub_availability_ = create_subscription(name, qos, callback); timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); @@ -60,8 +57,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) mode_available_[OperationModeState::Message::UNKNOWN] = false; mode_available_[OperationModeState::Message::STOP] = true; mode_available_[OperationModeState::Message::AUTONOMOUS] = false; - mode_available_[OperationModeState::Message::LOCAL] = true; - mode_available_[OperationModeState::Message::REMOTE] = true; + mode_available_[OperationModeState::Message::LOCAL] = false; + mode_available_[OperationModeState::Message::REMOTE] = false; } template @@ -135,23 +132,6 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { - bool autonomous_available = true; - std::string unhealthy_components = ""; - for (const auto & state : module_states_) { - if (!state.second) { - unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first; - } - autonomous_available &= state.second; - } - mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; - - if (!unhealthy_components.empty()) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 3000, - "%s component state is unhealthy. Autonomous is not available.", - unhealthy_components.c_str()); - } - update_state(); } diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 1830b7041b566..7cd609b5eb852 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -25,7 +25,7 @@ #include // TODO(Takagi, Isamu): define interface -#include +#include // This file should be included after messages. #include "utils/types.hpp" @@ -47,7 +47,7 @@ class OperationModeNode : public rclcpp::Node using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; - using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; + using OperationModeAvailability = tier4_system_msgs::msg::OperationModeAvailability; OperationModeState::Message curr_state_; OperationModeState::Message prev_state_; @@ -65,9 +65,7 @@ class OperationModeNode : public rclcpp::Node Sub sub_state_; Cli cli_mode_; Cli cli_control_; - - std::unordered_map module_states_; - std::vector::SharedPtr> sub_module_states_; + rclcpp::Subscription::SharedPtr sub_availability_; void on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr req, diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml index b00fcb8a26f68..8e2566a747abf 100644 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml @@ -1,6 +1,8 @@ + + From d827b1bd1f4bbacf0333eb14a62ef42e56caef25 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 24 May 2024 20:49:05 +0900 Subject: [PATCH 09/10] fix(perception_online_evaluator): revert "add metric_value not only stat (#7100)" (#7118) --- .../metrics/metric.hpp | 5 --- .../metrics_calculator.hpp | 4 +- .../perception_online_evaluator_node.hpp | 2 - .../src/metrics_calculator.cpp | 16 ++++---- .../src/perception_online_evaluator_node.cpp | 39 ++++--------------- .../test_perception_online_evaluator_node.cpp | 14 +------ 6 files changed, 19 insertions(+), 61 deletions(-) diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index eaa07f2317940..926fbb7435f3a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include namespace perception_diagnostics @@ -37,11 +36,7 @@ enum class Metric { SIZE, }; -// Each metric has a different return type. (statistic or just a one value etc). -// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; -using MetricValueMap = std::unordered_map; -using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index a04819d4844ce..96b1c01ee2d16 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricValueMap calcObjectsCountMetrics() const; + MetricStatMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index 070d938b8bd0a..e5c41ff28c4da 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -61,8 +61,6 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; - DiagnosticStatus generateDiagnosticStatus( - const std::string metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index cc455445ca9f8..9c1e0667e4fef 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -26,7 +26,7 @@ namespace perception_diagnostics using object_recognition_utils::convertLabelToString; using tier4_autoware_utils::inverseTransformPoint; -std::optional MetricsCalculator::calculate(const Metric & metric) const +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -455,14 +455,15 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const +MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricValueMap metric_stat_map; + MetricStatMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( + count); } } // calculate the average number of objects in the detection area in the past @@ -471,8 +472,8 @@ MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map - ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; + metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] + .add(count); } } @@ -480,7 +481,8 @@ MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( + count); } } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index be9ac2332f62c..0fcdd77f4d515 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -69,25 +69,16 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_result = metrics_calculator_.calculate(Metric(metric)); - if (!metric_result.has_value()) { + const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); + if (!metric_stat_map.has_value()) { continue; } - std::visit( - [&metrics_msg, this](auto && arg) { - using T = std::decay_t; - for (const auto & [metric, value] : arg) { - if constexpr (std::is_same_v) { - if (value.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); - } - } else if constexpr (std::is_same_v) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); - } - } - }, - metric_result.value()); + for (const auto & [metric, stat] : metric_stat_map.value()) { + if (stat.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); + } + } } // publish metrics @@ -120,22 +111,6 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } -DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( - const std::string metric, const double value) const -{ - DiagnosticStatus status; - - status.level = status.OK; - status.name = metric; - - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "metric_value"; - key_value.value = std::to_string(value); - status.values.push_back(key_value); - - return status; -} - void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 53ef0fb99c8e3..95d6f07cca5d9 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -141,19 +141,7 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - const auto mean_it = std::find_if( - it->values.begin(), it->values.end(), - [](const auto & key_value) { return key_value.key == "mean"; }); - if (mean_it != it->values.end()) { - metric_value_ = boost::lexical_cast(mean_it->value); - } else { - const auto metric_value_it = std::find_if( - it->values.begin(), it->values.end(), - [](const auto & key_value) { return key_value.key == "metric_value"; }); - if (metric_value_it != it->values.end()) { - metric_value_ = boost::lexical_cast(metric_value_it->value); - } - } + metric_value_ = boost::lexical_cast(it->values[2].value); metric_updated_ = true; } }); From 7b51a43ccf224c33a4f83021824d043824332bfc Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 24 May 2024 22:58:08 +0900 Subject: [PATCH 10/10] feat(default_ad_api): add heratbeat api (#6969) * feat(default_ad_api): add heratbeat api Signed-off-by: Takagi, Isamu * fix node dying Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/system.hpp | 36 ++++++++++++++++ system/default_ad_api/CMakeLists.txt | 2 + .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/src/heartbeat.cpp | 42 +++++++++++++++++++ system/default_ad_api/src/heartbeat.hpp | 40 ++++++++++++++++++ 5 files changed, 121 insertions(+) create mode 100644 common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp create mode 100644 system/default_ad_api/src/heartbeat.cpp create mode 100644 system/default_ad_api/src/heartbeat.hpp diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp new file mode 100644 index 0000000000000..09144c1d8ff50 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp @@ -0,0 +1,36 @@ +// Copyright 2024 The Autoware Contributors +// +// 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_AD_API_SPECS__SYSTEM_HPP_ +#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ + +#include + +#include + +namespace autoware_ad_api::system +{ + +struct Heartbeat +{ + using Message = autoware_adapi_v1_msgs::msg::Heartbeat; + static constexpr char name[] = "/api/system/heartbeat"; + static constexpr size_t depth = 10; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::system + +#endif // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 982112189412e..4dacc0b893714 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/fail_safe.cpp + src/heartbeat.cpp src/interface.cpp src/localization.cpp src/motion.cpp @@ -23,6 +24,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::AutowareStateNode" "default_ad_api::FailSafeNode" + "default_ad_api::HeartbeatNode" "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index cf8fbe7d001bf..f4f3986c678e9 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -43,6 +43,7 @@ def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), create_api_node("fail_safe", "FailSafeNode"), + create_api_node("heartbeat", "HeartbeatNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/default_ad_api/src/heartbeat.cpp new file mode 100644 index 0000000000000..4550302bb8bae --- /dev/null +++ b/system/default_ad_api/src/heartbeat.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "heartbeat.hpp" + +#include + +namespace default_ad_api +{ + +HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) +{ + // Move this function so that the timer no longer holds it as a reference. + const auto on_timer = [this]() { + autoware_ad_api::system::Heartbeat::Message heartbeat; + heartbeat.stamp = now(); + heartbeat.seq = ++sequence_; // Wraps at 65535. + pub_->publish(heartbeat); + }; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_); + + const auto period = rclcpp::Rate(10.0).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(on_timer)); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::HeartbeatNode) diff --git a/system/default_ad_api/src/heartbeat.hpp b/system/default_ad_api/src/heartbeat.hpp new file mode 100644 index 0000000000000..d922b88489639 --- /dev/null +++ b/system/default_ad_api/src/heartbeat.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 HEARTBEAT_HPP_ +#define HEARTBEAT_HPP_ + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class HeartbeatNode : public rclcpp::Node +{ +public: + explicit HeartbeatNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::TimerBase::SharedPtr timer_; + Pub pub_; + uint16_t sequence_ = 0; +}; + +} // namespace default_ad_api + +#endif // HEARTBEAT_HPP_