From 52db00deaefbd2bb6ceefd49b2bb291c5304e524 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Fri, 16 Aug 2024 15:01:54 +0900 Subject: [PATCH] pre-commit run -a Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../planning_evaluator_node.hpp | 5 +-- .../src/planner_manager.cpp | 36 ++++++++----------- .../src/planner_manager.hpp | 9 ++--- 3 files changed, 21 insertions(+), 29 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 7a22f3e7985e0..4a84bdd2ffcd5 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -184,7 +184,8 @@ class PlanningEvaluatorNode : public rclcpp::Node // queue for diagnostics and time stamp std::deque> diag_queue_; const std::vector target_functions_ = { - "obstacle_cruise_planner_stop", "obstacle_cruise_planner_slow_down", + "obstacle_cruise_planner_stop", + "obstacle_cruise_planner_slow_down", "obstacle_cruise_planner_cruise", "autoware::motion_velocity_planner::OutOfLaneModule.stop", "autoware::motion_velocity_planner::OutOfLaneModule.slow_down", @@ -192,7 +193,7 @@ class PlanningEvaluatorNode : public rclcpp::Node "autoware::motion_velocity_planner::ObstacleVelocityLimiterModule.slow_down", "autoware::motion_velocity_planner::DynamicObstacleStopModule.stop", "autoware::motion_velocity_planner::DynamicObstacleStopModule.slow_down", - }; + }; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index a0c0881d32a44..fad5d6691f698 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -72,8 +72,7 @@ void MotionVelocityPlannerManager::update_module_parameters( } std::shared_ptr MotionVelocityPlannerManager::make_diagnostic( - const std::string & reason, - const bool is_decided) + const std::string & reason, const bool is_decided) { auto status = std::make_shared(); status->level = status->OK; @@ -92,25 +91,19 @@ std::shared_ptr MotionVelocityPlannerManager::make_diagnostic( return status; } - void MotionVelocityPlannerManager::publish_diagnostics( - const rclcpp::Publisher::SharedPtr pub_ptr, - const rclcpp::Time & current_time, - const bool publish_decided_diagnostics_only - ) const + const rclcpp::Publisher::SharedPtr pub_ptr, const rclcpp::Time & current_time, + const bool publish_decided_diagnostics_only) const { - - if (publish_decided_diagnostics_only && - !std::any_of( - diagnostics_.begin(), - diagnostics_.end(), - [](const auto& ds_ptr) { - return ds_ptr && !ds_ptr->values.empty() && ds_ptr->values[0].key == "decision" && ds_ptr->values[0].value != "none"; - } - )) { + if ( + publish_decided_diagnostics_only && + !std::any_of(diagnostics_.begin(), diagnostics_.end(), [](const auto & ds_ptr) { + return ds_ptr && !ds_ptr->values.empty() && ds_ptr->values[0].key == "decision" && + ds_ptr->values[0].value != "none"; + })) { return; } - + DiagnosticArray diagnostics; diagnostics.header.stamp = current_time; diagnostics.header.frame_id = "map"; @@ -122,20 +115,21 @@ void MotionVelocityPlannerManager::publish_diagnostics( pub_ptr->publish(diagnostics); } - std::vector MotionVelocityPlannerManager::plan_velocities( const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { std::vector results; - for (auto & plugin : loaded_plugins_){ + for (auto & plugin : loaded_plugins_) { VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); - auto stop_reason_diag = make_diagnostic(plugin->get_module_name()+".stop", res.stop_points.size() > 0); + auto stop_reason_diag = + make_diagnostic(plugin->get_module_name() + ".stop", res.stop_points.size() > 0); diagnostics_.push_back(stop_reason_diag); - auto slow_down_reason_diag = make_diagnostic(plugin->get_module_name()+".slow_down", res.slowdown_intervals.size() > 0); + auto slow_down_reason_diag = + make_diagnostic(plugin->get_module_name() + ".slow_down", res.slowdown_intervals.size() > 0); diagnostics_.push_back(slow_down_reason_diag); } return results; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp index 46d8c47af6a8a..b29bf54a05f61 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -54,14 +54,11 @@ class MotionVelocityPlannerManager // Diagnostic std::shared_ptr make_diagnostic( - const std::string & reason, - const bool is_decided = true); + const std::string & reason, const bool is_decided = true); void publish_diagnostics( - const rclcpp::Publisher::SharedPtr pub_ptr, - const rclcpp::Time & current_time, + const rclcpp::Publisher::SharedPtr pub_ptr, const rclcpp::Time & current_time, const bool publish_decided_diagnostics_only = true) const; - void clear_diagnostics(){diagnostics_.clear();} - + void clear_diagnostics() { diagnostics_.clear(); } private: std::vector> diagnostics_;