diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index 8bf46c697104a..59fd976a0feb4 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,7 +72,7 @@ MarkerArray getFootprintsMarkerArray( } MarkerArray getBoundsWidthMarkerArray( - const std::vector & ref_points, const double vehicle_width, + const std::vector & ref_points, const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); @@ -105,8 +106,9 @@ 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 +127,9 @@ 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 +143,7 @@ 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 +161,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,7 +179,8 @@ MarkerArray getBoundsLineMarkerArray( MarkerArray getVehicleCircleLinesMarkerArray( const std::vector & ref_points, - const std::vector & vehicle_circle_longitudinal_offsets, const double vehicle_width, + 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(); @@ -206,11 +211,12 @@ 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); @@ -260,8 +266,8 @@ MarkerArray getCurrentVehicleCirclesMarkerArray( MarkerArray getVehicleCirclesMarkerArray( const std::vector & mpt_traj_points, const std::vector & vehicle_circle_longitudinal_offsets, - const std::vector & vehicle_circle_radiuses, const size_t sampling_num, - const std::string & ns, const double r, const double g, const double b) + const std::vector & vehicle_circle_radiuses, const vehicle_info_util::VehicleInfo & vehicle_info, + const size_t sampling_num, const std::string & ns, const double r, const double g, const double b) { MarkerArray msg; @@ -369,19 +375,19 @@ MarkerArray getDebugMarker( // bounds line appendMarkerArray( - getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array); + 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 appendMarkerArray( getCurrentVehicleCirclesMarkerArray( debug_data.ego_pose, debug_data.vehicle_circle_longitudinal_offsets, - debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), + debug_data.vehicle_circle_radiuses, vehicle_info, "current_vehicle_circles", 1.0, 0.3, 0.3), &marker_array); // NOTE: Default debug marker is limited for less calculation time @@ -405,7 +411,7 @@ MarkerArray getDebugMarker( appendMarkerArray( getVehicleCircleLinesMarkerArray( debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, - vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, + vehicle_info, debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), &marker_array); diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 359485fadcfff..9a81cf7201a88 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,17 @@ 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 +87,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 =