Skip to content

Commit

Permalink
use right_overhang and left_overhang for lateral calculation
Browse files Browse the repository at this point in the history 
Signed-off-by: beyza <bnk@leodrive.ai>
  • Loading branch information
@beyzanurkaya
beyza authored and beyzanurkaya committed Oct 17, 2023
1 parent c65bccd commit 2ffd93c
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 28 deletions.
50 changes: 28 additions & 22 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,7 +72,7 @@ MarkerArray getFootprintsMarkerArray(
}

MarkerArray getBoundsWidthMarkerArray(
const std::vector<ReferencePoint> & ref_points, const double vehicle_width,
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();
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -140,7 +143,7 @@ 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 +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);
}
Expand All @@ -175,7 +179,8 @@ MarkerArray getBoundsLineMarkerArray(

MarkerArray getVehicleCircleLinesMarkerArray(
const std::vector<ReferencePoint> & ref_points,
const std::vector<double> & vehicle_circle_longitudinal_offsets, const double vehicle_width,
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();
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -260,8 +266,8 @@ MarkerArray getCurrentVehicleCirclesMarkerArray(
MarkerArray getVehicleCirclesMarkerArray(
const std::vector<TrajectoryPoint> & mpt_traj_points,
const std::vector<double> & vehicle_circle_longitudinal_offsets,
const std::vector<double> & vehicle_circle_radiuses, const size_t sampling_num,
const std::string & ns, const double r, const double g, const double b)
const std::vector<double> & 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;

Expand Down Expand Up @@ -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
Expand All @@ -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);

Expand Down
16 changes: 10 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,17 @@ 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 +87,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 2ffd93c

Please sign in to comment.