diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml index a74245d57c6d9..8f08bfab92542 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml @@ -14,4 +14,4 @@ ignore_unavoidable_collisions : true # if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) ignore_objects_behind_ego: false # if true, ignore objects that are behind the ego vehicle behind_object_distance_threshold: 5.0 # [m] distance behind the ego vehicle to ignore objects - use_predicted_path: false # if true, use the predicted path of the object to calculate the collision point \ No newline at end of file + use_predicted_path: false # if true, use the predicted path of the object to calculate the collision point diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 8cd84cfbed663..73b42b805d286 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -61,8 +61,9 @@ tier4_autoware_utils::Polygon2d make_forward_footprint( {}}; } -//create footprint using predicted_path of object -tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y) +// create footprint using predicted_path of object +tier4_autoware_utils::Polygon2d translate_polygon( + const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y) { tier4_autoware_utils::Polygon2d translated_polygon; const boost::geometry::strategy::transform::translate_transformer translation(x, y); @@ -70,7 +71,8 @@ tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Po return translated_polygon; } -tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d& base_footprint) +tier4_autoware_utils::Polygon2d create_footprint( + const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d & base_footprint) { const auto angle = tf2::getYaw(pose.orientation); return translate_polygon( @@ -78,7 +80,8 @@ tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose } tier4_autoware_utils::MultiPolygon2d create_object_footprints( - const std::vector & objects, const PlannerParam & params) + const std::vector & objects, + const PlannerParam & params) { tier4_autoware_utils::MultiPolygon2d footprints; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index bf98545f5dc0b..9e02d1ce6b04e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include + #include #include @@ -42,9 +43,12 @@ tier4_autoware_utils::Polygon2d make_forward_footprint( const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis); tier4_autoware_utils::MultiPolygon2d create_object_footprints( - const std::vector & objects, const PlannerParam & params); -tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y); -tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d& base_footprint); + const std::vector & objects, + const PlannerParam & params); +tier4_autoware_utils::Polygon2d translate_polygon( + const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y); +tier4_autoware_utils::Polygon2d create_footprint( + const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d & base_footprint); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose 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 04d4bb892b085..a39cbe2363080 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -49,8 +49,7 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node getOrDeclareParameter(node, ns + ".ignore_unavoidable_collisions"); pp.ignore_objects_behind_ego = getOrDeclareParameter(node, ns + ".ignore_objects_behind_ego"); - pp.use_predicted_path = - getOrDeclareParameter(node, ns + ".use_predicted_path"); + pp.use_predicted_path = getOrDeclareParameter(node, ns + ".use_predicted_path"); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); pp.ego_lateral_offset = std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); 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 e62eb07918def..e046e94b34b25 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 @@ -90,7 +90,7 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe tier4_autoware_utils::MultiPolygon2d obstacle_predicted_footprints; std::vector collisions; double footprints_duration_us; - if (params_.use_predicted_path){ + if (params_.use_predicted_path) { stopwatch.tic("footprints"); obstacle_predicted_footprints = create_object_footprints(dynamic_obstacles, params_); footprints_duration_us = stopwatch.toc("footprints"); @@ -100,12 +100,10 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe debug_data_.obstacle_footprints = obstacle_predicted_footprints; } else { stopwatch.tic("footprints"); - obstacle_forward_footprints = - make_forward_footprints(dynamic_obstacles, params_, hysteresis); + obstacle_forward_footprints = make_forward_footprints(dynamic_obstacles, params_, hysteresis); footprints_duration_us = stopwatch.toc("footprints"); stopwatch.tic("collisions"); - collisions = - find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_); + collisions = find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_); debug_data_.obstacle_footprints = obstacle_forward_footprints; } update_object_map(object_map_, collisions, clock_->now(), ego_data.path.points, params_);