Skip to content

Commit

Permalink
fix(obstacle_avoidance_planner): fix lateral calculations (#4292)
Browse files Browse the repository at this point in the history 
* use right_overhang and left_overhang for lateral calculation

Signed-off-by: beyza <[email protected]>

* style(pre-commit): autofix

* update debug_marker.cpp

* style(pre-commit): autofix

---------

Signed-off-by: beyza <[email protected]>
Co-authored-by: beyza <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@beyzanurkaya @pre-commit-ci
3 people authored Oct 17, 2023
1 parent c65bccd commit b3bdedc
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 30 deletions.
56 changes: 32 additions & 24 deletions planning/obstacle_avoidance_planner/src/debug_marker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());

Expand All @@ -71,8 +72,8 @@ MarkerArray getFootprintsMarkerArray(
}

MarkerArray getBoundsWidthMarkerArray(
const std::vector<ReferencePoint> & ref_points, const double vehicle_width,
const size_t sampling_num)
const std::vector<ReferencePoint> & ref_points,
const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num)
{
const auto current_time = rclcpp::Clock().now();
MarkerArray marker_array;
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -140,7 +145,8 @@ MarkerArray getBoundsWidthMarkerArray(
}

MarkerArray getBoundsLineMarkerArray(
const std::vector<ReferencePoint> & ref_points, const double vehicle_width)
const std::vector<ReferencePoint> & ref_points,
const vehicle_info_util::VehicleInfo & vehicle_info)
{
MarkerArray marker_array;

Expand All @@ -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);
}
Expand All @@ -175,8 +182,9 @@ MarkerArray getBoundsLineMarkerArray(

MarkerArray getVehicleCircleLinesMarkerArray(
const std::vector<ReferencePoint> & ref_points,
const std::vector<double> & vehicle_circle_longitudinal_offsets, const double vehicle_width,
const size_t sampling_num, const std::string & ns)
const std::vector<double> & 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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
17 changes: 11 additions & 6 deletions planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,9 +36,11 @@ std::tuple<std::vector<double>, std::vector<double>> 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<double>(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<double> radiuses(circle_num, radius);

Expand All @@ -59,16 +61,18 @@ std::tuple<std::vector<double>, std::vector<double>> 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<double>(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<double>(circle_num);
Expand All @@ -84,8 +88,9 @@ std::tuple<std::vector<double>, std::vector<double>> 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<double> radiuses(circle_num, radius);

const double unit_lon_length =
Expand Down

0 comments on commit b3bdedc

Please sign in to comment.