fix(obstacle_avoidance_planner): fix lateral calculations (#2878)

* fix lateral calculations

Signed-off-by: beyza <bnk@leodrive.ai>

* style(pre-commit): autofix

* use right_overhang and left_overhang four bound calculation

Signed-off-by: beyza <bnk@leodrive.ai>

* style(pre-commit): autofix

* add lateral_offset

Signed-off-by: beyza <bnk@leodrive.ai>

* style(pre-commit): autofix

---------

Signed-off-by: beyza <bnk@leodrive.ai>
Co-authored-by: beyza <bnk@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -43,13 +43,16 @@ geometry_msgs::msg::Pose convertRefPointsToPose(const ReferencePoint & ref_point
 }
 
 std::tuple<Eigen::VectorXd, Eigen::VectorXd> extractBounds(
-  const std::vector<ReferencePoint> & ref_points, const size_t l_idx, const double offset)
+  const std::vector<ReferencePoint> & ref_points, const size_t l_idx,
+  const VehicleParam & vehicle_param_)
 {
+  const double base_to_right = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.right_overhang;
+  const double base_to_left = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.left_overhang;
   Eigen::VectorXd ub_vec(ref_points.size());
   Eigen::VectorXd lb_vec(ref_points.size());
   for (size_t i = 0; i < ref_points.size(); ++i) {
-    ub_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).upper_bound + offset;
-    lb_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).lower_bound - offset;
+    ub_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).upper_bound + base_to_left;
+    lb_vec(i) = ref_points.at(i).vehicle_bounds.at(l_idx).lower_bound - base_to_right;
   }
   return {ub_vec, lb_vec};
 }
