From 6455b58011211919618d6c7547ddb080d2be79f1 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 19 Dec 2023 18:02:19 +0900 Subject: [PATCH] Update for rebase Signed-off-by: Maxime CLEMENT --- .../src/collision.cpp | 6 ++++-- .../src/manager.cpp | 19 +++++++------------ .../src/object_filtering.cpp | 9 ++++++--- .../src/scene_dynamic_obstacle_stop.cpp | 2 +- .../behavior_velocity_planner.param.yaml | 1 + 5 files changed, 19 insertions(+), 18 deletions(-) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp index 46c58d6be8805..a06580fbbaac5 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -31,7 +31,9 @@ std::optional 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 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 earliest_collision_point; debug_data.collisions.clear(); std::vector rough_collisions; @@ -55,7 +57,7 @@ std::optional 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}; 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 99981007b20c2..226bd3b19a415 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -16,29 +16,24 @@ #include "scene_dynamic_obstacle_stop.hpp" -#include - #include 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(node, ns + ".extra_object_width"); - pp.minimum_object_velocity = getOrDeclareParameter(node, ns + ".minimum_object_velocity"); - pp.stop_distance_buffer = getOrDeclareParameter(node, ns + ".stop_distance_buffer"); - pp.time_horizon = getOrDeclareParameter(node, ns + ".time_horizon"); - pp.hysteresis = getOrDeclareParameter(node, ns + ".hysteresis"); - pp.decision_duration_buffer = - getOrDeclareParameter(node, ns + ".decision_duration_buffer"); + pp.extra_object_width = node.declare_parameter(ns + ".extra_object_width"); + pp.minimum_object_velocity = node.declare_parameter(ns + ".minimum_object_velocity"); + pp.stop_distance_buffer = node.declare_parameter(ns + ".stop_distance_buffer"); + pp.time_horizon = node.declare_parameter(ns + ".time_horizon"); + pp.hysteresis = node.declare_parameter(ns + ".hysteresis"); + pp.decision_duration_buffer = node.declare_parameter(ns + ".decision_duration_buffer"); pp.minimum_object_distance_from_ego_path = - getOrDeclareParameter(node, ns + ".minimum_object_distance_from_ego_path"); + node.declare_parameter(ns + ".minimum_object_distance_from_ego_path"); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); pp.ego_lateral_offset = diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 0411ab2d01cfe..0e11223f54158 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -18,6 +18,8 @@ #include +#include + #include #include @@ -46,16 +48,17 @@ std::vector filter_predicte c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; }) != o.classification.end(); }; + std::vector 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; }; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp index 98b7984bab1dc..5deac35403550 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp +++ b/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) diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index e8377f841aa77..6fd666ea67a4e 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/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