Update for rebase

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp

@@ -31,7 +31,9 @@ std::optional<geometry_msgs::msg::Point> find_earliest_collision(
   const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data)
 {
   auto earliest_idx = ego_data.path_footprints.size();
-  auto earliest_arc_length = motion_utils::calcArcLength(ego_data.path.points);
+  std::vector<autoware_auto_planning_msgs::msg::PathPoint> path_points;
+  for (const auto & p : ego_data.path.points) path_points.push_back(p.point);
+  auto earliest_arc_length = motion_utils::calcArcLength(path_points);
   std::optional<geometry_msgs::msg::Point> earliest_collision_point;
   debug_data.collisions.clear();
   std::vector<BoxIndexPair> rough_collisions;
@@ -55,7 +57,7 @@ std::optional<geometry_msgs::msg::Point> find_earliest_collision(
         for (const auto & coll_p : collision_points) {
           auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y());
           const auto arc_length =
-            motion_utils::calcSignedArcLength(ego_data.path.points, ego_data.first_path_idx, p);
+            motion_utils::calcSignedArcLength(path_points, ego_data.first_path_idx, p);
           if (arc_length < earliest_arc_length) {
             earliest_arc_length = arc_length;
             debug_data.collisions = {obstacle_footprint, ego_footprint};

planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp

@@ -16,29 +16,24 @@
 
 #include "scene_dynamic_obstacle_stop.hpp"
 
-#include <tier4_autoware_utils/ros/parameter.hpp>
-
 #include <algorithm>
 
 namespace behavior_velocity_planner
 {
-using tier4_autoware_utils::getOrDeclareParameter;
-
 DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node & node)
 : SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL)
 {
   const std::string ns(getModuleName());
   auto & pp = planner_param_;
 
-  pp.extra_object_width = getOrDeclareParameter<double>(node, ns + ".extra_object_width");
-  pp.minimum_object_velocity = getOrDeclareParameter<double>(node, ns + ".minimum_object_velocity");
-  pp.stop_distance_buffer = getOrDeclareParameter<double>(node, ns + ".stop_distance_buffer");
-  pp.time_horizon = getOrDeclareParameter<double>(node, ns + ".time_horizon");
-  pp.hysteresis = getOrDeclareParameter<double>(node, ns + ".hysteresis");
-  pp.decision_duration_buffer =
-    getOrDeclareParameter<double>(node, ns + ".decision_duration_buffer");
+  pp.extra_object_width = node.declare_parameter<double>(ns + ".extra_object_width");
+  pp.minimum_object_velocity = node.declare_parameter<double>(ns + ".minimum_object_velocity");
+  pp.stop_distance_buffer = node.declare_parameter<double>(ns + ".stop_distance_buffer");
+  pp.time_horizon = node.declare_parameter<double>(ns + ".time_horizon");
+  pp.hysteresis = node.declare_parameter<double>(ns + ".hysteresis");
+  pp.decision_duration_buffer = node.declare_parameter<double>(ns + ".decision_duration_buffer");
   pp.minimum_object_distance_from_ego_path =
-    getOrDeclareParameter<double>(node, ns + ".minimum_object_distance_from_ego_path");
+    node.declare_parameter<double>(ns + ".minimum_object_distance_from_ego_path");
 
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
   pp.ego_lateral_offset =

planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

@@ -18,6 +18,8 @@
 
 #include <motion_utils/trajectory/trajectory.hpp>
 
+#include <autoware_auto_planning_msgs/msg/path_point.hpp>
+
 #include <algorithm>
 #include <vector>
 
@@ -46,16 +48,17 @@ std::vector<autoware_auto_perception_msgs::msg::PredictedObject> filter_predicte
                     c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE;
            }) != o.classification.end();
   };
+  std::vector<autoware_auto_planning_msgs::msg::PathPoint> path_points;
+  for (const auto & p : ego_data.path.points) path_points.push_back(p.point);
   const auto is_in_range = [&](const auto & o) {
     const auto distance = std::abs(motion_utils::calcLateralOffset(
-      ego_data.path.points, o.kinematics.initial_pose_with_covariance.pose.position));
+      path_points, o.kinematics.initial_pose_with_covariance.pose.position));
     return distance <= params.minimum_object_distance_from_ego_path + params.ego_lateral_offset +
                          o.shape.dimensions.y / 2.0 + hysteresis;
   };
   const auto is_not_too_close = [&](const auto & o) {
     const auto obj_arc_length = motion_utils::calcSignedArcLength(
-      ego_data.path.points, ego_data.pose.position,
-      o.kinematics.initial_pose_with_covariance.pose.position);
+      path_points, ego_data.pose.position, o.kinematics.initial_pose_with_covariance.pose.position);
     return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx +
                               params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0;
   };

planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp

@@ -45,7 +45,7 @@ DynamicObstacleStopModule::DynamicObstacleStopModule(
 : SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param))
 {
   prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type());
-  velocity_factor_.init(PlanningBehavior::UNKNOWN);
+  velocity_factor_.init(VelocityFactor::UNKNOWN);
 }
 
 bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)

planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml

@@ -24,3 +24,4 @@
       # behavior_velocity_planner::SpeedBumpModulePlugin
       - behavior_velocity_planner::OutOfLaneModulePlugin
       - behavior_velocity_planner::NoDrivableLaneModulePlugin
+      - behavior_velocity_planner::DynamicObstacleStopModulePlugin