refactor(behavior_velocity_planner_common): move VelocityFactorInterf…

…ace to motion_utils (autowarefoundation#7007)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Signed-off-by: vividf <yihsiang.fang@tier4.jp>

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)

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp → 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 <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
@@ -25,35 +25,31 @@
 #include <string>
 #include <vector>
 
-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<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & 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_

common/motion_utils/package.xml

@@ -21,6 +21,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>builtin_interfaces</depend>

planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp → 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 <behavior_velocity_planner_common/velocity_factor_interface.hpp>
+#include <motion_utils/factor/velocity_factor_interface.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 
-namespace behavior_velocity_planner
+namespace motion_utils
 {
 void VelocityFactorInterface::set(
   const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & 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<float>(motion_utils::calcSignedArcLength(points, curr_point, stop_point));
   velocity_factor_.status = status;
   velocity_factor_.detail = detail;
 }
 
-}  // namespace behavior_velocity_planner
+}  // namespace motion_utils

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -20,6 +20,7 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <motion_utils/factor/velocity_factor_interface.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>  // for toPolygon2d
 #include <tier4_autoware_utils/geometry/geometry.hpp>
@@ -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,

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

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 <behavior_velocity_planner_common/planner_data.hpp>
-#include <behavior_velocity_planner_common/velocity_factor_interface.hpp>
 #include <builtin_interfaces/msg/time.hpp>
+#include <motion_utils/factor/velocity_factor_interface.hpp>
 #include <motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
 #include <rtc_interface/rtc_interface.hpp>
@@ -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<const PlannerData> planner_data_;
   std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
   std::optional<int> first_stop_path_point_index_;
-  VelocityFactorInterface velocity_factor_;
+  motion_utils::VelocityFactorInterface velocity_factor_;
   std::vector<ObjectOfInterest> objects_of_interest_;
 
   void setSafe(const bool safe)