From e03b92693f89a0dedac7bed4e1aea69252e626fe Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 27 Mar 2024 16:07:01 +0100 Subject: [PATCH 01/53] build(behavior_velocity_planner): prefix package and namespace with autoware_ Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/CMakeLists.txt | 4 ++-- .../launch/behavior_velocity_planner.launch.xml | 2 +- planning/behavior_velocity_planner/package.xml | 4 ++-- planning/behavior_velocity_planner/src/node.cpp | 5 ++++- planning/behavior_velocity_planner/src/node.hpp | 7 +++++-- planning/behavior_velocity_planner/src/planner_manager.cpp | 7 +++++-- planning/behavior_velocity_planner/src/planner_manager.hpp | 3 +++ .../test/src/test_node_interface.cpp | 2 +- 8 files changed, 23 insertions(+), 11 deletions(-) diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index ee7dd7c07a77a..37a02b844dfe9 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_planner) +project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -19,7 +19,7 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_lib - PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" + PLUGIN "autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index c1631fdb739de..2b8431d17dfee 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -30,7 +30,7 @@ - + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 1357b555f0ba4..573d862f1725b 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_planner + autoware_behavior_velocity_planner 0.1.0 - The behavior_velocity_planner package + The autoware_behavior_velocity_planner package Mamoru Sobue Takayuki Murooka diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 1801db4658a72..d981224e772bc 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -52,6 +52,8 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) } } // namespace +namespace autoware +{ namespace behavior_velocity_planner { namespace @@ -478,6 +480,7 @@ void BehaviorVelocityPlannerNode::publishDebugMarker( debug_viz_pub_->publish(output_msg); } } // namespace behavior_velocity_planner +} // namespace autoware #include -RCLCPP_COMPONENTS_REGISTER_NODE(behavior_velocity_planner::BehaviorVelocityPlannerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode) diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index dd6edf6ae0bc0..20fac2feb4928 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -44,11 +44,13 @@ #include #include +namespace autoware +{ namespace behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; -using behavior_velocity_planner::srv::LoadPlugin; -using behavior_velocity_planner::srv::UnloadPlugin; +using autoware::behavior_velocity_planner::srv::LoadPlugin; +using autoware::behavior_velocity_planner::srv::UnloadPlugin; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node @@ -135,5 +137,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; } // namespace behavior_velocity_planner +} // namespace autoware #endif // NODE_HPP_ diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index cb491920c87d4..a646b66c727f3 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -19,6 +19,8 @@ #include #include +namespace autoware +{ namespace behavior_velocity_planner { namespace @@ -50,7 +52,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") +: plugin_loader_("behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") { } @@ -82,7 +84,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr plugin) { + [&](const std::shared_ptr plugin) { return plugin->getModuleName() == name; }); @@ -129,3 +131,4 @@ diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopRe return stop_reason_diag_; } } // namespace behavior_velocity_planner +} // namespace autoware diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index 7485e5faba8bf..02c343f82a7b9 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -36,6 +36,8 @@ #include #include +namespace autoware +{ namespace behavior_velocity_planner { class BehaviorVelocityPlannerManager @@ -57,5 +59,6 @@ class BehaviorVelocityPlannerManager std::vector> scene_manager_plugins_; }; } // namespace behavior_velocity_planner +} // namespace autoware #endif // PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index f70259b6a1f80..c65f76dc2ebb0 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -23,7 +23,7 @@ #include #include -using behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() From 3edb0c284633e9d1bc70e7206cdce5587f535153 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 27 Mar 2024 15:11:03 +0000 Subject: [PATCH 02/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.hpp | 2 +- planning/behavior_velocity_planner/src/planner_manager.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 20fac2feb4928..599858a07c0e1 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -48,9 +48,9 @@ namespace autoware { namespace behavior_velocity_planner { -using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware::behavior_velocity_planner::srv::LoadPlugin; using autoware::behavior_velocity_planner::srv::UnloadPlugin; +using autoware_auto_mapping_msgs::msg::HADMapBin; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index a646b66c727f3..39dc3fc496b88 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -52,7 +52,8 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") +: plugin_loader_( + "behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") { } From 76b8f4a94e06c57dd87ea45d4bf36262afc7d79f Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 9 Apr 2024 15:19:13 +0200 Subject: [PATCH 03/53] build(autoware_behavior_velocity_planner): fix namespace Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 6 +++--- planning/behavior_velocity_planner/src/node.hpp | 10 ++++++---- .../behavior_velocity_planner/src/planner_manager.hpp | 3 +++ 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index d981224e772bc..6e3f58c16728e 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -438,14 +438,14 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation const auto interpolated_path_msg = - interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); + ::behavior_velocity_planner::interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = filterStopPathPoint(interpolated_path_msg); + output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 599858a07c0e1..3814562f11cca 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -18,8 +18,8 @@ #include "planner_manager.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include +#include +#include #include #include #include @@ -48,10 +48,12 @@ namespace autoware { namespace behavior_velocity_planner { -using autoware::behavior_velocity_planner::srv::LoadPlugin; -using autoware::behavior_velocity_planner::srv::UnloadPlugin; using autoware_auto_mapping_msgs::msg::HADMapBin; using tier4_planning_msgs::msg::VelocityLimit; +using ::behavior_velocity_planner::PlannerData; +using autoware_behavior_velocity_planner::srv::LoadPlugin; +using autoware_behavior_velocity_planner::srv::UnloadPlugin; +using ::behavior_velocity_planner::TrafficSignalStamped; class BehaviorVelocityPlannerNode : public rclcpp::Node { diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index 02c343f82a7b9..eb102c5b3f71a 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -40,6 +40,9 @@ namespace autoware { namespace behavior_velocity_planner { +using ::behavior_velocity_planner::PlannerData; +using ::behavior_velocity_planner::PluginInterface; + class BehaviorVelocityPlannerManager { public: From c9e7510e323fcb20abf6a0e5ae876cbc31bdd7e2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 9 Apr 2024 13:21:04 +0000 Subject: [PATCH 04/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 7 ++++--- planning/behavior_velocity_planner/src/node.hpp | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 6e3f58c16728e..f11718b48434e 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -438,11 +438,12 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = + ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = - ::behavior_velocity_planner::interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); + const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 3814562f11cca..a7815a9dac35a 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -49,11 +49,11 @@ namespace autoware namespace behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; -using tier4_planning_msgs::msg::VelocityLimit; -using ::behavior_velocity_planner::PlannerData; using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; +using ::behavior_velocity_planner::PlannerData; using ::behavior_velocity_planner::TrafficSignalStamped; +using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node { From 3a3bff26693811d3d8d85e1e8f5a14ba7d2d2408 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 9 Apr 2024 16:49:23 +0200 Subject: [PATCH 05/53] fix(autoware_behavior_velocity_planner): fix tests Signed-off-by: Esteve Fernandez --- .../launch/behavior_velocity_planner.launch.xml | 4 ++-- planning/behavior_velocity_planner/src/planner_manager.cpp | 2 +- .../test/src/test_node_interface.cpp | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 2b8431d17dfee..81021728c5ec1 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -28,9 +28,9 @@ - + - + diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 39dc3fc496b88..c7e068a35124e 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -53,7 +53,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() : plugin_loader_( - "behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") + "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") { } diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index c65f76dc2ebb0..21a6e543f3d3f 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -50,7 +50,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("behavior_velocity_planner"); + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); const auto motion_velocity_smoother_dir = ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); @@ -153,6 +153,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) // test with empty path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); } From 8d4ded961a4f72143d1c5e3e3a45dda145ddf24d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 10 Apr 2024 12:55:53 +0200 Subject: [PATCH 06/53] fix(autoware_behavior_velocity_planner): fix clang-tidy issues Signed-off-by: Esteve Fernandez --- .../behavior_velocity_planner/src/node.cpp | 29 +++++++++---------- .../behavior_velocity_planner/src/node.hpp | 4 +-- .../src/planner_manager.cpp | 25 +++++++--------- .../test/src/test_node_interface.cpp | 22 +++++++------- 4 files changed, 37 insertions(+), 43 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index f11718b48434e..567f19944f69c 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -40,7 +40,7 @@ namespace { -rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node_ptr) { rclcpp::CallbackGroup::SharedPtr callback_group = node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -52,9 +52,7 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) } } // namespace -namespace autoware -{ -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -83,33 +81,33 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio trigger_sub_path_with_lane_id_ = this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); // Subscribers sub_predicted_objects_ = this->create_subscription( "~/input/dynamic_objects", 1, std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_no_ground_pointcloud_ = this->create_subscription( "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), std::bind(&BehaviorVelocityPlannerNode::onNoGroundPointCloud, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_vehicle_odometry_ = this->create_subscription( "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onOdometry, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_traffic_signals_ = this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); @@ -117,10 +115,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->create_subscription( "~/input/virtual_traffic_light_states", 1, std::bind(&BehaviorVelocityPlannerNode::onVirtualTrafficLightStates, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_occupancy_grid_ = this->create_subscription( "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); @@ -154,7 +152,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. - if (name == "") { + if (name.empty()) { break; } planner_manager_.launchScenePlugin(*this, name); @@ -182,7 +180,7 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin( // NOTE: argument planner_data must not be referenced for multithreading bool BehaviorVelocityPlannerNode::isDataReady( - const PlannerData planner_data, rclcpp::Clock clock) const + const PlannerData& planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; @@ -480,7 +478,6 @@ void BehaviorVelocityPlannerNode::publishDebugMarker( } debug_viz_pub_->publish(output_msg); } -} // namespace behavior_velocity_planner } // namespace autoware #include diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index a7815a9dac35a..55c2b2a4f4619 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -114,7 +114,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; HADMapBin::ConstSharedPtr map_ptr_{nullptr}; - bool has_received_map_; + bool has_received_map_{}; rclcpp::Service::SharedPtr srv_load_plugin_; rclcpp::Service::SharedPtr srv_unload_plugin_; @@ -129,7 +129,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped getCurrentPose(); - bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const; + bool isDataReady(const PlannerData& planner_data, rclcpp::Clock clock) const; autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index c7e068a35124e..bb98a3c1efd72 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -19,13 +19,11 @@ #include #include -namespace autoware -{ -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) +std::string json_dumps_pose(const geometry_msgs::msg::Pose & pose) { const std::string json_dumps_pose = (boost::format( @@ -36,8 +34,8 @@ std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) return json_dumps_pose; } -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) +diagnostic_msgs::msg::DiagnosticStatus make_stop_reason_diag( + const std::string& stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; @@ -45,7 +43,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( stop_reason_diag.name = "stop_reason"; stop_reason_diag.message = stop_reason; stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); + stop_reason_diag_kv.value = json_dumps_pose(stop_pose); stop_reason_diag.values.push_back(stop_reason_diag_kv); return stop_reason_diag; } @@ -85,7 +83,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr plugin) { + [&](const std::shared_ptr& plugin) { return plugin->getModuleName() == name; }); @@ -111,17 +109,17 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); plugin->plan(&output_path_msg); - const auto firstStopPathPointIndex = plugin->getFirstStopPathPointIndex(); + const auto first_stop_path_point_index = plugin->getFirstStopPathPointIndex(); - if (firstStopPathPointIndex) { - if (firstStopPathPointIndex.value() < first_stop_path_point_index) { - first_stop_path_point_index = firstStopPathPointIndex.value(); + if (first_stop_path_point_index) { + if (first_stop_path_point_index.value() < first_stop_path_point_index) { + first_stop_path_point_index = first_stop_path_point_index.value(); stop_reason_msg = plugin->getModuleName(); } } } - stop_reason_diag_ = makeStopReasonDiag( + stop_reason_diag_ = make_stop_reason_diag( stop_reason_msg, output_path_msg.points[first_stop_path_point_index].point.pose); return output_path_msg; @@ -131,5 +129,4 @@ diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopRe { return stop_reason_diag_; } -} // namespace behavior_velocity_planner } // namespace autoware diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 21a6e543f3d3f..8bb568ce89a6c 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -26,7 +26,7 @@ using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; -std::shared_ptr generateTestManager() +std::shared_ptr generate_test_manager() { auto test_manager = std::make_shared(); @@ -43,7 +43,7 @@ std::shared_ptr generateTestManager() return test_manager; } -std::shared_ptr generateNode() +std::shared_ptr generate_node() { auto node_options = rclcpp::NodeOptions{}; @@ -114,9 +114,9 @@ std::shared_ptr generateNode() return std::make_shared(node_options); } -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) +void publish_mandatory_topics( + const std::shared_ptr& test_manager, + const std::shared_ptr& test_target_node) { // publish necessary topics from test_manager test_manager->publishTF(test_target_node, "/tf"); @@ -142,10 +142,10 @@ void publishMandatoryTopics( TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_manager = generate_test_manager(); + auto test_target_node = generate_node(); - publishMandatoryTopics(test_manager, test_target_node); + publish_mandatory_topics(test_manager, test_target_node); // test with nominal path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); @@ -161,9 +161,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - publishMandatoryTopics(test_manager, test_target_node); + auto test_manager = generate_test_manager(); + auto test_target_node = generate_node(); + publish_mandatory_topics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); From cb6abfeaa9c6150b7ea7cbdd1a193a87392ed482 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 10 Apr 2024 10:57:53 +0000 Subject: [PATCH 07/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 4 ++-- planning/behavior_velocity_planner/src/node.hpp | 2 +- planning/behavior_velocity_planner/src/planner_manager.cpp | 6 +++--- .../test/src/test_node_interface.cpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 567f19944f69c..063991d479903 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -180,7 +180,7 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin( // NOTE: argument planner_data must not be referenced for multithreading bool BehaviorVelocityPlannerNode::isDataReady( - const PlannerData& planner_data, rclcpp::Clock clock) const + const PlannerData & planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; @@ -478,7 +478,7 @@ void BehaviorVelocityPlannerNode::publishDebugMarker( } debug_viz_pub_->publish(output_msg); } -} // namespace autoware +} // namespace autoware::behavior_velocity_planner #include RCLCPP_COMPONENTS_REGISTER_NODE(autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode) diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 55c2b2a4f4619..b343a3ee5afc9 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -129,7 +129,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped getCurrentPose(); - bool isDataReady(const PlannerData& planner_data, rclcpp::Clock clock) const; + bool isDataReady(const PlannerData & planner_data, rclcpp::Clock clock) const; autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index bb98a3c1efd72..ba2855d1a39b3 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -35,7 +35,7 @@ std::string json_dumps_pose(const geometry_msgs::msg::Pose & pose) } diagnostic_msgs::msg::DiagnosticStatus make_stop_reason_diag( - const std::string& stop_reason, const geometry_msgs::msg::Pose & stop_pose) + const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; @@ -83,7 +83,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr& plugin) { + [&](const std::shared_ptr & plugin) { return plugin->getModuleName() == name; }); @@ -129,4 +129,4 @@ diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopRe { return stop_reason_diag_; } -} // namespace autoware +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 8bb568ce89a6c..b63d4e25d3e1e 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -115,8 +115,8 @@ std::shared_ptr generate_node() } void publish_mandatory_topics( - const std::shared_ptr& test_manager, - const std::shared_ptr& test_target_node) + const std::shared_ptr & test_manager, + const std::shared_ptr & test_target_node) { // publish necessary topics from test_manager test_manager->publishTF(test_target_node, "/tf"); From 4947bc5b4af1e40bba2805e5063caaac613568ff Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 16:38:40 +0200 Subject: [PATCH 08/53] fix(autoware_behavior_velocity_planner): fix build errors Signed-off-by: Esteve Fernandez --- .../behavior_velocity_planner/src/node.cpp | 31 +++++++++---------- .../behavior_velocity_planner/src/node.hpp | 4 +-- .../src/planner_manager.cpp | 24 +++++++------- .../test/src/test_node_interface.cpp | 22 ++++++------- 4 files changed, 40 insertions(+), 41 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 063991d479903..3105a22927ad3 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -40,7 +40,7 @@ namespace { -rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node_ptr) +rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) { rclcpp::CallbackGroup::SharedPtr callback_group = node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -81,33 +81,33 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio trigger_sub_path_with_lane_id_ = this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); // Subscribers sub_predicted_objects_ = this->create_subscription( "~/input/dynamic_objects", 1, std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); sub_no_ground_pointcloud_ = this->create_subscription( "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), std::bind(&BehaviorVelocityPlannerNode::onNoGroundPointCloud, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); sub_vehicle_odometry_ = this->create_subscription( "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onOdometry, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); sub_traffic_signals_ = this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); @@ -115,10 +115,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->create_subscription( "~/input/virtual_traffic_light_states", 1, std::bind(&BehaviorVelocityPlannerNode::onVirtualTrafficLightStates, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); sub_occupancy_grid_ = this->create_subscription( "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); @@ -152,7 +152,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. - if (name.empty()) { + if (name == "") { break; } planner_manager_.launchScenePlugin(*this, name); @@ -180,7 +180,7 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin( // NOTE: argument planner_data must not be referenced for multithreading bool BehaviorVelocityPlannerNode::isDataReady( - const PlannerData & planner_data, rclcpp::Clock clock) const + const PlannerData planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; @@ -436,12 +436,11 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = - ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( - filtered_path, forward_path_length_, behavior_output_path_interval_); + const auto interpolated_path_msg = + ::behavior_velocity_planner::interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index b343a3ee5afc9..a7815a9dac35a 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -114,7 +114,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; HADMapBin::ConstSharedPtr map_ptr_{nullptr}; - bool has_received_map_{}; + bool has_received_map_; rclcpp::Service::SharedPtr srv_load_plugin_; rclcpp::Service::SharedPtr srv_unload_plugin_; @@ -129,7 +129,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped getCurrentPose(); - bool isDataReady(const PlannerData & planner_data, rclcpp::Clock clock) const; + bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const; autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index ba2855d1a39b3..ce5b18754ac24 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -23,19 +23,19 @@ namespace autoware::behavior_velocity_planner { namespace { -std::string json_dumps_pose(const geometry_msgs::msg::Pose & pose) +std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) { - const std::string json_dumps_pose = + const std::string jsonDumpsPose = (boost::format( R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % pose.orientation.y % pose.orientation.z) .str(); - return json_dumps_pose; + return jsonDumpsPose; } -diagnostic_msgs::msg::DiagnosticStatus make_stop_reason_diag( - const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) +diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( + const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; @@ -43,7 +43,7 @@ diagnostic_msgs::msg::DiagnosticStatus make_stop_reason_diag( stop_reason_diag.name = "stop_reason"; stop_reason_diag.message = stop_reason; stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = json_dumps_pose(stop_pose); + stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); stop_reason_diag.values.push_back(stop_reason_diag_kv); return stop_reason_diag; } @@ -83,7 +83,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr & plugin) { + [&](const std::shared_ptr plugin) { return plugin->getModuleName() == name; }); @@ -109,17 +109,17 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); plugin->plan(&output_path_msg); - const auto first_stop_path_point_index = plugin->getFirstStopPathPointIndex(); + const auto firstStopPathPointIndex = plugin->getFirstStopPathPointIndex(); - if (first_stop_path_point_index) { - if (first_stop_path_point_index.value() < first_stop_path_point_index) { - first_stop_path_point_index = first_stop_path_point_index.value(); + if (firstStopPathPointIndex) { + if (firstStopPathPointIndex.value() < first_stop_path_point_index) { + first_stop_path_point_index = firstStopPathPointIndex.value(); stop_reason_msg = plugin->getModuleName(); } } } - stop_reason_diag_ = make_stop_reason_diag( + stop_reason_diag_ = makeStopReasonDiag( stop_reason_msg, output_path_msg.points[first_stop_path_point_index].point.pose); return output_path_msg; diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index b63d4e25d3e1e..21a6e543f3d3f 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -26,7 +26,7 @@ using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; -std::shared_ptr generate_test_manager() +std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); @@ -43,7 +43,7 @@ std::shared_ptr generate_test_manager() return test_manager; } -std::shared_ptr generate_node() +std::shared_ptr generateNode() { auto node_options = rclcpp::NodeOptions{}; @@ -114,9 +114,9 @@ std::shared_ptr generate_node() return std::make_shared(node_options); } -void publish_mandatory_topics( - const std::shared_ptr & test_manager, - const std::shared_ptr & test_target_node) +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) { // publish necessary topics from test_manager test_manager->publishTF(test_target_node, "/tf"); @@ -142,10 +142,10 @@ void publish_mandatory_topics( TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) { rclcpp::init(0, nullptr); - auto test_manager = generate_test_manager(); - auto test_target_node = generate_node(); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - publish_mandatory_topics(test_manager, test_target_node); + publishMandatoryTopics(test_manager, test_target_node); // test with nominal path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); @@ -161,9 +161,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) { rclcpp::init(0, nullptr); - auto test_manager = generate_test_manager(); - auto test_target_node = generate_node(); - publish_mandatory_topics(test_manager, test_target_node); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); From e1d4e433ee447a679dadbc42ffc24185323062a6 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 16:51:51 +0200 Subject: [PATCH 09/53] fix(autoware_behavior_velocity_planner): fix some clang-tidy errors Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 4 ++-- planning/behavior_velocity_planner/src/node.hpp | 9 +++------ .../behavior_velocity_planner/src/planner_manager.cpp | 2 +- .../behavior_velocity_planner/src/planner_manager.hpp | 5 +---- 4 files changed, 7 insertions(+), 13 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 3105a22927ad3..7c811365dcd25 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -152,7 +152,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. - if (name == "") { + if (name.empty()) { break; } planner_manager_.launchScenePlugin(*this, name); @@ -180,7 +180,7 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin( // NOTE: argument planner_data must not be referenced for multithreading bool BehaviorVelocityPlannerNode::isDataReady( - const PlannerData planner_data, rclcpp::Clock clock) const + const PlannerData& planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index a7815a9dac35a..34d6ade19d248 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -44,9 +44,7 @@ #include #include -namespace autoware -{ -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_behavior_velocity_planner::srv::LoadPlugin; @@ -114,7 +112,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; HADMapBin::ConstSharedPtr map_ptr_{nullptr}; - bool has_received_map_; + bool has_received_map_{}; rclcpp::Service::SharedPtr srv_load_plugin_; rclcpp::Service::SharedPtr srv_unload_plugin_; @@ -129,7 +127,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped getCurrentPose(); - bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const; + bool isDataReady(const PlannerData & planner_data, rclcpp::Clock clock) const; autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); @@ -138,7 +136,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace behavior_velocity_planner } // namespace autoware #endif // NODE_HPP_ diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index ce5b18754ac24..9682453540b0b 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -83,7 +83,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr plugin) { + [&](const std::shared_ptr& plugin) { return plugin->getModuleName() == name; }); diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index eb102c5b3f71a..c48294c9c1e63 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -36,9 +36,7 @@ #include #include -namespace autoware -{ -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using ::behavior_velocity_planner::PlannerData; using ::behavior_velocity_planner::PluginInterface; @@ -61,7 +59,6 @@ class BehaviorVelocityPlannerManager pluginlib::ClassLoader plugin_loader_; std::vector> scene_manager_plugins_; }; -} // namespace behavior_velocity_planner } // namespace autoware #endif // PLANNER_MANAGER_HPP_ From f6c35a9795406463dcd2b66548d644ddacb12405 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 12 Apr 2024 14:55:24 +0000 Subject: [PATCH 10/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 9 +++++---- planning/behavior_velocity_planner/src/node.hpp | 2 +- .../behavior_velocity_planner/src/planner_manager.cpp | 2 +- .../behavior_velocity_planner/src/planner_manager.hpp | 2 +- 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 7c811365dcd25..06d4abd08811d 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -180,7 +180,7 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin( // NOTE: argument planner_data must not be referenced for multithreading bool BehaviorVelocityPlannerNode::isDataReady( - const PlannerData& planner_data, rclcpp::Clock clock) const + const PlannerData & planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; @@ -436,11 +436,12 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = + ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = - ::behavior_velocity_planner::interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); + const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 34d6ade19d248..c4d903001cb4f 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -136,6 +136,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace autoware +} // namespace autoware::behavior_velocity_planner #endif // NODE_HPP_ diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 9682453540b0b..9d581f8609749 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -83,7 +83,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr& plugin) { + [&](const std::shared_ptr & plugin) { return plugin->getModuleName() == name; }); diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index c48294c9c1e63..f9c1d0253de62 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -59,6 +59,6 @@ class BehaviorVelocityPlannerManager pluginlib::ClassLoader plugin_loader_; std::vector> scene_manager_plugins_; }; -} // namespace autoware +} // namespace autoware::behavior_velocity_planner #endif // PLANNER_MANAGER_HPP_ From 36698c570d32ff73a1d843e9553b990aafed1e14 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 16:58:47 +0200 Subject: [PATCH 11/53] fix(autoware_behavior_velocity_planner): fix some clang-tidy errors Signed-off-by: Esteve Fernandez --- .../include/behavior_velocity_planner_common/planner_data.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 1511d430ddd0c..2fed958640ae8 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -47,7 +47,6 @@ namespace behavior_velocity_planner { -class BehaviorVelocityPlannerNode; struct PlannerData { explicit PlannerData(rclcpp::Node & node) From 579bb94b0a7cf4b6564a96d51a9af9844b6ba90b Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:03:27 +0200 Subject: [PATCH 12/53] fix(autoware_behavior_velocity_planner): fix some clang-tidy errors Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 2 ++ planning/behavior_velocity_planner/src/node.hpp | 1 - planning/behavior_velocity_planner/src/planner_manager.hpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 06d4abd08811d..1ccf386abcba4 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -54,6 +54,8 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::TrafficSignalStamped; + namespace { diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index c4d903001cb4f..2392aef24bdca 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -50,7 +50,6 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index f9c1d0253de62..448210116e56b 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -52,7 +52,7 @@ class BehaviorVelocityPlannerManager const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg); - diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; + [[nodiscard]] diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; private: diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; From 959c1e663e9dc20e52cc9be9437e173c5edc085f Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:08:53 +0200 Subject: [PATCH 13/53] fix(autoware_behavior_velocity_planner): fix some clang-tidy errors Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/planner_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 9d581f8609749..27671080f8ae9 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -35,7 +35,7 @@ std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) } diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) + const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; From 7b451c0e1d760eebb2159e1c16c7fe8ea216e15d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:11:14 +0200 Subject: [PATCH 14/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 8 ++++---- planning/behavior_velocity_planner/src/node.hpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 1ccf386abcba4..aa85f794378e2 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -82,14 +82,14 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Trigger Subscriber trigger_sub_path_with_lane_id_ = this->create_subscription( - "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1), + "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::on_trigger, this, _1), createSubscriptionOptions(this)); // Subscribers sub_predicted_objects_ = this->create_subscription( "~/input/dynamic_objects", 1, - std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1), + std::bind(&BehaviorVelocityPlannerNode::on_predicted_objects, this, _1), createSubscriptionOptions(this)); sub_no_ground_pointcloud_ = this->create_subscription( "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), @@ -232,7 +232,7 @@ void BehaviorVelocityPlannerNode::onOccupancyGrid( planner_data_.occupancy_grid = msg; } -void BehaviorVelocityPlannerNode::onPredictedObjects( +void BehaviorVelocityPlannerNode::on_predicted_objects( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -374,7 +374,7 @@ void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( planner_data_.virtual_traffic_light_states = msg; } -void BehaviorVelocityPlannerNode::onTrigger( +void BehaviorVelocityPlannerNode::on_trigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { std::unique_lock lk(mutex_); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 2392aef24bdca..1c3fe41544498 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -78,9 +78,9 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; - void onTrigger( + void on_trigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); - void onPredictedObjects( + void on_predicted_objects( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); From 4ff635147a782094a10227ab9586321b79593af1 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:14:20 +0200 Subject: [PATCH 15/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 8 ++++---- planning/behavior_velocity_planner/src/node.hpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index aa85f794378e2..eee66e05c30a2 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -93,7 +93,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio createSubscriptionOptions(this)); sub_no_ground_pointcloud_ = this->create_subscription( "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&BehaviorVelocityPlannerNode::onNoGroundPointCloud, this, _1), + std::bind(&BehaviorVelocityPlannerNode::on_no_ground_point_cloud, this, _1), createSubscriptionOptions(this)); sub_vehicle_odometry_ = this->create_subscription( "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onOdometry, this, _1), @@ -116,7 +116,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio sub_virtual_traffic_light_states_ = this->create_subscription( "~/input/virtual_traffic_light_states", 1, - std::bind(&BehaviorVelocityPlannerNode::onVirtualTrafficLightStates, this, _1), + std::bind(&BehaviorVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), createSubscriptionOptions(this)); sub_occupancy_grid_ = this->create_subscription( "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), @@ -239,7 +239,7 @@ void BehaviorVelocityPlannerNode::on_predicted_objects( planner_data_.predicted_objects = msg; } -void BehaviorVelocityPlannerNode::onNoGroundPointCloud( +void BehaviorVelocityPlannerNode::on_no_ground_point_cloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { geometry_msgs::msg::TransformStamped transform; @@ -367,7 +367,7 @@ void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::C planner_data_.external_velocity_limit = *msg; } -void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( +void BehaviorVelocityPlannerNode::on_virtual_traffic_light_states( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 1c3fe41544498..1cbec2191ced1 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -82,13 +82,13 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); void on_predicted_objects( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); - void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void on_no_ground_point_cloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); - void onVirtualTrafficLightStates( + void on_virtual_traffic_light_states( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); From 8f80c8530a0cb0ef86b58f94d9041103abb1bb46 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:21:59 +0200 Subject: [PATCH 16/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- .../behavior_velocity_planner/src/node.cpp | 40 +++++++++---------- .../behavior_velocity_planner/src/node.hpp | 10 ++--- 2 files changed, 25 insertions(+), 25 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index eee66e05c30a2..e93b982d09b9c 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -40,7 +40,7 @@ namespace { -rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node_ptr) { rclcpp::CallbackGroup::SharedPtr callback_group = node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -83,33 +83,33 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio trigger_sub_path_with_lane_id_ = this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::on_trigger, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); // Subscribers sub_predicted_objects_ = this->create_subscription( "~/input/dynamic_objects", 1, std::bind(&BehaviorVelocityPlannerNode::on_predicted_objects, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_no_ground_pointcloud_ = this->create_subscription( "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), std::bind(&BehaviorVelocityPlannerNode::on_no_ground_point_cloud, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_vehicle_odometry_ = this->create_subscription( - "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onOdometry, this, _1), - createSubscriptionOptions(this)); + "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::on_odometry, this, _1), + create_subscription_options(this)); sub_acceleration_ = this->create_subscription( - "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), - createSubscriptionOptions(this)); + "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::on_acceleration, this, _1), + create_subscription_options(this)); sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), - std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), - createSubscriptionOptions(this)); + std::bind(&BehaviorVelocityPlannerNode::on_lanelet_map, this, _1), + create_subscription_options(this)); sub_traffic_signals_ = this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); @@ -117,10 +117,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->create_subscription( "~/input/virtual_traffic_light_states", 1, std::bind(&BehaviorVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), - createSubscriptionOptions(this)); + create_subscription_options(this)); sub_occupancy_grid_ = this->create_subscription( - "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), - createSubscriptionOptions(this)); + "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::on_occupancy_grid, this, _1), + create_subscription_options(this)); srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); @@ -181,7 +181,7 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin( } // NOTE: argument planner_data must not be referenced for multithreading -bool BehaviorVelocityPlannerNode::isDataReady( +bool BehaviorVelocityPlannerNode::is_data_ready( const PlannerData & planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; @@ -225,7 +225,7 @@ bool BehaviorVelocityPlannerNode::isDataReady( return true; } -void BehaviorVelocityPlannerNode::onOccupancyGrid( +void BehaviorVelocityPlannerNode::on_occupancy_grid( const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -266,7 +266,7 @@ void BehaviorVelocityPlannerNode::on_no_ground_point_cloud( } } -void BehaviorVelocityPlannerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -299,7 +299,7 @@ void BehaviorVelocityPlannerNode::onOdometry(const nav_msgs::msg::Odometry::Cons } } -void BehaviorVelocityPlannerNode::onAcceleration( +void BehaviorVelocityPlannerNode::on_acceleration( const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -316,7 +316,7 @@ void BehaviorVelocityPlannerNode::onParam() planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } -void BehaviorVelocityPlannerNode::onLaneletMap( +void BehaviorVelocityPlannerNode::on_lanelet_map( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -379,7 +379,7 @@ void BehaviorVelocityPlannerNode::on_trigger( { std::unique_lock lk(mutex_); - if (!isDataReady(planner_data_, *get_clock())) { + if (!is_data_ready(planner_data_, *get_clock())) { return; } diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 1cbec2191ced1..fc75ad59f550e 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -83,14 +83,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void on_predicted_objects( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void on_no_ground_point_cloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); - void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + void on_lanelet_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); void on_virtual_traffic_light_states( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); - void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); + void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); void onParam(); @@ -126,7 +126,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped getCurrentPose(); - bool isDataReady(const PlannerData & planner_data, rclcpp::Clock clock) const; + bool is_data_ready(const PlannerData & planner_data, rclcpp::Clock clock) const; autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); From 0b3b8173e3027887976ecffc71f567419a50907b Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:23:52 +0200 Subject: [PATCH 17/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 16 ++++++++-------- planning/behavior_velocity_planner/src/node.hpp | 8 ++++---- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index e93b982d09b9c..e149666c0c0bb 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -108,11 +108,11 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio sub_traffic_signals_ = this->create_subscription( "~/input/traffic_signals", 1, - std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), + std::bind(&BehaviorVelocityPlannerNode::on_traffic_signals, this, _1), create_subscription_options(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), - std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); + std::bind(&BehaviorVelocityPlannerNode::on_external_velocity_limit, this, _1)); sub_virtual_traffic_light_states_ = this->create_subscription( "~/input/virtual_traffic_light_states", 1, @@ -126,7 +126,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); srv_unload_plugin_ = create_service( "~/service/unload_plugin", - std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); + std::bind(&BehaviorVelocityPlannerNode::on_unload_plugin, this, _1, _2)); // set velocity smoother param onParam(); @@ -172,7 +172,7 @@ void BehaviorVelocityPlannerNode::onLoadPlugin( planner_manager_.launchScenePlugin(*this, request->plugin_name); } -void BehaviorVelocityPlannerNode::onUnloadPlugin( +void BehaviorVelocityPlannerNode::on_unload_plugin( const UnloadPlugin::Request::SharedPtr request, [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) { @@ -325,7 +325,7 @@ void BehaviorVelocityPlannerNode::on_lanelet_map( has_received_map_ = true; } -void BehaviorVelocityPlannerNode::onTrafficSignals( +void BehaviorVelocityPlannerNode::on_traffic_signals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -361,7 +361,7 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( } } -void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::on_external_velocity_limit(const VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.external_velocity_limit = *msg; @@ -408,7 +408,7 @@ void BehaviorVelocityPlannerNode::on_trigger( stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); if (debug_viz_pub_->get_subscription_count() > 0) { - publishDebugMarker(output_path_msg); + publish_debug_marker(output_path_msg); } } @@ -458,7 +458,7 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath return output_path_msg; } -void BehaviorVelocityPlannerNode::publishDebugMarker( +void BehaviorVelocityPlannerNode::publish_debug_marker( const autoware_auto_planning_msgs::msg::Path & path) { visualization_msgs::msg::MarkerArray output_msg; diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index fc75ad59f550e..039bfbefd8254 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -86,12 +86,12 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); void on_lanelet_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); - void onTrafficSignals( + void on_traffic_signals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); void on_virtual_traffic_light_states( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); - void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); + void on_external_velocity_limit(const VelocityLimit::ConstSharedPtr msg); void onParam(); // publisher @@ -99,7 +99,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - void publishDebugMarker(const autoware_auto_planning_msgs::msg::Path & path); + void publish_debug_marker(const autoware_auto_planning_msgs::msg::Path & path); // parameter double forward_path_length_; @@ -115,7 +115,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Service::SharedPtr srv_load_plugin_; rclcpp::Service::SharedPtr srv_unload_plugin_; - void onUnloadPlugin( + void on_unload_plugin( const UnloadPlugin::Request::SharedPtr request, const UnloadPlugin::Response::SharedPtr response); void onLoadPlugin( From 400bc8ea60610d676102d788577dd6c4db602ef7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 12 Apr 2024 15:24:02 +0000 Subject: [PATCH 18/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index e149666c0c0bb..3e76b0decbadc 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -119,7 +119,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio std::bind(&BehaviorVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), create_subscription_options(this)); sub_occupancy_grid_ = this->create_subscription( - "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::on_occupancy_grid, this, _1), + "~/input/occupancy_grid", 1, + std::bind(&BehaviorVelocityPlannerNode::on_occupancy_grid, this, _1), create_subscription_options(this)); srv_load_plugin_ = create_service( From b107418ab4dcb1c0204a5732c37b730690ba71b4 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:28:29 +0200 Subject: [PATCH 19/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 12 ++++++------ planning/behavior_velocity_planner/src/node.hpp | 2 +- .../src/planner_manager.cpp | 14 +++++++------- .../src/planner_manager.hpp | 6 +++--- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 3e76b0decbadc..733383ec4afc9 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -124,7 +124,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio create_subscription_options(this)); srv_load_plugin_ = create_service( - "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); + "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::on_load_plugin, this, _1, _2)); srv_unload_plugin_ = create_service( "~/service/unload_plugin", std::bind(&BehaviorVelocityPlannerNode::on_unload_plugin, this, _1, _2)); @@ -158,19 +158,19 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio if (name.empty()) { break; } - planner_manager_.launchScenePlugin(*this, name); + planner_manager_.launch_scene_plugin(*this, name); } logger_configure_ = std::make_unique(this); published_time_publisher_ = std::make_unique(this); } -void BehaviorVelocityPlannerNode::onLoadPlugin( +void BehaviorVelocityPlannerNode::on_load_plugin( const LoadPlugin::Request::SharedPtr request, [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.launchScenePlugin(*this, request->plugin_name); + planner_manager_.launch_scene_plugin(*this, request->plugin_name); } void BehaviorVelocityPlannerNode::on_unload_plugin( @@ -178,7 +178,7 @@ void BehaviorVelocityPlannerNode::on_unload_plugin( [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.removeScenePlugin(*this, request->plugin_name); + planner_manager_.remove_scene_plugin(*this, request->plugin_name); } // NOTE: argument planner_data must not be referenced for multithreading @@ -435,7 +435,7 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath } // Plan path velocity - const auto velocity_planned_path = planner_manager_.planPathVelocity( + const auto velocity_planned_path = planner_manager_.plan_path_velocity( std::make_shared(planner_data), *input_path_msg); // screening diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 039bfbefd8254..a0c85e9ca93b1 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -118,7 +118,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void on_unload_plugin( const UnloadPlugin::Request::SharedPtr request, const UnloadPlugin::Response::SharedPtr response); - void onLoadPlugin( + void on_load_plugin( const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); // mutex for planner_data_ diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 27671080f8ae9..da83caea66a35 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -23,15 +23,15 @@ namespace autoware::behavior_velocity_planner { namespace { -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) +std::string json_dumps_pose(const geometry_msgs::msg::Pose & pose) { - const std::string jsonDumpsPose = + const std::string json_dumps_pose = (boost::format( R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % pose.orientation.y % pose.orientation.z) .str(); - return jsonDumpsPose; + return json_dumps_pose; } diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( @@ -43,7 +43,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( stop_reason_diag.name = "stop_reason"; stop_reason_diag.message = stop_reason; stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); + stop_reason_diag_kv.value = json_dumps_pose(stop_pose); stop_reason_diag.values.push_back(stop_reason_diag_kv); return stop_reason_diag; } @@ -55,7 +55,7 @@ BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() { } -void BehaviorVelocityPlannerManager::launchScenePlugin( +void BehaviorVelocityPlannerManager::launch_scene_plugin( rclcpp::Node & node, const std::string & name) { if (plugin_loader_.isClassAvailable(name)) { @@ -78,7 +78,7 @@ void BehaviorVelocityPlannerManager::launchScenePlugin( } } -void BehaviorVelocityPlannerManager::removeScenePlugin( +void BehaviorVelocityPlannerManager::remove_scene_plugin( rclcpp::Node & node, const std::string & name) { auto it = std::remove_if( @@ -97,7 +97,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( } } -autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( +autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::plan_path_velocity( const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg) { diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index 448210116e56b..59773eeb937d0 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -45,10 +45,10 @@ class BehaviorVelocityPlannerManager { public: BehaviorVelocityPlannerManager(); - void launchScenePlugin(rclcpp::Node & node, const std::string & name); - void removeScenePlugin(rclcpp::Node & node, const std::string & name); + void launch_scene_plugin(rclcpp::Node & node, const std::string & name); + void remove_scene_plugin(rclcpp::Node & node, const std::string & name); - autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId plan_path_velocity( const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg); From 45ee87bbefd226c4ba83ffd8106cda6d7e8717c2 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:29:41 +0200 Subject: [PATCH 20/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/planner_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index da83caea66a35..fc8dd672ab13d 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -34,7 +34,7 @@ std::string json_dumps_pose(const geometry_msgs::msg::Pose & pose) return json_dumps_pose; } -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( +diagnostic_msgs::msg::DiagnosticStatus make_stop_reason_diag( const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; @@ -119,7 +119,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: } } - stop_reason_diag_ = makeStopReasonDiag( + stop_reason_diag_ = make_stop_reason_diag( stop_reason_msg, output_path_msg.points[first_stop_path_point_index].point.pose); return output_path_msg; From b48d55edf15cfdd49b9434ef981dc90cb03c98ac Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:35:54 +0200 Subject: [PATCH 21/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 12 ++++++------ planning/behavior_velocity_planner/src/node.hpp | 6 +++--- .../src/planner_manager.cpp | 2 +- .../src/planner_manager.hpp | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 733383ec4afc9..74218af53e9c8 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -130,7 +130,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio std::bind(&BehaviorVelocityPlannerNode::on_unload_plugin, this, _1, _2)); // set velocity smoother param - onParam(); + on_param(); // Publishers path_pub_ = this->create_publisher("~/output/path", 1); @@ -307,9 +307,9 @@ void BehaviorVelocityPlannerNode::on_acceleration( planner_data_.current_acceleration = msg; } -void BehaviorVelocityPlannerNode::onParam() +void BehaviorVelocityPlannerNode::on_param() { - // Note(VRichardJP): mutex lock is not necessary as onParam is only called once in the + // Note(VRichardJP): mutex lock is not necessary as on_param is only called once in the // constructed. It would be required if it was a callback. std::lock_guard // lock(mutex_); planner_data_.velocity_smoother_ = @@ -400,20 +400,20 @@ void BehaviorVelocityPlannerNode::on_trigger( } const autoware_auto_planning_msgs::msg::Path output_path_msg = - generatePath(input_path_msg, planner_data_); + generate_path(input_path_msg, planner_data_); lk.unlock(); path_pub_->publish(output_path_msg); published_time_publisher_->publish_if_subscribed(path_pub_, output_path_msg.header.stamp); - stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); + stop_reason_diag_pub_->publish(planner_manager_.get_stop_reason_diag()); if (debug_viz_pub_->get_subscription_count() > 0) { publish_debug_marker(output_path_msg); } } -autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( +autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generate_path( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data) { diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index a0c85e9ca93b1..3b5a264c1d792 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -92,7 +92,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); void on_external_velocity_limit(const VelocityLimit::ConstSharedPtr msg); - void onParam(); + void on_param(); // publisher rclcpp::Publisher::SharedPtr path_pub_; @@ -125,9 +125,9 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::mutex mutex_; // function - geometry_msgs::msg::PoseStamped getCurrentPose(); + geometry_msgs::msg::PoseStamped get_current_pose(); bool is_data_ready(const PlannerData & planner_data, rclcpp::Clock clock) const; - autoware_auto_planning_msgs::msg::Path generatePath( + autoware_auto_planning_msgs::msg::Path generate_path( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index fc8dd672ab13d..72393e3e7a3ac 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -125,7 +125,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: return output_path_msg; } -diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopReasonDiag() const +diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::get_stop_reason_diag() const { return stop_reason_diag_; } diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index 59773eeb937d0..8c3caa8e88d60 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -52,7 +52,7 @@ class BehaviorVelocityPlannerManager const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg); - [[nodiscard]] diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; + [[nodiscard]] diagnostic_msgs::msg::DiagnosticStatus get_stop_reason_diag() const; private: diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; From b34f7a6d7250f42edd56b0bb05b51556451fb100 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 17:37:14 +0200 Subject: [PATCH 22/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- .../behavior_velocity_planner/src/planner_manager.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 72393e3e7a3ac..d93d693033292 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -109,11 +109,11 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); plugin->plan(&output_path_msg); - const auto firstStopPathPointIndex = plugin->getFirstStopPathPointIndex(); + const auto first_stop_path_point_index_from_plugin = plugin->getFirstStopPathPointIndex(); - if (firstStopPathPointIndex) { - if (firstStopPathPointIndex.value() < first_stop_path_point_index) { - first_stop_path_point_index = firstStopPathPointIndex.value(); + if (first_stop_path_point_index_from_plugin) { + if (first_stop_path_point_index_from_plugin.value() < first_stop_path_point_index) { + first_stop_path_point_index = first_stop_path_point_index_from_plugin.value(); stop_reason_msg = plugin->getModuleName(); } } From b39bc8941101ebf2204b9b2570534bc273f978e5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 12 Apr 2024 15:37:59 +0000 Subject: [PATCH 23/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 74218af53e9c8..7c0a8b9f191b3 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -362,7 +362,8 @@ void BehaviorVelocityPlannerNode::on_traffic_signals( } } -void BehaviorVelocityPlannerNode::on_external_velocity_limit(const VelocityLimit::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::on_external_velocity_limit( + const VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.external_velocity_limit = *msg; From 986cb2f948354168abf2722c74a13694d8dc6c29 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 14:41:55 +0200 Subject: [PATCH 24/53] fix(autoware_behavior_velocity_planner): fix naming issues Signed-off-by: Esteve Fernandez --- .../test/src/test_node_interface.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 21a6e543f3d3f..8bb568ce89a6c 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -26,7 +26,7 @@ using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; -std::shared_ptr generateTestManager() +std::shared_ptr generate_test_manager() { auto test_manager = std::make_shared(); @@ -43,7 +43,7 @@ std::shared_ptr generateTestManager() return test_manager; } -std::shared_ptr generateNode() +std::shared_ptr generate_node() { auto node_options = rclcpp::NodeOptions{}; @@ -114,9 +114,9 @@ std::shared_ptr generateNode() return std::make_shared(node_options); } -void publishMandatoryTopics( - std::shared_ptr test_manager, - std::shared_ptr test_target_node) +void publish_mandatory_topics( + const std::shared_ptr& test_manager, + const std::shared_ptr& test_target_node) { // publish necessary topics from test_manager test_manager->publishTF(test_target_node, "/tf"); @@ -142,10 +142,10 @@ void publishMandatoryTopics( TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_manager = generate_test_manager(); + auto test_target_node = generate_node(); - publishMandatoryTopics(test_manager, test_target_node); + publish_mandatory_topics(test_manager, test_target_node); // test with nominal path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); @@ -161,9 +161,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - publishMandatoryTopics(test_manager, test_target_node); + auto test_manager = generate_test_manager(); + auto test_target_node = generate_node(); + publish_mandatory_topics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); From 1e192a0446e5232509435870ec3e4385d7ef45aa Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 14:49:20 +0200 Subject: [PATCH 25/53] fix(autoware_behavior_velocity_planner): fix clang-tidy warnings Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 2 +- planning/behavior_velocity_planner/src/planner_manager.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 7c0a8b9f191b3..7fa9d9d4a28a0 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -467,7 +467,7 @@ void BehaviorVelocityPlannerNode::publish_debug_marker( for (size_t i = 0; i < path.points.size(); ++i) { visualization_msgs::msg::Marker marker; marker.header = path.header; - marker.id = i; + marker.id = static_cast(i); marker.type = visualization_msgs::msg::Marker::ARROW; marker.pose = path.points.at(i).pose; marker.scale.y = marker.scale.z = 0.05; diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index d93d693033292..0fce03ac3b23d 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -25,7 +25,7 @@ namespace { std::string json_dumps_pose(const geometry_msgs::msg::Pose & pose) { - const std::string json_dumps_pose = + std::string json_dumps_pose = (boost::format( R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % From ff7a9500c5fa989156cdbfe4390a776ea3cc41b2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 15 Apr 2024 12:52:36 +0000 Subject: [PATCH 26/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- .../test/src/test_node_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 8bb568ce89a6c..b63d4e25d3e1e 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -115,8 +115,8 @@ std::shared_ptr generate_node() } void publish_mandatory_topics( - const std::shared_ptr& test_manager, - const std::shared_ptr& test_target_node) + const std::shared_ptr & test_manager, + const std::shared_ptr & test_target_node) { // publish necessary topics from test_manager test_manager->publishTF(test_target_node, "/tf"); From 98fd1d48ebeb7e0e4768e7f2cb7044b7a74ce2f5 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 16:22:25 +0200 Subject: [PATCH 27/53] fix(autoware_behavior_velocity_planner): fix clang-tidy warnings Signed-off-by: Esteve Fernandez --- .../behavior_velocity_planner_common/planner_data.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2fed958640ae8..ea765d3876da9 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -71,8 +71,8 @@ struct PlannerData nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; // nearest search - double ego_nearest_dist_threshold; - double ego_nearest_yaw_threshold; + double ego_nearest_dist_threshold = 0.0; + double ego_nearest_yaw_threshold = 0.0; // other internal data // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the @@ -98,9 +98,9 @@ struct PlannerData double max_stop_jerk_threshold; double system_delay; double delay_response_time; - double stop_line_extend_length; + double stop_line_extend_length = 0.0; - bool isVehicleStopped(const double stop_duration = 0.0) const + [[nodiscard]] bool is_vehicle_stopped(const double stop_duration = 0.0) const { if (velocity_buffer.empty()) { return false; @@ -136,7 +136,7 @@ struct PlannerData *@brief queries the traffic signal information of given Id. if keep_last_observation = true, *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation */ - std::optional getTrafficSignal( + [[nodiscard]] std::optional get_traffic_signal( const lanelet::Id id, const bool keep_last_observation = false) const { const auto & traffic_light_id_map = From 55895c1719b99fb5503ac5300e9b40f45ccdcdaf Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 17:17:01 +0200 Subject: [PATCH 28/53] build: revert changes Signed-off-by: Esteve Fernandez --- .../behavior_velocity_planner_common/planner_data.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index ea765d3876da9..1511d430ddd0c 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -47,6 +47,7 @@ namespace behavior_velocity_planner { +class BehaviorVelocityPlannerNode; struct PlannerData { explicit PlannerData(rclcpp::Node & node) @@ -71,8 +72,8 @@ struct PlannerData nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; // nearest search - double ego_nearest_dist_threshold = 0.0; - double ego_nearest_yaw_threshold = 0.0; + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; // other internal data // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the @@ -98,9 +99,9 @@ struct PlannerData double max_stop_jerk_threshold; double system_delay; double delay_response_time; - double stop_line_extend_length = 0.0; + double stop_line_extend_length; - [[nodiscard]] bool is_vehicle_stopped(const double stop_duration = 0.0) const + bool isVehicleStopped(const double stop_duration = 0.0) const { if (velocity_buffer.empty()) { return false; @@ -136,7 +137,7 @@ struct PlannerData *@brief queries the traffic signal information of given Id. if keep_last_observation = true, *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation */ - [[nodiscard]] std::optional get_traffic_signal( + std::optional getTrafficSignal( const lanelet::Id id, const bool keep_last_observation = false) const { const auto & traffic_light_id_map = From 1ee79589d5ee7d43b12034434eca3771e7a0123d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 16 Apr 2024 14:50:38 +0200 Subject: [PATCH 29/53] build: update autoware_behavior_velocity_planner downstream dependencies Signed-off-by: Esteve Fernandez --- launch/tier4_planning_launch/package.xml | 2 +- planning/autoware_static_centerline_generator/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index d04a405a61c7b..b97ad589d07f7 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -59,7 +59,7 @@ autoware_remaining_distance_time_calculator behavior_path_planner - behavior_velocity_planner + autoware_behavior_velocity_planner costmap_generator external_cmd_selector external_velocity_limit_selector diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 96e17595d49ee..2a2b35e4ebf7b 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -45,7 +45,7 @@ ament_lint_auto autoware_lint_common behavior_path_planner - behavior_velocity_planner + autoware_behavior_velocity_planner ros_testing rosidl_interface_packages From 53f85978d370469a792a65aaace254711c76c497 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 16 Apr 2024 12:54:13 +0000 Subject: [PATCH 30/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- launch/tier4_planning_launch/package.xml | 2 +- planning/autoware_static_centerline_generator/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index b97ad589d07f7..ac79f384ea2f7 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,7 +58,7 @@ autoware_cmake autoware_remaining_distance_time_calculator - behavior_path_planner + autoware_behavior_path_planner autoware_behavior_velocity_planner costmap_generator external_cmd_selector diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 2a2b35e4ebf7b..56d30a715c006 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -43,9 +43,9 @@ ament_cmake_gmock ament_lint_auto + autoware_behavior_velocity_planner autoware_lint_common behavior_path_planner - autoware_behavior_velocity_planner ros_testing rosidl_interface_packages From b1b880e3216a27c8519708cc4401e12e767d42ff Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 22 Apr 2024 15:06:41 +0200 Subject: [PATCH 31/53] fix(autoware_behavior_velocity_planner): undo function renames Signed-off-by: Esteve Fernandez --- .../behavior_velocity_planner/src/node.cpp | 80 +++++++++---------- .../behavior_velocity_planner/src/node.hpp | 26 +++--- .../src/planner_manager.cpp | 4 +- .../src/planner_manager.hpp | 4 +- 4 files changed, 57 insertions(+), 57 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 7fa9d9d4a28a0..33373417e2da0 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -40,7 +40,7 @@ namespace { -rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node_ptr) +rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) { rclcpp::CallbackGroup::SharedPtr callback_group = node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -82,55 +82,55 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Trigger Subscriber trigger_sub_path_with_lane_id_ = this->create_subscription( - "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::on_trigger, this, _1), - create_subscription_options(this)); + "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1), + createSubscriptionOptions(this)); // Subscribers sub_predicted_objects_ = this->create_subscription( "~/input/dynamic_objects", 1, - std::bind(&BehaviorVelocityPlannerNode::on_predicted_objects, this, _1), - create_subscription_options(this)); + std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1), + createSubscriptionOptions(this)); sub_no_ground_pointcloud_ = this->create_subscription( "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&BehaviorVelocityPlannerNode::on_no_ground_point_cloud, this, _1), - create_subscription_options(this)); + std::bind(&BehaviorVelocityPlannerNode::onNoGroundPointCloud, this, _1), + createSubscriptionOptions(this)); sub_vehicle_odometry_ = this->create_subscription( - "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::on_odometry, this, _1), - create_subscription_options(this)); + "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onOdometry, this, _1), + createSubscriptionOptions(this)); sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::on_acceleration, this, _1), - create_subscription_options(this)); + createSubscriptionOptions(this)); sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), - std::bind(&BehaviorVelocityPlannerNode::on_lanelet_map, this, _1), - create_subscription_options(this)); + std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), + createSubscriptionOptions(this)); sub_traffic_signals_ = this->create_subscription( "~/input/traffic_signals", 1, - std::bind(&BehaviorVelocityPlannerNode::on_traffic_signals, this, _1), - create_subscription_options(this)); + std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), + createSubscriptionOptions(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), - std::bind(&BehaviorVelocityPlannerNode::on_external_velocity_limit, this, _1)); + std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); sub_virtual_traffic_light_states_ = this->create_subscription( "~/input/virtual_traffic_light_states", 1, - std::bind(&BehaviorVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), - create_subscription_options(this)); + std::bind(&BehaviorVelocityPlannerNode::onVirtualTrafficLightStates, this, _1), + createSubscriptionOptions(this)); sub_occupancy_grid_ = this->create_subscription( "~/input/occupancy_grid", 1, - std::bind(&BehaviorVelocityPlannerNode::on_occupancy_grid, this, _1), - create_subscription_options(this)); + std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), + createSubscriptionOptions(this)); srv_load_plugin_ = create_service( - "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::on_load_plugin, this, _1, _2)); + "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); srv_unload_plugin_ = create_service( "~/service/unload_plugin", - std::bind(&BehaviorVelocityPlannerNode::on_unload_plugin, this, _1, _2)); + std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); // set velocity smoother param - on_param(); + onParam(); // Publishers path_pub_ = this->create_publisher("~/output/path", 1); @@ -158,31 +158,31 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio if (name.empty()) { break; } - planner_manager_.launch_scene_plugin(*this, name); + planner_manager_.launchScenePlugin(*this, name); } logger_configure_ = std::make_unique(this); published_time_publisher_ = std::make_unique(this); } -void BehaviorVelocityPlannerNode::on_load_plugin( +void BehaviorVelocityPlannerNode::onLoadPlugin( const LoadPlugin::Request::SharedPtr request, [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.launch_scene_plugin(*this, request->plugin_name); + planner_manager_.launchScenePlugin(*this, request->plugin_name); } -void BehaviorVelocityPlannerNode::on_unload_plugin( +void BehaviorVelocityPlannerNode::onUnloadPlugin( const UnloadPlugin::Request::SharedPtr request, [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.remove_scene_plugin(*this, request->plugin_name); + planner_manager_.removeScenePlugin(*this, request->plugin_name); } // NOTE: argument planner_data must not be referenced for multithreading -bool BehaviorVelocityPlannerNode::is_data_ready( +bool BehaviorVelocityPlannerNode::isDataReady( const PlannerData & planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; @@ -226,21 +226,21 @@ bool BehaviorVelocityPlannerNode::is_data_ready( return true; } -void BehaviorVelocityPlannerNode::on_occupancy_grid( +void BehaviorVelocityPlannerNode::onOccupancyGrid( const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.occupancy_grid = msg; } -void BehaviorVelocityPlannerNode::on_predicted_objects( +void BehaviorVelocityPlannerNode::onPredictedObjects( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.predicted_objects = msg; } -void BehaviorVelocityPlannerNode::on_no_ground_point_cloud( +void BehaviorVelocityPlannerNode::onNoGroundPointCloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { geometry_msgs::msg::TransformStamped transform; @@ -267,7 +267,7 @@ void BehaviorVelocityPlannerNode::on_no_ground_point_cloud( } } -void BehaviorVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -307,9 +307,9 @@ void BehaviorVelocityPlannerNode::on_acceleration( planner_data_.current_acceleration = msg; } -void BehaviorVelocityPlannerNode::on_param() +void BehaviorVelocityPlannerNode::onParam() { - // Note(VRichardJP): mutex lock is not necessary as on_param is only called once in the + // Note(VRichardJP): mutex lock is not necessary as onParam is only called once in the // constructed. It would be required if it was a callback. std::lock_guard // lock(mutex_); planner_data_.velocity_smoother_ = @@ -317,7 +317,7 @@ void BehaviorVelocityPlannerNode::on_param() planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } -void BehaviorVelocityPlannerNode::on_lanelet_map( +void BehaviorVelocityPlannerNode::onLaneletMap( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -326,7 +326,7 @@ void BehaviorVelocityPlannerNode::on_lanelet_map( has_received_map_ = true; } -void BehaviorVelocityPlannerNode::on_traffic_signals( +void BehaviorVelocityPlannerNode::onTrafficSignals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -362,26 +362,26 @@ void BehaviorVelocityPlannerNode::on_traffic_signals( } } -void BehaviorVelocityPlannerNode::on_external_velocity_limit( +void BehaviorVelocityPlannerNode::onExternalVelocityLimit( const VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.external_velocity_limit = *msg; } -void BehaviorVelocityPlannerNode::on_virtual_traffic_light_states( +void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.virtual_traffic_light_states = msg; } -void BehaviorVelocityPlannerNode::on_trigger( +void BehaviorVelocityPlannerNode::onTrigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { std::unique_lock lk(mutex_); - if (!is_data_ready(planner_data_, *get_clock())) { + if (!isDataReady(planner_data_, *get_clock())) { return; } diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 3b5a264c1d792..2e9b0f0042a66 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -78,21 +78,21 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; - void on_trigger( + void onTrigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); - void on_predicted_objects( + void onPredictedObjects( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); - void on_no_ground_point_cloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); - void on_lanelet_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); - void on_traffic_signals( + void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onTrafficSignals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); - void on_virtual_traffic_light_states( + void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); - void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); - void on_external_velocity_limit(const VelocityLimit::ConstSharedPtr msg); - void on_param(); + void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); + void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); + void onParam(); // publisher rclcpp::Publisher::SharedPtr path_pub_; @@ -115,10 +115,10 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Service::SharedPtr srv_load_plugin_; rclcpp::Service::SharedPtr srv_unload_plugin_; - void on_unload_plugin( + void onUnloadPlugin( const UnloadPlugin::Request::SharedPtr request, const UnloadPlugin::Response::SharedPtr response); - void on_load_plugin( + void onLoadPlugin( const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); // mutex for planner_data_ @@ -126,7 +126,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped get_current_pose(); - bool is_data_ready(const PlannerData & planner_data, rclcpp::Clock clock) const; + bool isDataReady(const PlannerData & planner_data, rclcpp::Clock clock) const; autoware_auto_planning_msgs::msg::Path generate_path( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 0fce03ac3b23d..e94a056fbc31d 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -55,7 +55,7 @@ BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() { } -void BehaviorVelocityPlannerManager::launch_scene_plugin( +void BehaviorVelocityPlannerManager::launchScenePlugin( rclcpp::Node & node, const std::string & name) { if (plugin_loader_.isClassAvailable(name)) { @@ -78,7 +78,7 @@ void BehaviorVelocityPlannerManager::launch_scene_plugin( } } -void BehaviorVelocityPlannerManager::remove_scene_plugin( +void BehaviorVelocityPlannerManager::removeScenePlugin( rclcpp::Node & node, const std::string & name) { auto it = std::remove_if( diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index 8c3caa8e88d60..9b88bfd1b2c78 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -45,8 +45,8 @@ class BehaviorVelocityPlannerManager { public: BehaviorVelocityPlannerManager(); - void launch_scene_plugin(rclcpp::Node & node, const std::string & name); - void remove_scene_plugin(rclcpp::Node & node, const std::string & name); + void launchScenePlugin(rclcpp::Node & node, const std::string & name); + void removeScenePlugin(rclcpp::Node & node, const std::string & name); autoware_auto_planning_msgs::msg::PathWithLaneId plan_path_velocity( const std::shared_ptr & planner_data, From 0fdd3c33345f00a3c79e4b9f1bb3b0dd17b990d2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 22 Apr 2024 13:08:45 +0000 Subject: [PATCH 32/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 33373417e2da0..f8666eb35e7be 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -119,8 +119,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio std::bind(&BehaviorVelocityPlannerNode::onVirtualTrafficLightStates, this, _1), createSubscriptionOptions(this)); sub_occupancy_grid_ = this->create_subscription( - "~/input/occupancy_grid", 1, - std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), + "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); srv_load_plugin_ = create_service( @@ -362,8 +361,7 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( } } -void BehaviorVelocityPlannerNode::onExternalVelocityLimit( - const VelocityLimit::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.external_velocity_limit = *msg; From 21331aae060af91d2448d2b619b7f0216f2f59a0 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 22 Apr 2024 15:09:15 +0200 Subject: [PATCH 33/53] fix(autoware_behavior_velocity_planner): undo function renames Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/node.cpp | 16 ++++++++-------- planning/behavior_velocity_planner/src/node.hpp | 6 +++--- .../src/planner_manager.cpp | 4 ++-- .../src/planner_manager.hpp | 4 ++-- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index f8666eb35e7be..ef162285db7c8 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -99,7 +99,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onOdometry, this, _1), createSubscriptionOptions(this)); sub_acceleration_ = this->create_subscription( - "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::on_acceleration, this, _1), + "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), createSubscriptionOptions(this)); sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), @@ -299,7 +299,7 @@ void BehaviorVelocityPlannerNode::onOdometry(const nav_msgs::msg::Odometry::Cons } } -void BehaviorVelocityPlannerNode::on_acceleration( +void BehaviorVelocityPlannerNode::onAcceleration( const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -399,20 +399,20 @@ void BehaviorVelocityPlannerNode::onTrigger( } const autoware_auto_planning_msgs::msg::Path output_path_msg = - generate_path(input_path_msg, planner_data_); + generatePath(input_path_msg, planner_data_); lk.unlock(); path_pub_->publish(output_path_msg); published_time_publisher_->publish_if_subscribed(path_pub_, output_path_msg.header.stamp); - stop_reason_diag_pub_->publish(planner_manager_.get_stop_reason_diag()); + stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); if (debug_viz_pub_->get_subscription_count() > 0) { - publish_debug_marker(output_path_msg); + publishDebugMarker(output_path_msg); } } -autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generate_path( +autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data) { @@ -434,7 +434,7 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generate_pat } // Plan path velocity - const auto velocity_planned_path = planner_manager_.plan_path_velocity( + const auto velocity_planned_path = planner_manager_.planPathVelocity( std::make_shared(planner_data), *input_path_msg); // screening @@ -458,7 +458,7 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generate_pat return output_path_msg; } -void BehaviorVelocityPlannerNode::publish_debug_marker( +void BehaviorVelocityPlannerNode::publishDebugMarker( const autoware_auto_planning_msgs::msg::Path & path) { visualization_msgs::msg::MarkerArray output_msg; diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 2e9b0f0042a66..3970cde2f727d 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -84,7 +84,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); @@ -99,7 +99,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - void publish_debug_marker(const autoware_auto_planning_msgs::msg::Path & path); + void publishDebugMarker(const autoware_auto_planning_msgs::msg::Path & path); // parameter double forward_path_length_; @@ -127,7 +127,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped get_current_pose(); bool isDataReady(const PlannerData & planner_data, rclcpp::Clock clock) const; - autoware_auto_planning_msgs::msg::Path generate_path( + autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index e94a056fbc31d..6a312e5fdc609 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -97,7 +97,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( } } -autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::plan_path_velocity( +autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg) { @@ -125,7 +125,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: return output_path_msg; } -diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::get_stop_reason_diag() const +diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopReasonDiag() const { return stop_reason_diag_; } diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index 9b88bfd1b2c78..448210116e56b 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -48,11 +48,11 @@ class BehaviorVelocityPlannerManager void launchScenePlugin(rclcpp::Node & node, const std::string & name); void removeScenePlugin(rclcpp::Node & node, const std::string & name); - autoware_auto_planning_msgs::msg::PathWithLaneId plan_path_velocity( + autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg); - [[nodiscard]] diagnostic_msgs::msg::DiagnosticStatus get_stop_reason_diag() const; + [[nodiscard]] diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; private: diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; From 172911ab5fa28a08376d589d5bb52d1151d68f9e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 22 Apr 2024 15:12:28 +0200 Subject: [PATCH 34/53] fix(autoware_behavior_velocity_planner): undo function renames Signed-off-by: Esteve Fernandez --- .../src/planner_manager.cpp | 12 ++++++------ .../test/src/test_node_interface.cpp | 19 +++++++++---------- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 6a312e5fdc609..69f596949e6da 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -23,18 +23,18 @@ namespace autoware::behavior_velocity_planner { namespace { -std::string json_dumps_pose(const geometry_msgs::msg::Pose & pose) +std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) { - std::string json_dumps_pose = + std::string jsonDumpsPose = (boost::format( R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % pose.orientation.y % pose.orientation.z) .str(); - return json_dumps_pose; + return jsonDumpsPose; } -diagnostic_msgs::msg::DiagnosticStatus make_stop_reason_diag( +diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; @@ -43,7 +43,7 @@ diagnostic_msgs::msg::DiagnosticStatus make_stop_reason_diag( stop_reason_diag.name = "stop_reason"; stop_reason_diag.message = stop_reason; stop_reason_diag_kv.key = "stop_pose"; - stop_reason_diag_kv.value = json_dumps_pose(stop_pose); + stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); stop_reason_diag.values.push_back(stop_reason_diag_kv); return stop_reason_diag; } @@ -119,7 +119,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: } } - stop_reason_diag_ = make_stop_reason_diag( + stop_reason_diag_ = makeStopReasonDiag( stop_reason_msg, output_path_msg.points[first_stop_path_point_index].point.pose); return output_path_msg; diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index b63d4e25d3e1e..54df925c518db 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -26,7 +26,7 @@ using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; -std::shared_ptr generate_test_manager() +std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); @@ -43,7 +43,7 @@ std::shared_ptr generate_test_manager() return test_manager; } -std::shared_ptr generate_node() +std::shared_ptr generateNode() { auto node_options = rclcpp::NodeOptions{}; @@ -114,7 +114,7 @@ std::shared_ptr generate_node() return std::make_shared(node_options); } -void publish_mandatory_topics( +void publishMandatoryTopics( const std::shared_ptr & test_manager, const std::shared_ptr & test_target_node) { @@ -142,10 +142,10 @@ void publish_mandatory_topics( TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) { rclcpp::init(0, nullptr); - auto test_manager = generate_test_manager(); - auto test_target_node = generate_node(); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - publish_mandatory_topics(test_manager, test_target_node); + publishMandatoryTopics(test_manager, test_target_node); // test with nominal path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); @@ -153,7 +153,6 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) // test with empty path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); - rclcpp::shutdown(); } @@ -161,9 +160,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) { rclcpp::init(0, nullptr); - auto test_manager = generate_test_manager(); - auto test_target_node = generate_node(); - publish_mandatory_topics(test_manager, test_target_node); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); From 19236559b21c07d96078a497327a8e8d2c37b82c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 22 Apr 2024 15:14:14 +0200 Subject: [PATCH 35/53] fix(autoware_behavior_velocity_planner): undo function renames Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_planner/src/planner_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 69f596949e6da..877f45068614a 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -25,13 +25,13 @@ namespace { std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) { - std::string jsonDumpsPose = + const std::string json_dumps_pose = (boost::format( R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % pose.orientation.y % pose.orientation.z) .str(); - return jsonDumpsPose; + return json_dumps_pose; } diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( From 6f8d2b27a50d2a58c5f8867409ff56e130774794 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 24 Apr 2024 17:27:37 +0200 Subject: [PATCH 36/53] fix: fix loading package from test suite Signed-off-by: Esteve Fernandez --- .../test/test_static_centerline_generator.test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index 3011abccd48ca..dc3c26798b12d 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -59,7 +59,7 @@ def generate_test_description(): "config/behavior_path_planner.param.yaml", ), os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("autoware_behavior_velocity_planner"), "config/behavior_velocity_planner.param.yaml", ), os.path.join( From 4f4c49846ff563afcab89a0dfc9ed509fb95e678 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 7 May 2024 16:50:02 +0200 Subject: [PATCH 37/53] build(autoware_behavior_velocity_planner): rename base directory for autoware_behavior_velocity_planner Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 0 .../README.md | 0 .../config/behavior_velocity_planner.param.yaml | 0 .../docs/BehaviorVelocityPlanner-Architecture.drawio.svg | 0 .../docs/set_stop_velocity.drawio.svg | 0 .../launch/behavior_velocity_planner.launch.xml | 0 .../package.xml | 0 .../schema/behavior_velocity_planner.schema.json | 0 .../src/node.cpp | 0 .../src/node.hpp | 0 .../src/planner_manager.cpp | 0 .../src/planner_manager.hpp | 0 .../srv/LoadPlugin.srv | 0 .../srv/UnloadPlugin.srv | 0 .../test/src/test_node_interface.cpp | 0 15 files changed, 0 insertions(+), 0 deletions(-) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/CMakeLists.txt (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/README.md (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/config/behavior_velocity_planner.param.yaml (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/docs/BehaviorVelocityPlanner-Architecture.drawio.svg (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/docs/set_stop_velocity.drawio.svg (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/launch/behavior_velocity_planner.launch.xml (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/package.xml (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/schema/behavior_velocity_planner.schema.json (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/node.cpp (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/node.hpp (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/planner_manager.cpp (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/planner_manager.hpp (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/srv/LoadPlugin.srv (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/srv/UnloadPlugin.srv (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/test/src/test_node_interface.cpp (100%) diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/autoware_behavior_velocity_planner/CMakeLists.txt similarity index 100% rename from planning/behavior_velocity_planner/CMakeLists.txt rename to planning/autoware_behavior_velocity_planner/CMakeLists.txt diff --git a/planning/behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md similarity index 100% rename from planning/behavior_velocity_planner/README.md rename to planning/autoware_behavior_velocity_planner/README.md diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml rename to planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml diff --git a/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg similarity index 100% rename from planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg rename to planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg diff --git a/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg b/planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg rename to planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml similarity index 100% rename from planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml rename to planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml diff --git a/planning/behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml similarity index 100% rename from planning/behavior_velocity_planner/package.xml rename to planning/autoware_behavior_velocity_planner/package.xml diff --git a/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json similarity index 100% rename from planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json rename to planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp similarity index 100% rename from planning/behavior_velocity_planner/src/node.cpp rename to planning/autoware_behavior_velocity_planner/src/node.cpp diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp similarity index 100% rename from planning/behavior_velocity_planner/src/node.hpp rename to planning/autoware_behavior_velocity_planner/src/node.hpp diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp similarity index 100% rename from planning/behavior_velocity_planner/src/planner_manager.cpp rename to planning/autoware_behavior_velocity_planner/src/planner_manager.cpp diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp similarity index 100% rename from planning/behavior_velocity_planner/src/planner_manager.hpp rename to planning/autoware_behavior_velocity_planner/src/planner_manager.hpp diff --git a/planning/behavior_velocity_planner/srv/LoadPlugin.srv b/planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/srv/LoadPlugin.srv rename to planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv diff --git a/planning/behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/srv/UnloadPlugin.srv rename to planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp similarity index 100% rename from planning/behavior_velocity_planner/test/src/test_node_interface.cpp rename to planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp From aea6f5322228c4e870ca9df7fa17d2f8cf61841d Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 13 May 2024 16:03:33 +0200 Subject: [PATCH 38/53] fix: added prefix to missing strings Signed-off-by: Esteve Fernandez --- .github/CODEOWNERS | 2 +- planning/.pages | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 44632c7bf99ce..2eb1bdcdfcb33 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -157,6 +157,7 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @@ -179,7 +180,6 @@ planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp diff --git a/planning/.pages b/planning/.pages index 645a32b4b05fa..e355639eb8da5 100644 --- a/planning/.pages +++ b/planning/.pages @@ -18,7 +18,7 @@ nav: - 'Side Shift': planning/behavior_path_side_shift_module - 'Start Planner': planning/behavior_path_start_planner_module - 'Behavior Velocity Planner': - - 'About Behavior Velocity': planning/behavior_velocity_planner + - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - 'Template for Custom Module': planning/behavior_velocity_template_module - 'Available Module': - 'Blind Spot': planning/behavior_velocity_blind_spot_module From 52146968f4039afb00ae820266a00c26e08c760c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 13 May 2024 14:08:17 +0000 Subject: [PATCH 39/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- launch/tier4_planning_launch/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index ac79f384ea2f7..84cadcd3dccf6 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -57,9 +57,9 @@ ament_cmake_auto autoware_cmake - autoware_remaining_distance_time_calculator autoware_behavior_path_planner autoware_behavior_velocity_planner + autoware_remaining_distance_time_calculator costmap_generator external_cmd_selector external_velocity_limit_selector From 73c60b31418b50b670142a27cac9d7b438956692 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 15 May 2024 12:54:11 +0200 Subject: [PATCH 40/53] Update planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: M. Fatih Cırıt Signed-off-by: Esteve Fernandez --- .../launch/behavior_velocity_planner.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 81021728c5ec1..5d28b19b441a3 100644 --- a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -30,7 +30,7 @@ - + From a98d97ab815cbe835eb20e977f0a9d15d093be96 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 15 May 2024 13:33:01 +0200 Subject: [PATCH 41/53] build: fix namespaces Signed-off-by: Esteve Fernandez --- .../lane_driving/behavior_planning/behavior_planning.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 865ab99e870e4..57d0280330641 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -220,7 +220,7 @@ - + From 79963a2f8088053618fc40dcee96f1c8e1a4d9f1 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 21 May 2024 15:33:16 +0200 Subject: [PATCH 42/53] fix: use correct PluginInterface namespace Signed-off-by: Esteve Fernandez --- .../src/planner_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index 877f45068614a..9904087f2c8e5 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -51,7 +51,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() : plugin_loader_( - "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") + "autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") { } @@ -83,7 +83,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr & plugin) { + [&](const std::shared_ptr & plugin) { return plugin->getModuleName() == name; }); From 71b8f5bfd1ed8843f2980dcdbafdefc5c40087de Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 21 May 2024 16:25:28 +0200 Subject: [PATCH 43/53] build: revert clang-tidy related changes Signed-off-by: Esteve Fernandez --- .../autoware_behavior_velocity_planner/src/node.cpp | 6 +++--- .../autoware_behavior_velocity_planner/src/node.hpp | 6 +++--- .../src/planner_manager.cpp | 12 ++++++------ .../src/planner_manager.hpp | 2 +- .../test/src/test_node_interface.cpp | 4 ++-- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index ef162285db7c8..1dcef95a4420f 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -154,7 +154,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. - if (name.empty()) { + if (name == "") { break; } planner_manager_.launchScenePlugin(*this, name); @@ -182,7 +182,7 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin( // NOTE: argument planner_data must not be referenced for multithreading bool BehaviorVelocityPlannerNode::isDataReady( - const PlannerData & planner_data, rclcpp::Clock clock) const + const PlannerData planner_data, rclcpp::Clock clock) const { const auto & d = planner_data; @@ -465,7 +465,7 @@ void BehaviorVelocityPlannerNode::publishDebugMarker( for (size_t i = 0; i < path.points.size(); ++i) { visualization_msgs::msg::Marker marker; marker.header = path.header; - marker.id = static_cast(i); + marker.id = i; marker.type = visualization_msgs::msg::Marker::ARROW; marker.pose = path.points.at(i).pose; marker.scale.y = marker.scale.z = 0.05; diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index 3970cde2f727d..78b6b23569aa6 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -111,7 +111,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; HADMapBin::ConstSharedPtr map_ptr_{nullptr}; - bool has_received_map_{}; + bool has_received_map_; rclcpp::Service::SharedPtr srv_load_plugin_; rclcpp::Service::SharedPtr srv_unload_plugin_; @@ -125,8 +125,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::mutex mutex_; // function - geometry_msgs::msg::PoseStamped get_current_pose(); - bool isDataReady(const PlannerData & planner_data, rclcpp::Clock clock) const; + geometry_msgs::msg::PoseStamped getCurrentPose(); + bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const; autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index 9904087f2c8e5..839edab100b56 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -35,7 +35,7 @@ std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) } diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) + const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; @@ -83,7 +83,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr & plugin) { + [&](const std::shared_ptr plugin) { return plugin->getModuleName() == name; }); @@ -109,11 +109,11 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); plugin->plan(&output_path_msg); - const auto first_stop_path_point_index_from_plugin = plugin->getFirstStopPathPointIndex(); + const auto firstStopPathPointIndex = plugin->getFirstStopPathPointIndex(); - if (first_stop_path_point_index_from_plugin) { - if (first_stop_path_point_index_from_plugin.value() < first_stop_path_point_index) { - first_stop_path_point_index = first_stop_path_point_index_from_plugin.value(); + if (firstStopPathPointIndex) { + if (firstStopPathPointIndex.value() < first_stop_path_point_index) { + first_stop_path_point_index = firstStopPathPointIndex.value(); stop_reason_msg = plugin->getModuleName(); } } diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index 448210116e56b..f9c1d0253de62 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -52,7 +52,7 @@ class BehaviorVelocityPlannerManager const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg); - [[nodiscard]] diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; + diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; private: diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 54df925c518db..06c1bc48cf529 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -115,8 +115,8 @@ std::shared_ptr generateNode() } void publishMandatoryTopics( - const std::shared_ptr & test_manager, - const std::shared_ptr & test_target_node) + std::shared_ptr test_manager, + std::shared_ptr test_target_node) { // publish necessary topics from test_manager test_manager->publishTF(test_target_node, "/tf"); From 4ff7b6534dc7c9c8a41c530b9d6ab91c9f2f9263 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 21 May 2024 17:07:15 +0200 Subject: [PATCH 44/53] build: prefix all plugins and behavior_velocity_planner_common Signed-off-by: Esteve Fernandez --- .../src/planner_manager.cpp | 4 ++-- .../src/planner_manager.hpp | 1 - planning/behavior_velocity_blind_spot_module/plugins.xml | 2 +- .../behavior_velocity_blind_spot_module/src/manager.cpp | 2 +- .../behavior_velocity_blind_spot_module/src/manager.hpp | 2 +- .../behavior_velocity_crosswalk_module/src/manager.cpp | 2 +- .../behavior_velocity_crosswalk_module/src/manager.hpp | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../behavior_velocity_intersection_module/src/manager.cpp | 4 ++-- .../behavior_velocity_intersection_module/src/manager.hpp | 4 ++-- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../behavior_velocity_out_of_lane_module/src/manager.cpp | 2 +- .../behavior_velocity_out_of_lane_module/src/manager.hpp | 2 +- .../behavior_velocity_planner_common/plugin_interface.hpp | 7 ++++--- .../behavior_velocity_planner_common/plugin_wrapper.hpp | 6 +++--- planning/behavior_velocity_run_out_module/src/manager.cpp | 2 +- planning/behavior_velocity_run_out_module/src/manager.hpp | 2 +- .../behavior_velocity_speed_bump_module/src/manager.cpp | 2 +- .../behavior_velocity_speed_bump_module/src/manager.hpp | 2 +- .../behavior_velocity_stop_line_module/src/manager.cpp | 2 +- .../behavior_velocity_stop_line_module/src/manager.hpp | 2 +- planning/behavior_velocity_template_module/src/manager.cpp | 2 +- planning/behavior_velocity_template_module/src/manager.hpp | 2 +- .../behavior_velocity_traffic_light_module/src/manager.cpp | 2 +- .../behavior_velocity_traffic_light_module/src/manager.hpp | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- planning/behavior_velocity_walkway_module/src/manager.cpp | 2 +- planning/behavior_velocity_walkway_module/src/manager.hpp | 2 +- 37 files changed, 44 insertions(+), 44 deletions(-) diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index 839edab100b56..c8a9b26e05919 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -51,7 +51,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() : plugin_loader_( - "autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") + "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") { } @@ -83,7 +83,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr plugin) { + [&](const std::shared_ptr plugin) { return plugin->getModuleName() == name; }); diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index f9c1d0253de62..8fea862d80943 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -39,7 +39,6 @@ namespace autoware::behavior_velocity_planner { using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::PluginInterface; class BehaviorVelocityPlannerManager { diff --git a/planning/behavior_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_blind_spot_module/plugins.xml index 7dda59ea2fdbe..f588f2d1047b1 100644 --- a/planning/behavior_velocity_blind_spot_module/plugins.xml +++ b/planning/behavior_velocity_blind_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index faa10c1fbe9b4..ed14c57ca577f 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -94,4 +94,4 @@ BlindSpotModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::BlindSpotModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::BlindSpotModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp index 9aeaa0abfc4b7..f59099c4c803c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -45,7 +45,7 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class BlindSpotModulePlugin : public PluginWrapper +class BlindSpotModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 5110ff9993a62..183c2c56805bc 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -235,4 +235,4 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::CrosswalkModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::CrosswalkModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index 38bb63c121699..b573c4228f871 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -53,7 +53,7 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC const PathWithLaneId & path) override; }; -class CrosswalkModulePlugin : public PluginWrapper +class CrosswalkModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 8e9a6b6a58a96..8553750e9eb12 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -88,4 +88,4 @@ DetectionAreaModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::DetectionAreaModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::DetectionAreaModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp index 10fca7182d09a..72343483c5300 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -45,7 +45,7 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class DetectionAreaModulePlugin : public PluginWrapper +class DetectionAreaModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index 92004b4b9f249..fd6c91c1696dc 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -73,4 +73,4 @@ DynamicObstacleStopModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( behavior_velocity_planner::DynamicObstacleStopModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp index e461cc9c16445..44de61ec3dc06 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -49,7 +49,7 @@ class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class DynamicObstacleStopModulePlugin : public PluginWrapper +class DynamicObstacleStopModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 424fb6841eca9..0b923106933bc 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -593,7 +593,7 @@ bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegi #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::IntersectionModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::IntersectionModulePlugin, autoware::behavior_velocity_planner::PluginInterface) PLUGINLIB_EXPORT_CLASS( behavior_velocity_planner::MergeFromPrivateModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index 46281df2f29c7..9db40630168cd 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -81,11 +81,11 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; -class IntersectionModulePlugin : public PluginWrapper +class IntersectionModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; -class MergeFromPrivateModulePlugin : public PluginWrapper +class MergeFromPrivateModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index f1fa4c47e951b..75e7fe4c88243 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -70,4 +70,4 @@ NoDrivableLaneModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoDrivableLaneModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::NoDrivableLaneModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp index 2b3b80510c9e4..d0e072fafaf95 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -45,7 +45,7 @@ class NoDrivableLaneModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class NoDrivableLaneModulePlugin : public PluginWrapper +class NoDrivableLaneModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index 46cc224ea0f6b..65f91c3e1dd94 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -90,4 +90,4 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoStoppingAreaModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::NoStoppingAreaModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index f7a9a5433e900..978f904484cbd 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -45,7 +45,7 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class NoStoppingAreaModulePlugin : public PluginWrapper +class NoStoppingAreaModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index f257012c05519..c7428570dc509 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -140,4 +140,4 @@ OcclusionSpotModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OcclusionSpotModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::OcclusionSpotModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index c153a727b8a7e..31116caae29b6 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -59,7 +59,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class OcclusionSpotModulePlugin : public PluginWrapper +class OcclusionSpotModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 7d764722405af..5a672dec5ff34 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -105,4 +105,4 @@ OutOfLaneModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OutOfLaneModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::OutOfLaneModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp index c8ef397913c8f..0c86539b1bc01 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -57,7 +57,7 @@ class OutOfLaneModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class OutOfLaneModulePlugin : public PluginWrapper +class OutOfLaneModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp index 624da6b170d06..a86b96761ac70 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp @@ -22,22 +22,23 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class PluginInterface { + public: virtual ~PluginInterface() = default; virtual void init(rclcpp::Node & node) = 0; virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) = 0; virtual void updateSceneModuleInstances( - const std::shared_ptr & planner_data, + const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::optional getFirstStopPathPointIndex() = 0; virtual const char * getModuleName() = 0; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp index 7aa3e6bfef9ab..51116600c21d1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -20,7 +20,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { template @@ -33,7 +33,7 @@ class PluginWrapper : public PluginInterface scene_manager_->plan(path); }; void updateSceneModuleInstances( - const std::shared_ptr & planner_data, + const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override { scene_manager_->updateSceneModuleInstances(planner_data, path); @@ -48,6 +48,6 @@ class PluginWrapper : public PluginInterface std::unique_ptr scene_manager_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 1c85a22f57bf6..f24a4d930f38d 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -204,4 +204,4 @@ void RunOutModuleManager::setDynamicObstacleCreator( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::RunOutModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::RunOutModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_run_out_module/src/manager.hpp index 4dd66ad45898b..cc76e77f4ec16 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_run_out_module/src/manager.hpp @@ -45,7 +45,7 @@ class RunOutModuleManager : public SceneModuleManagerInterface void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; -class RunOutModulePlugin : public PluginWrapper +class RunOutModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index 5949357c90ff3..fc29dd15a6003 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -85,4 +85,4 @@ SpeedBumpModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::SpeedBumpModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::SpeedBumpModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp index 480208067d87e..56027de6ccf96 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -45,7 +45,7 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class SpeedBumpModulePlugin : public PluginWrapper +class SpeedBumpModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 5c9f0fd39c644..37809d6947cf8 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -109,4 +109,4 @@ StopLineModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::StopLineModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::StopLineModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp index 4b665a3a536f7..82aab7a3ebae7 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.hpp @@ -58,7 +58,7 @@ class StopLineModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class StopLineModulePlugin : public PluginWrapper +class StopLineModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_template_module/src/manager.cpp index 22763f44af7bf..a01eb8e162791 100644 --- a/planning/behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_template_module/src/manager.cpp @@ -60,4 +60,4 @@ TemplateModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::TemplateModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::TemplateModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_template_module/src/manager.hpp index ef4e1f00cae92..a2e5ffd765495 100644 --- a/planning/behavior_velocity_template_module/src/manager.hpp +++ b/planning/behavior_velocity_template_module/src/manager.hpp @@ -84,7 +84,7 @@ class TemplateModuleManager : public SceneModuleManagerInterface * The TemplateModulePlugin class is used to integrate the TemplateModuleManager into the Behavior * Velocity Planner. */ -class TemplateModulePlugin : public PluginWrapper +class TemplateModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index edddef5cef4d8..ceb904305e942 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -193,4 +193,4 @@ bool TrafficLightModuleManager::hasSameTrafficLight( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::TrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::TrafficLightModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 959ef2f91d36c..c490e94487cac 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -61,7 +61,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC std::optional nearest_ref_stop_path_point_index_; }; -class TrafficLightModulePlugin : public PluginWrapper +class TrafficLightModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 5310fae78c294..699e1b231acfb 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -81,4 +81,4 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( behavior_velocity_planner::VirtualTrafficLightModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp index 15bd6f468132e..eff8fc78d9d83 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -44,7 +44,7 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class VirtualTrafficLightModulePlugin : public PluginWrapper +class VirtualTrafficLightModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index d427e57009cf6..0f129d72e7da9 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -104,4 +104,4 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::WalkwayModulePlugin, behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::WalkwayModulePlugin, autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp index 453fcdb40f0db..f2872e94a6904 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -52,7 +52,7 @@ class WalkwayModuleManager : public SceneModuleManagerInterface const PathWithLaneId & path) override; }; -class WalkwayModulePlugin : public PluginWrapper +class WalkwayModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper { }; From 61b2d708ee6bae926d0e728857f7f52c06685508 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 21 May 2024 18:17:03 +0200 Subject: [PATCH 45/53] build: update PluginInterface namespace Signed-off-by: Esteve Fernandez --- .../src/planner_manager.cpp | 2 +- planning/behavior_velocity_crosswalk_module/plugins.xml | 2 +- planning/behavior_velocity_detection_area_module/plugins.xml | 2 +- .../plugins.xml | 2 +- planning/behavior_velocity_intersection_module/plugins.xml | 4 ++-- .../behavior_velocity_no_drivable_lane_module/plugins.xml | 2 +- .../behavior_velocity_no_stopping_area_module/plugins.xml | 2 +- planning/behavior_velocity_occlusion_spot_module/plugins.xml | 2 +- planning/behavior_velocity_out_of_lane_module/plugins.xml | 2 +- planning/behavior_velocity_run_out_module/plugins.xml | 2 +- planning/behavior_velocity_speed_bump_module/plugins.xml | 2 +- planning/behavior_velocity_stop_line_module/plugins.xml | 2 +- planning/behavior_velocity_template_module/plugins.xml | 2 +- planning/behavior_velocity_traffic_light_module/plugins.xml | 2 +- .../plugins.xml | 2 +- planning/behavior_velocity_walkway_module/plugins.xml | 2 +- 16 files changed, 17 insertions(+), 17 deletions(-) diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index c8a9b26e05919..529e8c68885f0 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -51,7 +51,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() : plugin_loader_( - "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") + "behavior_velocity_planner_common", "autoware::behavior_velocity_planner::PluginInterface") { } diff --git a/planning/behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml index 3d1d720857c17..a633be48c6a88 100644 --- a/planning/behavior_velocity_crosswalk_module/plugins.xml +++ b/planning/behavior_velocity_crosswalk_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_detection_area_module/plugins.xml b/planning/behavior_velocity_detection_area_module/plugins.xml index 73497c8bfdf2a..43e17ec269594 100644 --- a/planning/behavior_velocity_detection_area_module/plugins.xml +++ b/planning/behavior_velocity_detection_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml index 2f5662c7998ac..50cc0065b5d8d 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_intersection_module/plugins.xml b/planning/behavior_velocity_intersection_module/plugins.xml index 206f0324231ec..83dac60ac47ad 100644 --- a/planning/behavior_velocity_intersection_module/plugins.xml +++ b/planning/behavior_velocity_intersection_module/plugins.xml @@ -1,4 +1,4 @@ - - + + diff --git a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml index 33b0d5d6e7469..e798fa77ff042 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_stopping_area_module/plugins.xml b/planning/behavior_velocity_no_stopping_area_module/plugins.xml index a9b07297cfa30..d4dfd23408eba 100644 --- a/planning/behavior_velocity_no_stopping_area_module/plugins.xml +++ b/planning/behavior_velocity_no_stopping_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml index a38900d1a893b..4e39abeceffb9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/plugins.xml +++ b/planning/behavior_velocity_occlusion_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_out_of_lane_module/plugins.xml b/planning/behavior_velocity_out_of_lane_module/plugins.xml index 8c18fdce79480..c404bd3753517 100644 --- a/planning/behavior_velocity_out_of_lane_module/plugins.xml +++ b/planning/behavior_velocity_out_of_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_run_out_module/plugins.xml b/planning/behavior_velocity_run_out_module/plugins.xml index 5320d46f92929..f9f4dde807796 100644 --- a/planning/behavior_velocity_run_out_module/plugins.xml +++ b/planning/behavior_velocity_run_out_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_speed_bump_module/plugins.xml b/planning/behavior_velocity_speed_bump_module/plugins.xml index 506d25669f8cf..8d748aa832c2c 100644 --- a/planning/behavior_velocity_speed_bump_module/plugins.xml +++ b/planning/behavior_velocity_speed_bump_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_stop_line_module/plugins.xml index 51fb225fbebad..a4cc6bf728b8b 100644 --- a/planning/behavior_velocity_stop_line_module/plugins.xml +++ b/planning/behavior_velocity_stop_line_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_template_module/plugins.xml b/planning/behavior_velocity_template_module/plugins.xml index 3560c84e6a080..4fd1b111f41b2 100644 --- a/planning/behavior_velocity_template_module/plugins.xml +++ b/planning/behavior_velocity_template_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_traffic_light_module/plugins.xml b/planning/behavior_velocity_traffic_light_module/plugins.xml index b65cc66c5c232..de2780bf29882 100644 --- a/planning/behavior_velocity_traffic_light_module/plugins.xml +++ b/planning/behavior_velocity_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml index 943ef175aa8f8..d6f74b780f11a 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml +++ b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_walkway_module/plugins.xml b/planning/behavior_velocity_walkway_module/plugins.xml index 971a49f2cb044..9b1884a8cda30 100644 --- a/planning/behavior_velocity_walkway_module/plugins.xml +++ b/planning/behavior_velocity_walkway_module/plugins.xml @@ -1,3 +1,3 @@ - + From 72c3739fe792c4c92bdac591c34ca1b482c74625 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 22 May 2024 11:22:27 +0200 Subject: [PATCH 46/53] refactor: revert changes to behavior_velocity_planner Signed-off-by: Esteve Fernandez --- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 4 ++-- .../src/manager.cpp | 4 ++-- .../src/manager.hpp | 4 ++-- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../CMakeLists.txt | 4 ++-- .../README.md | 0 .../config/behavior_velocity_planner.param.yaml | 0 ...aviorVelocityPlanner-Architecture.drawio.svg | 0 .../docs/set_stop_velocity.drawio.svg | 0 .../launch/behavior_velocity_planner.launch.xml | 4 ++-- .../package.xml | 4 ++-- .../behavior_velocity_planner.schema.json | 0 .../src/node.cpp | 17 +++++++---------- .../src/node.hpp | 13 ++++++------- .../src/planner_manager.cpp | 9 ++++----- .../src/planner_manager.hpp | 6 ++---- .../srv/LoadPlugin.srv | 0 .../srv/UnloadPlugin.srv | 0 .../test/src/test_node_interface.cpp | 4 ++-- .../plugin_interface.hpp | 7 +++---- .../plugin_wrapper.hpp | 6 +++--- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- .../plugins.xml | 2 +- .../src/manager.cpp | 2 +- .../src/manager.hpp | 2 +- 65 files changed, 84 insertions(+), 92 deletions(-) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/CMakeLists.txt (90%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/README.md (100%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/config/behavior_velocity_planner.param.yaml (100%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/docs/BehaviorVelocityPlanner-Architecture.drawio.svg (100%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/docs/set_stop_velocity.drawio.svg (100%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/launch/behavior_velocity_planner.launch.xml (95%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/package.xml (96%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/schema/behavior_velocity_planner.schema.json (100%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/src/node.cpp (96%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/src/node.hpp (93%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/src/planner_manager.cpp (93%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/src/planner_manager.hpp (93%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/srv/LoadPlugin.srv (100%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/srv/UnloadPlugin.srv (100%) rename planning/{autoware_behavior_velocity_planner => behavior_velocity_planner}/test/src/test_node_interface.cpp (98%) diff --git a/planning/behavior_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_blind_spot_module/plugins.xml index f588f2d1047b1..7dda59ea2fdbe 100644 --- a/planning/behavior_velocity_blind_spot_module/plugins.xml +++ b/planning/behavior_velocity_blind_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index ed14c57ca577f..faa10c1fbe9b4 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -94,4 +94,4 @@ BlindSpotModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::BlindSpotModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::BlindSpotModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp index f59099c4c803c..9aeaa0abfc4b7 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -45,7 +45,7 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class BlindSpotModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class BlindSpotModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml index a633be48c6a88..3d1d720857c17 100644 --- a/planning/behavior_velocity_crosswalk_module/plugins.xml +++ b/planning/behavior_velocity_crosswalk_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 183c2c56805bc..5110ff9993a62 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -235,4 +235,4 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::CrosswalkModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::CrosswalkModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index b573c4228f871..38bb63c121699 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -53,7 +53,7 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC const PathWithLaneId & path) override; }; -class CrosswalkModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class CrosswalkModulePlugin : public PluginWrapper { }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/plugins.xml b/planning/behavior_velocity_detection_area_module/plugins.xml index 43e17ec269594..73497c8bfdf2a 100644 --- a/planning/behavior_velocity_detection_area_module/plugins.xml +++ b/planning/behavior_velocity_detection_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 8553750e9eb12..8e9a6b6a58a96 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -88,4 +88,4 @@ DetectionAreaModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::DetectionAreaModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::DetectionAreaModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp index 72343483c5300..10fca7182d09a 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -45,7 +45,7 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class DetectionAreaModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class DetectionAreaModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml index 50cc0065b5d8d..2f5662c7998ac 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index fd6c91c1696dc..92004b4b9f249 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -73,4 +73,4 @@ DynamicObstacleStopModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( behavior_velocity_planner::DynamicObstacleStopModulePlugin, - autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp index 44de61ec3dc06..e461cc9c16445 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -49,7 +49,7 @@ class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class DynamicObstacleStopModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class DynamicObstacleStopModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_intersection_module/plugins.xml b/planning/behavior_velocity_intersection_module/plugins.xml index 83dac60ac47ad..206f0324231ec 100644 --- a/planning/behavior_velocity_intersection_module/plugins.xml +++ b/planning/behavior_velocity_intersection_module/plugins.xml @@ -1,4 +1,4 @@ - - + + diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 0b923106933bc..424fb6841eca9 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -593,7 +593,7 @@ bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegi #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::IntersectionModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::IntersectionModulePlugin, behavior_velocity_planner::PluginInterface) PLUGINLIB_EXPORT_CLASS( behavior_velocity_planner::MergeFromPrivateModulePlugin, - autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index 9db40630168cd..46281df2f29c7 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -81,11 +81,11 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; -class IntersectionModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class IntersectionModulePlugin : public PluginWrapper { }; -class MergeFromPrivateModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class MergeFromPrivateModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml index e798fa77ff042..33b0d5d6e7469 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 75e7fe4c88243..f1fa4c47e951b 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -70,4 +70,4 @@ NoDrivableLaneModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoDrivableLaneModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::NoDrivableLaneModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp index d0e072fafaf95..2b3b80510c9e4 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -45,7 +45,7 @@ class NoDrivableLaneModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class NoDrivableLaneModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class NoDrivableLaneModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_no_stopping_area_module/plugins.xml b/planning/behavior_velocity_no_stopping_area_module/plugins.xml index d4dfd23408eba..a9b07297cfa30 100644 --- a/planning/behavior_velocity_no_stopping_area_module/plugins.xml +++ b/planning/behavior_velocity_no_stopping_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index 65f91c3e1dd94..46cc224ea0f6b 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -90,4 +90,4 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoStoppingAreaModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::NoStoppingAreaModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index 978f904484cbd..f7a9a5433e900 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -45,7 +45,7 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class NoStoppingAreaModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class NoStoppingAreaModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml index 4e39abeceffb9..a38900d1a893b 100644 --- a/planning/behavior_velocity_occlusion_spot_module/plugins.xml +++ b/planning/behavior_velocity_occlusion_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index c7428570dc509..f257012c05519 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -140,4 +140,4 @@ OcclusionSpotModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OcclusionSpotModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::OcclusionSpotModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index 31116caae29b6..c153a727b8a7e 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -59,7 +59,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class OcclusionSpotModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class OcclusionSpotModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_out_of_lane_module/plugins.xml b/planning/behavior_velocity_out_of_lane_module/plugins.xml index c404bd3753517..8c18fdce79480 100644 --- a/planning/behavior_velocity_out_of_lane_module/plugins.xml +++ b/planning/behavior_velocity_out_of_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 5a672dec5ff34..7d764722405af 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -105,4 +105,4 @@ OutOfLaneModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OutOfLaneModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::OutOfLaneModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp index 0c86539b1bc01..c8ef397913c8f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -57,7 +57,7 @@ class OutOfLaneModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class OutOfLaneModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class OutOfLaneModulePlugin : public PluginWrapper { }; diff --git a/planning/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt similarity index 90% rename from planning/autoware_behavior_velocity_planner/CMakeLists.txt rename to planning/behavior_velocity_planner/CMakeLists.txt index 37a02b844dfe9..ee7dd7c07a77a 100644 --- a/planning/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_behavior_velocity_planner) +project(behavior_velocity_planner) find_package(autoware_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -19,7 +19,7 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_lib - PLUGIN "autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" + PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md similarity index 100% rename from planning/autoware_behavior_velocity_planner/README.md rename to planning/behavior_velocity_planner/README.md diff --git a/planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml similarity index 100% rename from planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml rename to planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml diff --git a/planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg rename to planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg diff --git a/planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg b/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg similarity index 100% rename from planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg rename to planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg diff --git a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml similarity index 95% rename from planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml rename to planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 5d28b19b441a3..c1631fdb739de 100644 --- a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -28,9 +28,9 @@ - + - + diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml similarity index 96% rename from planning/autoware_behavior_velocity_planner/package.xml rename to planning/behavior_velocity_planner/package.xml index 573d862f1725b..1357b555f0ba4 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -1,9 +1,9 @@ - autoware_behavior_velocity_planner + behavior_velocity_planner 0.1.0 - The autoware_behavior_velocity_planner package + The behavior_velocity_planner package Mamoru Sobue Takayuki Murooka diff --git a/planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json similarity index 100% rename from planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json rename to planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp similarity index 96% rename from planning/autoware_behavior_velocity_planner/src/node.cpp rename to planning/behavior_velocity_planner/src/node.cpp index 1dcef95a4420f..1801db4658a72 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -52,10 +52,8 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) } } // namespace -namespace autoware::behavior_velocity_planner +namespace behavior_velocity_planner { -using ::behavior_velocity_planner::TrafficSignalStamped; - namespace { @@ -438,15 +436,14 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = - ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( - filtered_path, forward_path_length_, behavior_output_path_interval_); + const auto interpolated_path_msg = + interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); + output_path_msg = filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); @@ -480,7 +477,7 @@ void BehaviorVelocityPlannerNode::publishDebugMarker( } debug_viz_pub_->publish(output_msg); } -} // namespace autoware::behavior_velocity_planner +} // namespace behavior_velocity_planner #include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(behavior_velocity_planner::BehaviorVelocityPlannerNode) diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp similarity index 93% rename from planning/autoware_behavior_velocity_planner/src/node.hpp rename to planning/behavior_velocity_planner/src/node.hpp index 78b6b23569aa6..dd6edf6ae0bc0 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -18,8 +18,8 @@ #include "planner_manager.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include +#include +#include #include #include #include @@ -44,12 +44,11 @@ #include #include -namespace autoware::behavior_velocity_planner +namespace behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_behavior_velocity_planner::srv::LoadPlugin; -using autoware_behavior_velocity_planner::srv::UnloadPlugin; -using ::behavior_velocity_planner::PlannerData; +using behavior_velocity_planner::srv::LoadPlugin; +using behavior_velocity_planner::srv::UnloadPlugin; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node @@ -135,6 +134,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace autoware::behavior_velocity_planner +} // namespace behavior_velocity_planner #endif // NODE_HPP_ diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp similarity index 93% rename from planning/autoware_behavior_velocity_planner/src/planner_manager.cpp rename to planning/behavior_velocity_planner/src/planner_manager.cpp index 529e8c68885f0..cb491920c87d4 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -19,7 +19,7 @@ #include #include -namespace autoware::behavior_velocity_planner +namespace behavior_velocity_planner { namespace { @@ -50,8 +50,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_( - "behavior_velocity_planner_common", "autoware::behavior_velocity_planner::PluginInterface") +: plugin_loader_("behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") { } @@ -83,7 +82,7 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( { auto it = std::remove_if( scene_manager_plugins_.begin(), scene_manager_plugins_.end(), - [&](const std::shared_ptr plugin) { + [&](const std::shared_ptr plugin) { return plugin->getModuleName() == name; }); @@ -129,4 +128,4 @@ diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopRe { return stop_reason_diag_; } -} // namespace autoware::behavior_velocity_planner +} // namespace behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp similarity index 93% rename from planning/autoware_behavior_velocity_planner/src/planner_manager.hpp rename to planning/behavior_velocity_planner/src/planner_manager.hpp index 8fea862d80943..7485e5faba8bf 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -36,10 +36,8 @@ #include #include -namespace autoware::behavior_velocity_planner +namespace behavior_velocity_planner { -using ::behavior_velocity_planner::PlannerData; - class BehaviorVelocityPlannerManager { public: @@ -58,6 +56,6 @@ class BehaviorVelocityPlannerManager pluginlib::ClassLoader plugin_loader_; std::vector> scene_manager_plugins_; }; -} // namespace autoware::behavior_velocity_planner +} // namespace behavior_velocity_planner #endif // PLANNER_MANAGER_HPP_ diff --git a/planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv b/planning/behavior_velocity_planner/srv/LoadPlugin.srv similarity index 100% rename from planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv rename to planning/behavior_velocity_planner/srv/LoadPlugin.srv diff --git a/planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/behavior_velocity_planner/srv/UnloadPlugin.srv similarity index 100% rename from planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv rename to planning/behavior_velocity_planner/srv/UnloadPlugin.srv diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp similarity index 98% rename from planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp rename to planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 06c1bc48cf529..f70259b6a1f80 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -23,7 +23,7 @@ #include #include -using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; +using behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -50,7 +50,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); + ament_index_cpp::get_package_share_directory("behavior_velocity_planner"); const auto motion_velocity_smoother_dir = ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp index a86b96761ac70..624da6b170d06 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp @@ -22,23 +22,22 @@ #include -namespace autoware::behavior_velocity_planner +namespace behavior_velocity_planner { class PluginInterface { - public: virtual ~PluginInterface() = default; virtual void init(rclcpp::Node & node) = 0; virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) = 0; virtual void updateSceneModuleInstances( - const std::shared_ptr & planner_data, + const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::optional getFirstStopPathPointIndex() = 0; virtual const char * getModuleName() = 0; }; -} // namespace autoware::behavior_velocity_planner +} // namespace behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp index 51116600c21d1..7aa3e6bfef9ab 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -20,7 +20,7 @@ #include #include -namespace autoware::behavior_velocity_planner +namespace behavior_velocity_planner { template @@ -33,7 +33,7 @@ class PluginWrapper : public PluginInterface scene_manager_->plan(path); }; void updateSceneModuleInstances( - const std::shared_ptr & planner_data, + const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override { scene_manager_->updateSceneModuleInstances(planner_data, path); @@ -48,6 +48,6 @@ class PluginWrapper : public PluginInterface std::unique_ptr scene_manager_; }; -} // namespace autoware::behavior_velocity_planner +} // namespace behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_run_out_module/plugins.xml b/planning/behavior_velocity_run_out_module/plugins.xml index f9f4dde807796..5320d46f92929 100644 --- a/planning/behavior_velocity_run_out_module/plugins.xml +++ b/planning/behavior_velocity_run_out_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index f24a4d930f38d..1c85a22f57bf6 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -204,4 +204,4 @@ void RunOutModuleManager::setDynamicObstacleCreator( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::RunOutModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::RunOutModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_run_out_module/src/manager.hpp index cc76e77f4ec16..4dd66ad45898b 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_run_out_module/src/manager.hpp @@ -45,7 +45,7 @@ class RunOutModuleManager : public SceneModuleManagerInterface void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; -class RunOutModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class RunOutModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_speed_bump_module/plugins.xml b/planning/behavior_velocity_speed_bump_module/plugins.xml index 8d748aa832c2c..506d25669f8cf 100644 --- a/planning/behavior_velocity_speed_bump_module/plugins.xml +++ b/planning/behavior_velocity_speed_bump_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index fc29dd15a6003..5949357c90ff3 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -85,4 +85,4 @@ SpeedBumpModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::SpeedBumpModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::SpeedBumpModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp index 56027de6ccf96..480208067d87e 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -45,7 +45,7 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class SpeedBumpModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class SpeedBumpModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_stop_line_module/plugins.xml index a4cc6bf728b8b..51fb225fbebad 100644 --- a/planning/behavior_velocity_stop_line_module/plugins.xml +++ b/planning/behavior_velocity_stop_line_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 37809d6947cf8..5c9f0fd39c644 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -109,4 +109,4 @@ StopLineModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::StopLineModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::StopLineModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp index 82aab7a3ebae7..4b665a3a536f7 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.hpp @@ -58,7 +58,7 @@ class StopLineModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class StopLineModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class StopLineModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_template_module/plugins.xml b/planning/behavior_velocity_template_module/plugins.xml index 4fd1b111f41b2..3560c84e6a080 100644 --- a/planning/behavior_velocity_template_module/plugins.xml +++ b/planning/behavior_velocity_template_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_template_module/src/manager.cpp index a01eb8e162791..22763f44af7bf 100644 --- a/planning/behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_template_module/src/manager.cpp @@ -60,4 +60,4 @@ TemplateModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::TemplateModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::TemplateModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_template_module/src/manager.hpp index a2e5ffd765495..ef4e1f00cae92 100644 --- a/planning/behavior_velocity_template_module/src/manager.hpp +++ b/planning/behavior_velocity_template_module/src/manager.hpp @@ -84,7 +84,7 @@ class TemplateModuleManager : public SceneModuleManagerInterface * The TemplateModulePlugin class is used to integrate the TemplateModuleManager into the Behavior * Velocity Planner. */ -class TemplateModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class TemplateModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_traffic_light_module/plugins.xml b/planning/behavior_velocity_traffic_light_module/plugins.xml index de2780bf29882..b65cc66c5c232 100644 --- a/planning/behavior_velocity_traffic_light_module/plugins.xml +++ b/planning/behavior_velocity_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index ceb904305e942..edddef5cef4d8 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -193,4 +193,4 @@ bool TrafficLightModuleManager::hasSameTrafficLight( #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::TrafficLightModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::TrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index c490e94487cac..959ef2f91d36c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -61,7 +61,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC std::optional nearest_ref_stop_path_point_index_; }; -class TrafficLightModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class TrafficLightModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml index d6f74b780f11a..943ef175aa8f8 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml +++ b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 699e1b231acfb..5310fae78c294 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -81,4 +81,4 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( behavior_velocity_planner::VirtualTrafficLightModulePlugin, - autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp index eff8fc78d9d83..15bd6f468132e 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -44,7 +44,7 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; -class VirtualTrafficLightModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class VirtualTrafficLightModulePlugin : public PluginWrapper { }; diff --git a/planning/behavior_velocity_walkway_module/plugins.xml b/planning/behavior_velocity_walkway_module/plugins.xml index 9b1884a8cda30..971a49f2cb044 100644 --- a/planning/behavior_velocity_walkway_module/plugins.xml +++ b/planning/behavior_velocity_walkway_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index 0f129d72e7da9..d427e57009cf6 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -104,4 +104,4 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::WalkwayModulePlugin, autoware::behavior_velocity_planner::PluginInterface) + behavior_velocity_planner::WalkwayModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp index f2872e94a6904..453fcdb40f0db 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -52,7 +52,7 @@ class WalkwayModuleManager : public SceneModuleManagerInterface const PathWithLaneId & path) override; }; -class WalkwayModulePlugin : public autoware::behavior_velocity_planner::PluginWrapper +class WalkwayModulePlugin : public PluginWrapper { }; From 5a40e4a6a9dbc7a14ad047810f658411e510b68c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 22 May 2024 13:51:08 +0200 Subject: [PATCH 47/53] refactor: only rename the folder, not the package Signed-off-by: Esteve Fernandez --- .../lane_driving/behavior_planning/behavior_planning.launch.xml | 2 +- launch/tier4_planning_launch/package.xml | 2 +- .../CMakeLists.txt | 0 .../README.md | 0 .../config/behavior_velocity_planner.param.yaml | 0 .../docs/BehaviorVelocityPlanner-Architecture.drawio.svg | 0 .../docs/set_stop_velocity.drawio.svg | 0 .../launch/behavior_velocity_planner.launch.xml | 0 .../package.xml | 0 .../schema/behavior_velocity_planner.schema.json | 0 .../src/node.cpp | 0 .../src/node.hpp | 0 .../src/planner_manager.cpp | 0 .../src/planner_manager.hpp | 0 .../srv/LoadPlugin.srv | 0 .../srv/UnloadPlugin.srv | 0 .../test/src/test_node_interface.cpp | 0 planning/autoware_static_centerline_generator/package.xml | 2 +- .../test/test_static_centerline_generator.test.py | 2 +- 19 files changed, 4 insertions(+), 4 deletions(-) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/CMakeLists.txt (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/README.md (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/config/behavior_velocity_planner.param.yaml (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/docs/BehaviorVelocityPlanner-Architecture.drawio.svg (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/docs/set_stop_velocity.drawio.svg (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/launch/behavior_velocity_planner.launch.xml (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/package.xml (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/schema/behavior_velocity_planner.schema.json (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/node.cpp (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/node.hpp (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/planner_manager.cpp (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/src/planner_manager.hpp (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/srv/LoadPlugin.srv (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/srv/UnloadPlugin.srv (100%) rename planning/{behavior_velocity_planner => autoware_behavior_velocity_planner}/test/src/test_node_interface.cpp (100%) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 57d0280330641..865ab99e870e4 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -220,7 +220,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 84cadcd3dccf6..fcc697ab45de4 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,7 +58,7 @@ autoware_cmake autoware_behavior_path_planner - autoware_behavior_velocity_planner + behavior_velocity_planner autoware_remaining_distance_time_calculator costmap_generator external_cmd_selector diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/autoware_behavior_velocity_planner/CMakeLists.txt similarity index 100% rename from planning/behavior_velocity_planner/CMakeLists.txt rename to planning/autoware_behavior_velocity_planner/CMakeLists.txt diff --git a/planning/behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md similarity index 100% rename from planning/behavior_velocity_planner/README.md rename to planning/autoware_behavior_velocity_planner/README.md diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml rename to planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml diff --git a/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg similarity index 100% rename from planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg rename to planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg diff --git a/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg b/planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg rename to planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml similarity index 100% rename from planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml rename to planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml diff --git a/planning/behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml similarity index 100% rename from planning/behavior_velocity_planner/package.xml rename to planning/autoware_behavior_velocity_planner/package.xml diff --git a/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json similarity index 100% rename from planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json rename to planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp similarity index 100% rename from planning/behavior_velocity_planner/src/node.cpp rename to planning/autoware_behavior_velocity_planner/src/node.cpp diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp similarity index 100% rename from planning/behavior_velocity_planner/src/node.hpp rename to planning/autoware_behavior_velocity_planner/src/node.hpp diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp similarity index 100% rename from planning/behavior_velocity_planner/src/planner_manager.cpp rename to planning/autoware_behavior_velocity_planner/src/planner_manager.cpp diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp similarity index 100% rename from planning/behavior_velocity_planner/src/planner_manager.hpp rename to planning/autoware_behavior_velocity_planner/src/planner_manager.hpp diff --git a/planning/behavior_velocity_planner/srv/LoadPlugin.srv b/planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/srv/LoadPlugin.srv rename to planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv diff --git a/planning/behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/srv/UnloadPlugin.srv rename to planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp similarity index 100% rename from planning/behavior_velocity_planner/test/src/test_node_interface.cpp rename to planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 56d30a715c006..61b09447d3076 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -43,7 +43,7 @@ ament_cmake_gmock ament_lint_auto - autoware_behavior_velocity_planner + behavior_velocity_planner autoware_lint_common behavior_path_planner ros_testing diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index dc3c26798b12d..3011abccd48ca 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -59,7 +59,7 @@ def generate_test_description(): "config/behavior_path_planner.param.yaml", ), os.path.join( - get_package_share_directory("autoware_behavior_velocity_planner"), + get_package_share_directory("behavior_velocity_planner"), "config/behavior_velocity_planner.param.yaml", ), os.path.join( From 880862af7bb8cf122bbf6ae0d6cccfaeaaf0ff37 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 22 May 2024 15:09:43 +0200 Subject: [PATCH 48/53] refactor: fix package.xml dependencies Signed-off-by: Esteve Fernandez --- launch/tier4_planning_launch/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index fcc697ab45de4..9d39c397f9417 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -57,9 +57,9 @@ ament_cmake_auto autoware_cmake - autoware_behavior_path_planner - behavior_velocity_planner + autoware_behavior_velocity_planner autoware_remaining_distance_time_calculator + behavior_path_planner costmap_generator external_cmd_selector external_velocity_limit_selector From 6f66fcf03b708deb40ab8e87af747020220682c7 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 22 May 2024 17:08:09 +0200 Subject: [PATCH 49/53] refactor: behavior_velocity_planner package renamed to autoware_behavior_velocity_planner . BehaviorVelocityPlannerNode loads, but plugins do not. Signed-off-by: Esteve Fernandez --- .../behavior_planning/behavior_planning.launch.xml | 2 +- .../autoware_behavior_velocity_planner/CMakeLists.txt | 2 +- planning/autoware_behavior_velocity_planner/package.xml | 4 ++-- planning/autoware_behavior_velocity_planner/src/node.hpp | 8 ++++---- .../src/planner_manager.cpp | 2 +- planning/autoware_static_centerline_generator/package.xml | 2 +- 6 files changed, 10 insertions(+), 10 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 865ab99e870e4..fab88983d4507 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -220,7 +220,7 @@ - + diff --git a/planning/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/autoware_behavior_velocity_planner/CMakeLists.txt index ee7dd7c07a77a..20837553ea689 100644 --- a/planning/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_planner) +project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index 1357b555f0ba4..573d862f1725b 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_planner + autoware_behavior_velocity_planner 0.1.0 - The behavior_velocity_planner package + The autoware_behavior_velocity_planner package Mamoru Sobue Takayuki Murooka diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index dd6edf6ae0bc0..470f208326dfc 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -18,8 +18,8 @@ #include "planner_manager.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include +#include +#include #include #include #include @@ -47,8 +47,8 @@ namespace behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; -using behavior_velocity_planner::srv::LoadPlugin; -using behavior_velocity_planner::srv::UnloadPlugin; +using autoware_behavior_velocity_planner::srv::LoadPlugin; +using autoware_behavior_velocity_planner::srv::UnloadPlugin; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index cb491920c87d4..2e963071e6de7 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -50,7 +50,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") +: plugin_loader_("autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") { } diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 61b09447d3076..56d30a715c006 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -43,7 +43,7 @@ ament_cmake_gmock ament_lint_auto - behavior_velocity_planner + autoware_behavior_velocity_planner autoware_lint_common behavior_path_planner ros_testing From 70db5f78841ef70de3b8c8d0bab2916d8387f0f0 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 22 May 2024 17:14:14 +0200 Subject: [PATCH 50/53] refactor: update plugins export Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_blind_spot_module/CMakeLists.txt | 2 +- planning/behavior_velocity_crosswalk_module/CMakeLists.txt | 2 +- planning/behavior_velocity_detection_area_module/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- planning/behavior_velocity_intersection_module/CMakeLists.txt | 2 +- .../behavior_velocity_no_drivable_lane_module/CMakeLists.txt | 2 +- .../behavior_velocity_no_stopping_area_module/CMakeLists.txt | 2 +- planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt | 2 +- planning/behavior_velocity_out_of_lane_module/CMakeLists.txt | 2 +- planning/behavior_velocity_run_out_module/CMakeLists.txt | 2 +- planning/behavior_velocity_speed_bump_module/CMakeLists.txt | 2 +- planning/behavior_velocity_stop_line_module/CMakeLists.txt | 2 +- planning/behavior_velocity_template_module/CMakeLists.txt | 2 +- planning/behavior_velocity_traffic_light_module/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- planning/behavior_velocity_walkway_module/CMakeLists.txt | 2 +- 16 files changed, 16 insertions(+), 16 deletions(-) diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt index 682da3ae58d7e..8af66318b3fd3 100644 --- a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_blind_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 69dd1ced695ec..f21d5ab786cad 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_crosswalk_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY diff --git a/planning/behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_detection_area_module/CMakeLists.txt index 2a16ed95ba655..e6990d2d28642 100644 --- a/planning/behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_detection_area_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_detection_area_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt index 20f4b3aa8f4d3..481ac245cb432 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_dynamic_obstacle_stop_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 07847a08c1209..c02b5a3852722 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_intersection_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) find_package(OpenCV REQUIRED) diff --git a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt index ef3073df56c22..265b5b637e7ce 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_no_drivable_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt index 4d14228c4685c..ba3fd0e4f71dd 100644 --- a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_no_stopping_area_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt index 7a062ee854421..8b327291a60f1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_occlusion_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 8ce1a334c4b35..e8191f9b632d5 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_out_of_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt index 21758c4d0198c..3ee589ba69740 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_run_out_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt index a96eb3398e5c0..f5d3a7078be9c 100644 --- a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt +++ b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_speed_bump_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_stop_line_module/CMakeLists.txt index 3c8cc6a4f9626..bef98aafe6f75 100644 --- a/planning/behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_stop_line_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_stop_line_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_template_module/CMakeLists.txt b/planning/behavior_velocity_template_module/CMakeLists.txt index e0beb42daf50d..103a3d0a4ceb3 100644 --- a/planning/behavior_velocity_template_module/CMakeLists.txt +++ b/planning/behavior_velocity_template_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_template_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/manager.cpp diff --git a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt index cf9392fa0568f..5737802408996 100644 --- a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt index 48ffa35cbae5e..a1c00cf49db29 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_virtual_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_walkway_module/CMakeLists.txt b/planning/behavior_velocity_walkway_module/CMakeLists.txt index cd41dec991bc6..351a240743402 100644 --- a/planning/behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/behavior_velocity_walkway_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_walkway_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp From e4273ee7d490159e0eb7be57ab319b9a10851ee0 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 23 May 2024 12:28:43 +0200 Subject: [PATCH 51/53] refactor(autoware_behavior_velocity_planner): move code to autoware::behavior_velocity_planner namespace Signed-off-by: Esteve Fernandez --- .../behavior_planning/behavior_planning.launch.xml | 2 +- .../CMakeLists.txt | 2 +- .../launch/behavior_velocity_planner.launch.xml | 2 +- .../autoware_behavior_velocity_planner/src/node.cpp | 12 ++++++------ .../autoware_behavior_velocity_planner/src/node.hpp | 5 +++-- .../src/planner_manager.cpp | 4 ++-- .../src/planner_manager.hpp | 7 +++++-- .../test/src/test_node_interface.cpp | 4 ++-- 8 files changed, 21 insertions(+), 17 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index fab88983d4507..57d0280330641 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -220,7 +220,7 @@ - + diff --git a/planning/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/autoware_behavior_velocity_planner/CMakeLists.txt index 20837553ea689..37a02b844dfe9 100644 --- a/planning/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_planner/CMakeLists.txt @@ -19,7 +19,7 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_lib - PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" + PLUGIN "autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index c1631fdb739de..d4478f87f0610 100644 --- a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -30,7 +30,7 @@ - + diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index 1801db4658a72..3105a22927ad3 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -52,7 +52,7 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -436,14 +436,14 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation const auto interpolated_path_msg = - interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); + ::behavior_velocity_planner::interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = filterStopPathPoint(interpolated_path_msg); + output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); @@ -477,7 +477,7 @@ void BehaviorVelocityPlannerNode::publishDebugMarker( } debug_viz_pub_->publish(output_msg); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include -RCLCPP_COMPONENTS_REGISTER_NODE(behavior_velocity_planner::BehaviorVelocityPlannerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode) diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index 470f208326dfc..d727a28557465 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -44,12 +44,13 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; using tier4_planning_msgs::msg::VelocityLimit; +using ::behavior_velocity_planner::TrafficSignalStamped; class BehaviorVelocityPlannerNode : public rclcpp::Node { @@ -134,6 +135,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // NODE_HPP_ diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index 2e963071e6de7..f939f67a06f9e 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -19,7 +19,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -128,4 +128,4 @@ diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopRe { return stop_reason_diag_; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index 7485e5faba8bf..f9c1d0253de62 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -36,8 +36,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PlannerData; +using ::behavior_velocity_planner::PluginInterface; + class BehaviorVelocityPlannerManager { public: @@ -56,6 +59,6 @@ class BehaviorVelocityPlannerManager pluginlib::ClassLoader plugin_loader_; std::vector> scene_manager_plugins_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // PLANNER_MANAGER_HPP_ diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index f70259b6a1f80..06c1bc48cf529 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -23,7 +23,7 @@ #include #include -using behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -50,7 +50,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("behavior_velocity_planner"); + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); const auto motion_velocity_smoother_dir = ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); From 3d98f31d1a8d82f35dd777e0ec258da8f940008e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 23 May 2024 10:33:23 +0000 Subject: [PATCH 52/53] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/autoware_behavior_velocity_planner/src/node.cpp | 7 ++++--- planning/autoware_behavior_velocity_planner/src/node.hpp | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index 3105a22927ad3..d4ae3897bca65 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -436,11 +436,12 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = + ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = - ::behavior_velocity_planner::interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); + const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index d727a28557465..6e74203460542 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -49,8 +49,8 @@ namespace autoware::behavior_velocity_planner using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; -using tier4_planning_msgs::msg::VelocityLimit; using ::behavior_velocity_planner::TrafficSignalStamped; +using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node { From f8b4cb6f37fd20084b55310eb56cc2e15f485b52 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 24 May 2024 13:43:19 +0200 Subject: [PATCH 53/53] fix: fix package name Signed-off-by: Esteve Fernandez --- .../test/test_static_centerline_generator.test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index 3011abccd48ca..dc3c26798b12d 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -59,7 +59,7 @@ def generate_test_description(): "config/behavior_path_planner.param.yaml", ), os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("autoware_behavior_velocity_planner"), "config/behavior_velocity_planner.param.yaml", ), os.path.join(