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 67% 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..fdfd3a25eb3ad 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,35 +25,31 @@ #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; } + [[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 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 77% 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..eabd00fae85d6 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,23 +12,24 @@ // 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, 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; } -} // 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)