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 dc0cb0eb5188a..3d70f050dbc71 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 @@ -80,8 +80,12 @@ void visualizeBound( if (diff_angle <= 1e-7 && diff_angle >= -1e-7) { return std::make_pair(normal_vector_angle, width); } - - return std::make_pair(normal_vector_angle, width / std::sin(diff_angle)); + 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)); + } else { + return std::make_pair(normal_vector_angle, adjusted_width); + } }(); normal_vector_angles.push_back(normal_vector_angle); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index dc6f7552c5a61..5807530d18c6e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -207,8 +207,7 @@ void PlannerManager::generateCombinedDrivableArea( output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); } else if (di.is_already_expanded) { // for single side shift - utils::generateDrivableArea( - output.path, di.drivable_lanes, false, false, false, data, is_driving_forward); + utils::generateDrivableArea(output.path, di.drivable_lanes, false, false, false, data); } else { const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); @@ -220,8 +219,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data, - is_driving_forward); + di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data); } // extract obstacles from drivable area diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index f393ec18f8a32..49c0de3234509 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -42,7 +42,7 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool enable_expanding_freespace_areas, - const std::shared_ptr planner_data, const bool is_driving_forward = true); + const std::shared_ptr planner_data); void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, @@ -81,14 +81,12 @@ std::vector calcBound( const PathWithLaneId & path, const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool enable_expanding_freespace_areas, const bool is_left, - const bool is_driving_forward = true); + const bool enable_expanding_freespace_areas, const bool is_left); std::vector postProcess( const std::vector & original_bound, const PathWithLaneId & path, const std::shared_ptr planner_data, - const std::vector & drivable_lanes, const bool is_left, - const bool is_driving_forward = true); + const std::vector & drivable_lanes, const bool is_left); DrivableAreaInfo combineDrivableAreaInfo( const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 3d5908bc4e02f..41bd6d1a10b24 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -800,7 +800,7 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool enable_expanding_freespace_areas, - const std::shared_ptr planner_data, const bool is_driving_forward) + const std::shared_ptr planner_data) { if (path.points.empty()) { return; @@ -812,12 +812,10 @@ void generateDrivableArea( // Insert Position path.left_bound = calcBound( path, planner_data, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, enable_expanding_freespace_areas, true, - is_driving_forward); + enable_expanding_intersection_areas, enable_expanding_freespace_areas, true); path.right_bound = calcBound( path, planner_data, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, enable_expanding_freespace_areas, false, - is_driving_forward); + enable_expanding_intersection_areas, enable_expanding_freespace_areas, false); if (path.left_bound.empty() || path.right_bound.empty()) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; @@ -1459,8 +1457,7 @@ std::pair, bool> getBoundWithFreeSpaceAreas( std::vector postProcess( const std::vector & original_bound, const PathWithLaneId & path, const std::shared_ptr planner_data, - const std::vector & drivable_lanes, const bool is_left, - const bool is_driving_forward) + const std::vector & drivable_lanes, const bool is_left) { const auto lanelets = utils::transformToLanelets(drivable_lanes); const auto & current_pose = planner_data->self_odometry->pose.pose; @@ -1537,10 +1534,6 @@ std::vector postProcess( } } - if (!is_driving_forward) { - std::reverse(tmp_bound.begin(), tmp_bound.end()); - } - const auto start_idx = [&]() { const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); const auto cropped_path_points = autoware::motion_utils::cropPoints( @@ -1573,14 +1566,18 @@ 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_idx = std::max( - goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2)); - + 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 - for (size_t i = start_idx + 1; i <= goal_idx; ++i) { + 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 double dist = autoware::universe_utils::calcDistance2d(processed_bound.back(), next_point); @@ -1604,7 +1601,7 @@ std::vector calcBound( const PathWithLaneId & path, const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward) + const bool enable_expanding_freespace_areas, const bool is_left) { using autoware::motion_utils::removeOverlapPoints; @@ -1648,9 +1645,7 @@ std::vector calcBound( }(); const auto post_process = [&](const auto & bound, const auto skip) { - return skip - ? bound - : postProcess(bound, path, planner_data, drivable_lanes, is_left, is_driving_forward); + return skip ? bound : postProcess(bound, path, planner_data, drivable_lanes, is_left); }; // Step2. if there is no drivable area defined by polygon, return original drivable bound.