diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index fb7f1ab6327a1..d72603074c279 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -82,9 +82,9 @@ void visualizeBound( } const float adjusted_width = width / std::sin(diff_angle); if (std::abs(adjusted_width) > 1.0) { - return std::make_pair(normal_vector_angle, adjusted_width / std::abs(adjusted_width)); + return std::make_pair(normal_vector_angle, adjusted_width / std::abs(adjusted_width)); } else { - return std::make_pair(normal_vector_angle, adjusted_width); + return std::make_pair(normal_vector_angle, adjusted_width); } }(); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index eb8622261461b..3fa93ded608e0 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1582,15 +1582,19 @@ std::vector postProcess( calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length); const auto p_tmp = geometry_msgs::build().position(goal_point).orientation(goal_pose.orientation); - const size_t goal_nearest_idx = findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2); - const size_t goal_idx = ((goal_start_idx - start_idx)*(goal_start_idx - start_idx) > (goal_nearest_idx - start_idx)*(goal_nearest_idx - start_idx)) ? goal_start_idx : goal_nearest_idx; + const size_t goal_nearest_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2); + const size_t goal_idx = ((goal_start_idx - start_idx) * (goal_start_idx - start_idx) > + (goal_nearest_idx - start_idx) * (goal_nearest_idx - start_idx)) + ? goal_start_idx + : goal_nearest_idx; return std::make_pair(goal_idx, goal_point); }(); // Insert middle points size_t step = (start_idx < goal_idx) ? 1 : -1; for (size_t i = start_idx + step; i != goal_idx + step; i += step) { - const auto &next_point = tmp_bound.at(i); + const auto & next_point = tmp_bound.at(i); const double dist = tier4_autoware_utils::calcDistance2d(processed_bound.back(), next_point); if (dist > overlap_threshold) { processed_bound.push_back(next_point);