From 042aeea64c03f41d6d520ff67213b5a52c9e1b88 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 11 Jul 2023 11:50:39 +0000 Subject: [PATCH] style(pre-commit): autofix --- .../config/obstacle_avoidance_planner.param.yaml | 2 +- .../include/obstacle_avoidance_planner/node.hpp | 3 +-- planning/obstacle_avoidance_planner/src/node.cpp | 6 ++++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 5e6f845ccf0f2..65b4c9fab3d21 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -18,7 +18,7 @@ # output output_delta_arc_length: 0.5 # delta arc length for output trajectory [m] output_backward_traj_length: 5.0 # backward length for backward trajectory from base_link [m] - + vehicle_stop_margin_outside_drivable_area: 1.5 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . # This margin will be realized with delta_arc_length_for_mpt_points m precision. diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index ffbb56ca6b9e8..76ea6305573f2 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -128,9 +128,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; void publishDebugMarkerOfOptimization(const std::vector & traj_points) const; - private: +private: double vehicle_stop_margin_outside_drivable_area_; - }; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index adaf483147212..d46bf4aca0681 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -456,11 +456,13 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( for (size_t i = *first_outside_idx; i < optimized_traj_points.size(); ++i) { size_t stop_idx = i; const auto & op_target_point = motion_utils::calcLongitudinalOffsetPoint( - optimized_traj_points, optimized_traj_points.at(i).pose.position, -1.0 * vehicle_stop_margin_outside_drivable_area_); + optimized_traj_points, optimized_traj_points.at(i).pose.position, + -1.0 * vehicle_stop_margin_outside_drivable_area_); if (op_target_point) { const auto target_point = op_target_point.get(); // confirm that target point doesn't overlap with the stop point outside drivable area - const auto dist = tier4_autoware_utils::calcDistance2d(optimized_traj_points.at(i), target_point); + const auto dist = + tier4_autoware_utils::calcDistance2d(optimized_traj_points.at(i), target_point); const double overlap_threshold = 1e-3; if (dist > overlap_threshold) { stop_idx = motion_utils::findNearestSegmentIndex(optimized_traj_points, target_point);