From b3bdedc584682fe6ab6310af4792312c36521e74 Mon Sep 17 00:00:00 2001 From: beyzanurkaya <32412808+beyzanurkaya@users.noreply.github.com> Date: Wed, 18 Oct 2023 00:40:26 +0300 Subject: [PATCH] fix(obstacle_avoidance_planner): fix lateral calculations (#4292) * use right_overhang and left_overhang for lateral calculation Signed-off-by: beyza * style(pre-commit): autofix * update debug_marker.cpp * style(pre-commit): autofix --------- Signed-off-by: beyza Co-authored-by: beyza Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/debug_marker.cpp | 56 +++++++++++-------- .../src/mpt_optimizer.cpp | 17 ++++-- 2 files changed, 43 insertions(+), 30 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index 8bf46c697104a..2f8babf103877 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -47,21 +47,22 @@ MarkerArray getFootprintsMarkerArray( const auto & traj_point = mpt_traj.at(i); - const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) .position); marker.points.push_back(marker.points.front()); @@ -71,8 +72,8 @@ MarkerArray getFootprintsMarkerArray( } MarkerArray getBoundsWidthMarkerArray( - const std::vector & ref_points, const double vehicle_width, - const size_t sampling_num) + const std::vector & ref_points, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); MarkerArray marker_array; @@ -105,8 +106,10 @@ MarkerArray getBoundsWidthMarkerArray( } const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double base_to_right = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double lb_y = - ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - vehicle_width / 2.0; + ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - base_to_right; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(pose.position); @@ -125,8 +128,10 @@ MarkerArray getBoundsWidthMarkerArray( } const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double base_to_left = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = - ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + vehicle_width / 2.0; + ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + base_to_left; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(pose.position); @@ -140,7 +145,8 @@ MarkerArray getBoundsWidthMarkerArray( } MarkerArray getBoundsLineMarkerArray( - const std::vector & ref_points, const double vehicle_width) + const std::vector & ref_points, + const vehicle_info_util::VehicleInfo & vehicle_info) { MarkerArray marker_array; @@ -158,12 +164,13 @@ MarkerArray getBoundsLineMarkerArray( for (size_t i = 0; i < ref_points.size(); i++) { const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose; - - const double ub_y = ref_points.at(i).bounds.upper_bound + vehicle_width / 2.0; + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; + const double ub_y = ref_points.at(i).bounds.upper_bound + base_to_left; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(ub); - const double lb_y = ref_points.at(i).bounds.lower_bound - vehicle_width / 2.0; + const double lb_y = ref_points.at(i).bounds.lower_bound - base_to_right; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(lb); } @@ -175,8 +182,9 @@ MarkerArray getBoundsLineMarkerArray( MarkerArray getVehicleCircleLinesMarkerArray( const std::vector & ref_points, - const std::vector & vehicle_circle_longitudinal_offsets, const double vehicle_width, - const size_t sampling_num, const std::string & ns) + const std::vector & vehicle_circle_longitudinal_offsets, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num, + const std::string & ns) { const auto current_time = rclcpp::Clock().now(); MarkerArray msg; @@ -206,11 +214,13 @@ MarkerArray getVehicleCircleLinesMarkerArray( auto base_pose = tier4_autoware_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); base_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); - + const double base_to_right = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const auto ub = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, vehicle_width / 2.0, 0.0).position; + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; const auto lb = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -vehicle_width / 2.0, 0.0).position; + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; marker.points.push_back(ub); marker.points.push_back(lb); @@ -368,13 +378,12 @@ MarkerArray getDebugMarker( MarkerArray marker_array; // bounds line - appendMarkerArray( - getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array); + appendMarkerArray(getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info), &marker_array); // bounds width appendMarkerArray( getBoundsWidthMarkerArray( - debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), + debug_data.ref_points, vehicle_info, debug_data.mpt_visualize_sampling_num), &marker_array); // current vehicle circles @@ -404,9 +413,8 @@ MarkerArray getDebugMarker( // vehicle circle line appendMarkerArray( getVehicleCircleLinesMarkerArray( - debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, - vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, - "vehicle_circle_lines"), + debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, vehicle_info, + debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), &marker_array); // footprint by drivable area diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 359485fadcfff..765136b3cf6f6 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -36,9 +36,11 @@ std::tuple, std::vector> calcVehicleCirclesByUniform const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, const double radius_ratio) { + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; const double radius = std::hypot( vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, - vehicle_info.vehicle_width_m / 2.0) * + vehicle_info.vehicle_width_m / 2.0 + lateral_offset) * radius_ratio; const std::vector radiuses(circle_num, radius); @@ -59,16 +61,18 @@ std::tuple, std::vector> calcVehicleCirclesByBicycle if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); } - + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; // 1st circle (rear wheel) - const double rear_radius = vehicle_info.vehicle_width_m / 2.0 * rear_radius_ratio; + const double rear_radius = + vehicle_info.vehicle_width_m / 2.0 + lateral_offset * rear_radius_ratio; const double rear_lon_offset = 0.0; // 2nd circle (front wheel) const double front_radius = std::hypot( vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, - vehicle_info.vehicle_width_m / 2.0) * + vehicle_info.vehicle_width_m / 2.0 + lateral_offset) * front_radius_ratio; const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast(circle_num); @@ -84,8 +88,9 @@ std::tuple, std::vector> calcVehicleCirclesByFitting if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); } - - const double radius = vehicle_info.vehicle_width_m / 2.0; + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; + const double radius = vehicle_info.vehicle_width_m / 2.0 + lateral_offset; std::vector radiuses(circle_num, radius); const double unit_lon_length =