From afd80e07ec7d3de37d5ec8370db9f6ca2286f252 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 14 May 2024 15:05:43 +0900 Subject: [PATCH 1/2] refactor(behavior_velocity_planner_common): mv vel factor to motion_utils Signed-off-by: Maxime CLEMENT --- common/motion_utils/CMakeLists.txt | 10 +-------- .../factor}/velocity_factor_interface.hpp | 22 ++++++++----------- common/motion_utils/package.xml | 1 + .../src/factor}/velocity_factor_interface.cpp | 8 +++---- .../src/scene_intersection.cpp | 2 ++ .../CMakeLists.txt | 1 - .../scene_module_interface.hpp | 6 +++-- 7 files changed, 21 insertions(+), 29 deletions(-) rename {planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common => common/motion_utils/include/motion_utils/factor}/velocity_factor_interface.hpp (75%) rename {planning/behavior_velocity_planner_common/src => common/motion_utils/src/factor}/velocity_factor_interface.cpp (86%) diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index f42deaa7f8c41..cd81f16685d72 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -7,15 +7,7 @@ autoware_package() find_package(Boost REQUIRED) ament_auto_add_library(motion_utils SHARED - src/distance/distance.cpp - src/marker/marker_helper.cpp - src/marker/virtual_wall_marker_creator.cpp - src/resample/resample.cpp - src/trajectory/trajectory.cpp - src/trajectory/interpolation.cpp - src/trajectory/path_with_lane_id.cpp - src/trajectory/conversion.cpp - src/vehicle/vehicle_state_checker.cpp + DIRECTORY src ) if(BUILD_TESTING) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp similarity index 75% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp rename to common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp index eea45ec03807d..45f264bbe9f96 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp +++ b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp @@ -1,5 +1,5 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ +#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ #include #include @@ -25,21 +25,17 @@ #include #include -namespace behavior_velocity_planner +namespace motion_utils { - using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using geometry_msgs::msg::Pose; using VelocityFactorBehavior = VelocityFactor::_behavior_type; using VelocityFactorStatus = VelocityFactor::_status_type; +using geometry_msgs::msg::Pose; class VelocityFactorInterface { public: - VelocityFactorInterface() { behavior_ = VelocityFactor::UNKNOWN; } - VelocityFactor get() const { return velocity_factor_; } void init(const VelocityFactorBehavior behavior) { behavior_ = behavior; } void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } @@ -50,10 +46,10 @@ class VelocityFactorInterface const std::string detail = ""); private: - VelocityFactorBehavior behavior_; - VelocityFactor velocity_factor_; + VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; + VelocityFactor velocity_factor_{}; }; -} // namespace behavior_velocity_planner +} // namespace motion_utils -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ +#endif // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index 5f630572c061c..dec5287b0a520 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -21,6 +21,7 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs builtin_interfaces diff --git a/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp similarity index 86% rename from planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp rename to common/motion_utils/src/factor/velocity_factor_interface.cpp index 7bc46846afb83..f0e0c57926465 100644 --- a/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp +++ b/common/motion_utils/src/factor/velocity_factor_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2023-2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include -namespace behavior_velocity_planner +namespace motion_utils { void VelocityFactorInterface::set( const std::vector & points, @@ -31,4 +31,4 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } -} // namespace behavior_velocity_planner +} // namespace motion_utils diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 9d0c0c8e1defb..08e2d38a5905f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include // for toPolygon2d #include @@ -44,6 +45,7 @@ namespace bg = boost::geometry; using intersection::make_err; using intersection::make_ok; using intersection::Result; +using motion_utils::VelocityFactorInterface; IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner_common/CMakeLists.txt index e958df74afe2d..c8847164851e8 100644 --- a/planning/behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/behavior_velocity_planner_common/CMakeLists.txt @@ -6,7 +6,6 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/scene_module_interface.cpp - src/velocity_factor_interface.cpp src/utilization/path_utilization.cpp src/utilization/trajectory_utils.cpp src/utilization/arc_lane_util.cpp diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 082a88e306169..27db42d36f08b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -16,8 +16,8 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ #include -#include #include +#include #include #include #include @@ -50,6 +50,8 @@ namespace behavior_velocity_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; +using motion_utils::PlanningBehavior; +using motion_utils::VelocityFactor; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; @@ -128,7 +130,7 @@ class SceneModuleInterface std::shared_ptr planner_data_; std::optional infrastructure_command_; std::optional first_stop_path_point_index_; - VelocityFactorInterface velocity_factor_; + motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; void setSafe(const bool safe) From cabbb8a28321b87019a6d2c6707eea419c47e05e Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 14 May 2024 17:34:53 +0900 Subject: [PATCH 2/2] small improvements for clang-tidy Signed-off-by: Maxime CLEMENT --- .../motion_utils/factor/velocity_factor_interface.hpp | 6 +++--- .../motion_utils/src/factor/velocity_factor_interface.cpp | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp index 45f264bbe9f96..fdfd3a25eb3ad 100644 --- a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp +++ b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp @@ -36,14 +36,14 @@ using geometry_msgs::msg::Pose; class VelocityFactorInterface { public: - VelocityFactor get() const { return velocity_factor_; } - void init(const VelocityFactorBehavior behavior) { behavior_ = behavior; } + [[nodiscard]] VelocityFactor get() const { return velocity_factor_; } + void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } void set( const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string detail = ""); + const std::string & detail = ""); private: VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp index f0e0c57926465..eabd00fae85d6 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/motion_utils/src/factor/velocity_factor_interface.cpp @@ -20,13 +20,14 @@ namespace motion_utils void VelocityFactorInterface::set( const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, - const std::string detail) + const std::string & detail) { const auto & curr_point = curr_pose.position; const auto & stop_point = stop_pose.position; velocity_factor_.behavior = behavior_; velocity_factor_.pose = stop_pose; - velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); + velocity_factor_.distance = + static_cast(motion_utils::calcSignedArcLength(points, curr_point, stop_point)); velocity_factor_.status = status; velocity_factor_.detail = detail; }