Skip to content

Commit

Permalink
consider right/left overhang for lateral calculations
Browse files Browse the repository at this point in the history 
Signed-off-by: beyza <bnk@leodrive.ai>
  • Loading branch information
@beyzanurkaya
beyzanurkaya committed Jul 24, 2023
1 parent 04ea73c commit 9e62b0e
Show file tree
Hide file tree
Showing 2 changed files with 66 additions and 22 deletions.
62 changes: 43 additions & 19 deletions planning/obstacle_avoidance_planner/src/debug_marker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,21 +48,23 @@ 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_right, 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_left, 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(
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(marker.points.front());

Expand All @@ -72,12 +74,16 @@ 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();
MarkerArray marker_array;

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;

if (ref_points.empty()) return marker_array;

// create lower bound marker
Expand Down Expand Up @@ -107,7 +113,7 @@ MarkerArray getBoundsWidthMarkerArray(

const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx);
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 @@ -127,7 +133,7 @@ MarkerArray getBoundsWidthMarkerArray(

const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx);
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 @@ -141,9 +147,11 @@ 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;
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;

if (ref_points.empty()) return marker_array;

Expand All @@ -160,11 +168,11 @@ 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 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 @@ -176,7 +184,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 All @@ -196,6 +205,9 @@ MarkerArray getVehicleCircleLinesMarkerArray(
const double lat_dev = ref_point.optimized_kinematic_state.lat;
const double yaw_dev = ref_point.optimized_kinematic_state.yaw;

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;

// apply lateral and yaw deviation
auto pose_with_deviation =
tier4_autoware_utils::calcOffsetPose(ref_point.pose, 0.0, lat_dev, 0.0);
Expand All @@ -209,9 +221,9 @@ MarkerArray getVehicleCircleLinesMarkerArray(
tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha);

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 All @@ -231,6 +243,8 @@ MarkerArray getCurrentVehicleCirclesMarkerArray(
MarkerArray msg;

size_t id = 0;
constexpr size_t circle_dividing_num = 16;

for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) {
const double offset = vehicle_circle_longitudinal_offsets.at(v_idx);

Expand All @@ -240,8 +254,17 @@ MarkerArray getCurrentVehicleCirclesMarkerArray(
marker.lifetime = rclcpp::Duration::from_seconds(1.5);
marker.pose = tier4_autoware_utils::calcOffsetPose(ego_pose, offset, 0.0, 0.0);

constexpr size_t circle_dividing_num = 16;
for (size_t e_idx = 0; e_idx < circle_dividing_num + 1; ++e_idx) {
for (size_t e_idx = 0; e_idx < circle_dividing_num/2 +1; ++e_idx) {
geometry_msgs::msg::Point edge_pos;

const double edge_angle =
static_cast<double>(e_idx) / static_cast<double>(circle_dividing_num) * 2.0 * M_PI;
edge_pos.x = -vehicle_circle_radiuses.at(v_idx+1) * std::cos(edge_angle);
edge_pos.y = -vehicle_circle_radiuses.at(v_idx+1) * std::sin(edge_angle);

marker.points.push_back(edge_pos);
}
for (size_t e_idx = 0; e_idx < circle_dividing_num/2 +1; ++e_idx) {
geometry_msgs::msg::Point edge_pos;

const double edge_angle =
Expand All @@ -255,6 +278,7 @@ MarkerArray getCurrentVehicleCirclesMarkerArray(
msg.markers.push_back(marker);
++id;
}

return msg;
}

Expand Down Expand Up @@ -375,19 +399,19 @@ MarkerArray getDebugMarker(

// bounds lines
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);

// 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"),
vehicle_info, debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"),
&marker_array);

// current vehicle circles
Expand Down
26 changes: 23 additions & 3 deletions planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,21 @@ 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 radius = std::hypot(
const double left_radius = std::hypot(
vehicle_info.vehicle_length_m / static_cast<double>(circle_num) / 2.0,
vehicle_info.vehicle_width_m / 2.0) *
(vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m) *
radius_ratio;
const std::vector<double> radiuses(circle_num, radius);

const double right_radius = std::hypot(
vehicle_info.vehicle_length_m / static_cast<double>(circle_num) / 2.0,
(vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m) *
radius_ratio;

std::vector<double> radiuses;
for (size_t i = 0; i < circle_num ; ++i) {
radiuses.push_back(left_radius);
radiuses.push_back(right_radius);
}

const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast<double>(circle_num);
std::vector<double> longitudinal_offsets;
Expand All @@ -59,6 +69,16 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesByBicycle
throw std::invalid_argument("circle_num is less than 2.");
}

const double left_radius = std::hypot(
vehicle_info.vehicle_length_m / static_cast<double>(circle_num) / 2.0,
(vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m) *
front_radius_ratio;

const double right_radius = std::hypot(
vehicle_info.vehicle_length_m / static_cast<double>(circle_num) / 2.0,
(vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m) *
front_radius_ratio;

// 1st circle (rear wheel)
const double rear_radius = vehicle_info.vehicle_width_m / 2.0 * rear_radius_ratio;
const double rear_lon_offset = 0.0;
Expand Down

0 comments on commit 9e62b0e

Please sign in to comment.