@@ -1063,9 +1066,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::getConstraintMatrix(
     const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.Wex + C_vec;
 
     // calculate bounds
-    const double bounds_offset =
-      vehicle_param_.width / 2.0 - mpt_param_.vehicle_circle_radiuses.at(l_idx);
-    const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx, bounds_offset);
+    const auto & [part_ub, part_lb] = extractBounds(ref_points, l_idx, vehicle_param_);
 
     // soft constraints
     if (mpt_param_.soft_constraint) {
@@ -1411,7 +1412,8 @@ void MPTOptimizer::calcBounds(
                                          mpt_param_.soft_clearance_from_road +
                                          mpt_param_.extra_desired_clearance_from_road;
   */
-  const double min_soft_road_clearance = vehicle_param_.width / 2.0;
+  const double base_to_right = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.right_overhang;
+  const double base_to_left = (vehicle_param_.wheel_tread / 2.0) + vehicle_param_.left_overhang;
 
   // search bounds candidate for each ref points
   debug_data.bounds_candidates.clear();
@@ -1427,11 +1429,9 @@ void MPTOptimizer::calcBounds(
     const Eigen::Vector2d left_point = current_ref_point + left_vertical_vec.normalized() * 5.0;
     const Eigen::Vector2d right_point = current_ref_point + right_vertical_vec.normalized() * 5.0;
     const double lat_dist_to_left_bound = std::min(
-      calcLateralDistToBound(current_ref_point, left_point, left_bound) - min_soft_road_clearance,
-      5.0);
+      calcLateralDistToBound(current_ref_point, left_point, left_bound) - base_to_left, 5.0);
     const double lat_dist_to_right_bound = std::max(
-      calcLateralDistToBound(current_ref_point, right_point, right_bound, true) +
-        min_soft_road_clearance,
+      calcLateralDistToBound(current_ref_point, right_point, right_bound, true) + base_to_right,
       -5.0);
 
     ref_points.at(i).bounds = Bounds{
@@ -1444,11 +1444,11 @@ void MPTOptimizer::calcBounds(
   const double lat_dist_to_left_bound =
     -motion_utils::calcLateralOffset(
       left_bound, convertRefPointsToPose(ref_points.back()).position) -
-    min_soft_road_clearance;
+    base_to_left;
   const double lat_dist_to_right_bound =
     -motion_utils::calcLateralOffset(
       right_bound, convertRefPointsToPose(ref_points.back()).position) +
-    min_soft_road_clearance;
+    base_to_right;
   ref_points.back().bounds = Bounds{
     lat_dist_to_right_bound, lat_dist_to_left_bound, CollisionType::NO_COLLISION,
     CollisionType::NO_COLLISION};

planning/obstacle_avoidance_planner/src/node.cpp

@@ -43,22 +43,29 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesInfo(
 {
   std::vector<double> longitudinal_offsets;
   std::vector<double> radiuses;
+  const double lateral_offset =
+    abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0;
 
   {  // 1st circle (rear)
+    const double radius = (vehicle_param.width / 2.0 + lateral_offset) * rear_radius_ratio;
+
     longitudinal_offsets.push_back(-vehicle_param.rear_overhang);
-    radiuses.push_back(vehicle_param.width / 2.0 * rear_radius_ratio);
+    radiuses.push_back(radius);
   }
 
   {  // 2nd circle (front)
-    const double radius = std::hypot(
-      vehicle_param.length / static_cast<double>(circle_num) / 2.0, vehicle_param.width / 2.0);
+    const double radius =
+      (std::hypot(
+         vehicle_param.length / static_cast<double>(circle_num) / 2.0,
+         (vehicle_param.width / 2.0 + lateral_offset)) *
+       front_radius_ratio);
 
     const double unit_lon_length = vehicle_param.length / static_cast<double>(circle_num);
     const double longitudinal_offset =
       unit_lon_length / 2.0 + unit_lon_length * (circle_num - 1) - vehicle_param.rear_overhang;
 
     longitudinal_offsets.push_back(longitudinal_offset);
-    radiuses.push_back(radius * front_radius_ratio);
+    radiuses.push_back(radius);
   }
 
   return {radiuses, longitudinal_offsets};
@@ -69,11 +76,13 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesInfo(
 {
   std::vector<double> longitudinal_offsets;
   std::vector<double> radiuses;
+  const double lateral_offset =
+    abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0;
 
-  const double radius =
-    std::hypot(
-      vehicle_param.length / static_cast<double>(circle_num) / 2.0, vehicle_param.width / 2.0) *
-    radius_ratio;
+  const double radius = std::hypot(
+                          vehicle_param.length / static_cast<double>(circle_num) / 2.0,
+                          (vehicle_param.width / 2.0 + lateral_offset)) *
+                        radius_ratio;
   const double unit_lon_length = vehicle_param.length / static_cast<double>(circle_num);
 
   for (size_t i = 0; i < circle_num; ++i) {
@@ -92,8 +101,10 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesInfo(
 
   std::vector<double> longitudinal_offsets;
   std::vector<double> radiuses;
+  const double lateral_offset =
+    abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0;
 
-  const double radius = vehicle_param.width / 2.0;
+  const double radius = (vehicle_param.width / 2.0 + lateral_offset);
   const double unit_lon_length = vehicle_param.length / static_cast<double>(circle_num - 1);
 
   for (size_t i = 0; i < circle_num; ++i) {
@@ -110,22 +121,27 @@ std::tuple<std::vector<double>, std::vector<double>> calcVehicleCirclesInfoByBic
 {
   std::vector<double> longitudinal_offsets;
   std::vector<double> radiuses;
+  const double lateral_offset =
+    abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0;
 
   {  // 1st circle (rear wheel)
+    const double radius = (vehicle_param.width / 2.0 + lateral_offset) * rear_radius_ratio;
     longitudinal_offsets.push_back(0.0);
-    radiuses.push_back(vehicle_param.width / 2.0 * rear_radius_ratio);
+    radiuses.push_back(radius);
   }
 
   {  // 2nd circle (front wheel)
     const double radius = std::hypot(
-      vehicle_param.length / static_cast<double>(circle_num) / 2.0, vehicle_param.width / 2.0);
+                            vehicle_param.length / static_cast<double>(circle_num) / 2.0,
+                            (vehicle_param.width / 2.0 + lateral_offset)) *
+                          front_radius_ratio;
 
     const double unit_lon_length = vehicle_param.length / static_cast<double>(circle_num);
     const double longitudinal_offset =
       unit_lon_length / 2.0 + unit_lon_length * (circle_num - 1) - vehicle_param.rear_overhang;
 
     longitudinal_offsets.push_back(longitudinal_offset);
-    radiuses.push_back(radius * front_radius_ratio);
+    radiuses.push_back(radius);
   }
 
   return {radiuses, longitudinal_offsets};

planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp

@@ -419,11 +419,11 @@ visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray(
 
     marker.text = std::to_string(i);
 
-    const double half_width = vehicle_param.width / 2.0;
+    const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang;
     const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang;
 
     const auto top_right_pos =
-      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.id = i;
     marker.pose.position = top_right_pos;
@@ -477,13 +477,15 @@ visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray(
 }
 
 visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
-  const std::vector<ReferencePoint> & ref_points, const double r, const double g, const double b,
-  const double vehicle_width, const size_t sampling_num)
+  const std::vector<ReferencePoint> & ref_points, const VehicleParam & vehicle_param,
+  const double r, const double g, const double b, const size_t sampling_num)
 {
   const auto current_time = rclcpp::Clock().now();
   visualization_msgs::msg::MarkerArray msg;
 
   if (ref_points.empty()) return msg;
+  const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang;
+  const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang;
 
   for (size_t bound_idx = 0; bound_idx < ref_points.at(0).vehicle_bounds.size(); ++bound_idx) {
     const std::string ns = "base_bounds_" + std::to_string(bound_idx);
@@ -500,8 +502,7 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
         }
 
         const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx);
-        const double lb_y =
-          ref_points.at(i).vehicle_bounds[bound_idx].lower_bound - vehicle_width / 2.0;
+        const double lb_y = ref_points.at(i).vehicle_bounds[bound_idx].lower_bound - base_to_right;
         const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position;
 
         marker.points.push_back(pose.position);
@@ -522,8 +523,7 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
         }
 
         const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx);
-        const double ub_y =
-          ref_points.at(i).vehicle_bounds[bound_idx].upper_bound + vehicle_width / 2.0;
+        const double ub_y = ref_points.at(i).vehicle_bounds[bound_idx].upper_bound + base_to_left;
         const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position;
 
         marker.points.push_back(pose.position);
@@ -538,12 +538,13 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
 
 visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray(
   const std::vector<std::vector<geometry_msgs::msg::Pose>> & vehicle_circles_pose,
-  const double vehicle_width, const size_t sampling_num, const std::string & ns, const double r,
-  const double g, const double b)
+  const VehicleParam & vehicle_param, const size_t sampling_num, const std::string & ns,
+  const double r, const double g, const double b)
 {
   const auto current_time = rclcpp::Clock().now();
   visualization_msgs::msg::MarkerArray msg;
-
+  const double base_to_right = (vehicle_param.wheel_tread / 2.0) + vehicle_param.right_overhang;
+  const double base_to_left = (vehicle_param.wheel_tread / 2.0) + vehicle_param.left_overhang;
   for (size_t i = 0; i < vehicle_circles_pose.size(); ++i) {
     if (i % sampling_num != 0) {
       continue;
@@ -556,10 +557,8 @@ visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray(
 
     for (size_t j = 0; j < vehicle_circles_pose.at(i).size(); ++j) {
       const geometry_msgs::msg::Pose & pose = vehicle_circles_pose.at(i).at(j);
-      const auto ub =
-        tier4_autoware_utils::calcOffsetPose(pose, 0.0, vehicle_width / 2.0, 0.0).position;
-      const auto lb =
-        tier4_autoware_utils::calcOffsetPose(pose, 0.0, -vehicle_width / 2.0, 0.0).position;
+      const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, base_to_left, 0.0).position;
+      const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, -base_to_right, 0.0).position;
 
       marker.points.push_back(ub);
       marker.points.push_back(lb);
@@ -597,7 +596,7 @@ visualization_msgs::msg::MarkerArray getLateralErrorsLineMarkerArray(
 }
 
 visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray(
-  const geometry_msgs::msg::Pose & current_ego_pose,
+  const geometry_msgs::msg::Pose & current_ego_pose, const VehicleParam & vehicle_param,
   const std::vector<double> & vehicle_circle_longitudinal_offsets,
   const std::vector<double> & vehicle_circle_radiuses, const std::string & ns, const double r,
   const double g, const double b)
@@ -607,6 +606,8 @@ visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray(
   size_t id = 0;
   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);
+    const double lateral_offset =
+      abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0;
 
     auto marker = createDefaultMarker(
       "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP,
@@ -620,8 +621,8 @@ visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray(
 
       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) * std::cos(edge_angle);
-      edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle);
+      edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle) + lateral_offset;
+      edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle) - lateral_offset;
 
       marker.points.push_back(edge_pos);
     }
@@ -634,6 +635,7 @@ visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray(
 
 visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray(
   const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & mpt_traj_points,
+  [[maybe_unused]] const VehicleParam & vehicle_param,
   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)
@@ -649,6 +651,8 @@ visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray(
 
     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);
+      const double lateral_offset =
+        abs(vehicle_param.right_overhang - vehicle_param.left_overhang) / 2.0;
 
       auto marker = createDefaultMarker(
         "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP,
@@ -662,8 +666,8 @@ visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray(
 
         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) * std::cos(edge_angle);
-        edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle);
+        edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle) + lateral_offset;
+        edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle) - lateral_offset;
 
         marker.points.push_back(edge_pos);
       }
@@ -785,8 +789,7 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   // bounds
   appendMarkerArray(
     getBoundsLineMarkerArray(
-      debug_data.ref_points, 0.99, 0.99, 0.2, vehicle_param.width,
-      debug_data.mpt_visualize_sampling_num),
+      debug_data.ref_points, vehicle_param, 0.99, 0.99, 0.2, debug_data.mpt_visualize_sampling_num),
     &vis_marker_array);
 
   // bounds candidates
@@ -799,7 +802,7 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   // vehicle circle line
   appendMarkerArray(
     getVehicleCircleLineMarkerArray(
-      debug_data.vehicle_circles_pose, vehicle_param.width, debug_data.mpt_visualize_sampling_num,
+      debug_data.vehicle_circles_pose, vehicle_param, debug_data.mpt_visualize_sampling_num,
       "vehicle_circle_lines", 0.99, 0.99, 0.2),
     &vis_marker_array);
 
@@ -813,14 +816,14 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   // current vehicle circles
   appendMarkerArray(
     getCurrentVehicleCirclesMarkerArray(
-      debug_data.current_ego_pose, debug_data.vehicle_circle_longitudinal_offsets,
+      debug_data.current_ego_pose, vehicle_param, debug_data.vehicle_circle_longitudinal_offsets,
       debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3),
     &vis_marker_array);
 
   // vehicle circles
   appendMarkerArray(
     getVehicleCirclesMarkerArray(
-      debug_data.mpt_traj, debug_data.vehicle_circle_longitudinal_offsets,
+      debug_data.mpt_traj, vehicle_param, debug_data.vehicle_circle_longitudinal_offsets,
       debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles",
       1.0, 0.3, 0.3),
     &vis_marker_array);