search

autowarefoundation / autoware.universe

https://autowarefoundation.github.io/autoware.universe/
Apache License 2.0
964 stars 630 forks source link

(Regression) planner stops for sudden "outside driveable area" always at the same spot #3368

Closed VRichardJP closed 2 months ago

VRichardJP commented 1 year ago

Checklist

Description

I have observed both with our vehicle and the planning simulator autoware suddenly brake strongly always at the same spot in our test course. This behavior was first observed a few weeks ago.

Here is a simple demo using the planning simulator: outside_drivable_area-2023-04-12_13.18.47.webm

You can see Autoware generates a "outside driveable area" while driving on a straight road.

I have tried to remove unecessary lanelets, but the behavior is still the same (with a few added debug info): outside_drivable_area-2023-04-12_13.40.43.webm

You can see some very strange "side road" seem to be generated when this happen: image image

Expected behavior

No brake

Actual behavior

strong brake

Steps to reproduce

TODO

Versions

No response

Possible causes

No response

Additional context

No response

VRichardJP commented 1 year ago

The bug disappeared after changing the behavior_path_planner forward_path_length from 300.0 to 100.0 One difference I notice with the reduced forward length is that the path no longer cross itself: image

The bug may be related to that.

takayuki5168 commented 1 year ago

The bug may be related to that.

I agree. I cannot find the bug though.

VRichardJP commented 1 year ago

Maybe a cause or a side effect: when the "outside drivable area" wall appear, the path right bound is almost empty:

image

I can confirm this from the /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound topic:

markers:
- header:
    stamp:
      sec: 129
      nanosec: 430555341
    frame_id: map
  ns: left_bound
  id: 0
  type: 4
  action: 0
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  scale:
    x: 0.2
    y: 0.2
    z: 0.2
  color:
    r: 0.0
    g: 0.578125
    b: 0.80078125
    a: 0.9990000128746033
  lifetime:
    sec: 0
    nanosec: 500000000
  frame_locked: true
  points:
  - x: 75626.06559886156
    y: 31114.457643634596
    z: 3.8357227420438726
  - x: 75625.6824
    y: 31109.1809
    z: 3.7197
  - x: 75624.6156
    y: 31096.1511
    z: 3.8506
  - x: 75622.6666
    y: 31079.9395
    z: 3.8479
  - x: 75622.0653
    y: 31072.4909
    z: 3.8599
  - x: 75622.1652
    y: 31071.2246
    z: 3.8662
  - x: 75622.5824
    y: 31069.2654
    z: 3.8666
  - x: 75623.8721
    y: 31066.9943
    z: 3.7602
  - x: 75624.915
    y: 31066.0513
    z: 3.891
  - x: 75626.6408
    y: 31064.8195
    z: 3.9031
  - x: 75628.5102
    y: 31063.772
    z: 3.9238
  - x: 75630.7917
    y: 31063.0817
    z: 3.8433
  - x: 75632.8456
    y: 31062.7673
    z: 3.9705
  - x: 75660.2605
    y: 31060.8364
    z: 4.1203
  - x: 75672.0545
    y: 31059.658
    z: 4.0945
  - x: 75710.7133
    y: 31056.781
    z: 4.1484
  - x: 75711.9953
    y: 31056.9573
    z: 4.3948
  - x: 75713.4929
    y: 31058.0443
    z: 4.3508
  - x: 75714.3927
    y: 31059.3947
    z: 4.5404
  - x: 75714.8853
    y: 31060.7935
    z: 4.5301
  - x: 75715.0612
    y: 31062.2868
    z: 4.5311
  - x: 75716.7913
    y: 31088.9043
    z: 4.6189
  - x: 75717.2434
    y: 31096.0625
    z: 4.6783
  - x: 75717.0032
    y: 31097.7146
    z: 4.6783
  - x: 75715.9832
    y: 31099.1343
    z: 4.6731
  - x: 75714.4659
    y: 31100.0275
    z: 4.5577
  - x: 75708.955
    y: 31100.7919
    z: 4.428
  - x: 75675.8906
    y: 31104.8998
    z: 4.1107
  - x: 75665.5124
    y: 31106.7297
    z: 4.0602
  - x: 75648.3492
    y: 31108.2945
    z: 4.045
  - x: 75627.391
    y: 31110.1221
    z: 3.9397
  colors: []
  texture_resource: ''
  texture:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    format: ''
    data: []
  uv_coordinates: []
  text: ''
  mesh_resource: ''
  mesh_file:
    filename: ''
    data: []
  mesh_use_embedded_materials: false
- header:
    stamp:
      sec: 129
      nanosec: 430555341
    frame_id: map
  ns: right_bound
  id: 0
  type: 4
  action: 0
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  scale:
    x: 0.2
    y: 0.2
    z: 0.2
  color:
    r: 0.0
    g: 0.578125
    b: 0.80078125
    a: 0.9990000128746033
  lifetime:
    sec: 0
    nanosec: 500000000
  frame_locked: true
  points:
  - x: 75648.644
    y: 31111.8014
    z: 5.5367
  - x: 75627.7081
    y: 31113.8724
    z: 4.1098
  colors: []
  texture_resource: ''
  texture:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    format: ''
    data: []
  uv_coordinates: []
  text: ''
  mesh_resource: ''
  mesh_file:
    filename: ''
    data: []
  mesh_use_embedded_materials: false
---

You can see the right bound has only 2 points.

VRichardJP commented 1 year ago

Since the right_bound.point.size() == 2 is fairly easy to detect, I could run the program with gdb and stop right a the moment the strange right bound is generated.

I am still investigating, but while stepping inside generateDrivableArea, I have found these strange values:

(gdb) p front_pose
$26 = {position = {x = 75624.150317563472, y = 31114.308224363751, z = 3.8433717986237106}, orientation = {x = -0.0067077158534262181, y = -0.0061622348519188574, z = -0.73638514544570388, w = 0.67650125718772247}}
(gdb) front_left_start_idx
Undefined command: "front_left_start_idx".  Try "help".
(gdb) p front_left_start_idx
$27 = 0
(gdb) p front_right_start_idx
$28 = 34
(gdb) p left_start_point
$29 = {x = 75626.080972052165, y = 31114.669336292969, z = 3.8403773480652288}
(gdb) p right_start_point
$30 = {x = 75648.644, y = 31111.8014, z = 5.5366999999999997}
(gdb) p left_start_idx
$31 = 0
(gdb) p right_start_idx
$32 = 33
(gdb) p goal_pose
$33 = {position = {x = 75628.205621774119, y = 31111.936214821744, z = 4.0486484954393092}, orientation = {x = 0.018106582803569488, y = -0.00084042451797931123, z = 0.99876042799084508, w = 0.046357877705424495}}
(gdb) p goal_left_start_idx
$34 = 29
(gdb) p goal_right_start_idx
$35 = 34
(gdb) p left_goal_point
$36 = {x = 75627.391000000003, y = 31110.122100000001, z = 3.9397000000000002}
(gdb) p right_goal_point
$37 = {x = 75627.708100000003, y = 31113.8724, z = 4.1097999999999999}
(gdb) p left_goal_idx
$38 = 29
(gdb) p right_goal_idx
$39 = 34

Put on the map, the points look like this: image

The right_start_point looks oddly placed. I checked the code beyond, and points are added from right_start_idx to right_goal_idx. Here only the last 2 points are copied.

The problem boils down to these lines:

https://github.com/autowarefoundation/autoware.universe/blob/d73c3ae41b53b76589d78a7a019cd065d031fd9d/planning/behavior_path_planner/src/utils/utils.cpp#L900-L903

For the right bound it returns 34, which is very far on the path. We can clearly see that points on the previous comment screenshot

VRichardJP commented 1 year ago

Down the rabbit hole:

generateDrivableArea calls findNearestSegmentIndexFromLateralDistance, which calls findNearestSegmentIndex, which calls findNearestIndex with the right_bound and the front_pose.

So we get here:

https://github.com/autowarefoundation/autoware.universe/blob/d73c3ae41b53b76589d78a7a019cd065d031fd9d/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp#L360-L367

And what happens is that the right bound last point is closer from the target pose than the first few points. So nearest_idx is set to 35 = points.size() - 1, and then the function returns 34, which causes the issue.

Visually, it is easy to see the problem with the current logic:

image

VRichardJP commented 1 year ago

One solution could be to find the right_bound segment that is the closest from the front_pose (i.e. measure the distance point<->segment), and take the first point of that segment, Since front_pose is constructed on the centerline and the right_bound does not overlap on the path, this method would always return the desired result.

Edit: this is only true when we have a single lane. When we have multiple lanes the closest segment by projection may not be the one we want.

Maybe a better approach is not to try to compute anything here? The left and right bounds seems to have been calculated already, so why do we need to calculate new ones? If we assume the left and right are "almost" correct and just want to refine them, we could maybe do something like this: explore left/right points in order and keep going while the next point is closer than the previous. In other words getting the "closest point at the beginning". For the goal point it would be the same story but in reverse. In pretty sure this logic is still brittle and would break on some weirdly shaped lanelets (2d geometry is hard...)

VRichardJP commented 1 year ago

Example with the proposed fix (for single lane only): outside_drivable_area_fix-2023-04-14_20.04.09.webm

diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
index 775d2b0acb..18f16c609b 100644
--- a/planning/behavior_path_planner/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -15,16 +15,20 @@
 #include "behavior_path_planner/utils/utils.hpp"

 #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp"
+#include "behavior_path_planner/utils/safety_check.hpp"
 #include "motion_utils/trajectory/path_with_lane_id.hpp"

 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <motion_utils/trajectory/interpolation.hpp>

 #include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
 #include "autoware_auto_perception_msgs/msg/predicted_path.hpp"

 #include <boost/assign/list_of.hpp>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/primitives/Point.h>

 #include <algorithm>
 #include <limits>
@@ -895,36 +899,54 @@ void generateDrivableArea(
   }

   // Get Closest segment for the start point
+  auto getClosestSegmentIndex = [](const std::vector<geometry_msgs::msg::Point> & points, const auto & target) {
+    using behavior_path_planner::util::safety_check::Point2d;
+    using behavior_path_planner::util::safety_check::pointToSegment;
+    const Point2d p{target.x, target.y};
+    size_t best_idx = 0;
+    double best_dist = std::numeric_limits<double>::max();
+    for (size_t i = 0; i < points.size()-1; ++ i) {
+      Point2d s0 {points[i].x, points[i].y};
+      Point2d s1 {points[i+1].x, points[i+1].y};
+      auto projected_point = pointToSegment(p, s0, s1);
+      if (projected_point.distance < best_dist) {
+        best_dist = projected_point.distance;
+        best_idx = i;
+      }
+    }
+    return best_idx;
+  };
+
   constexpr double front_length = 0.5;
   const auto front_pose = path.points.empty() ? current_pose : path.points.front().point.pose;
   const size_t front_left_start_idx =
-    findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position);
+    getClosestSegmentIndex(left_bound, front_pose.position);
   const size_t front_right_start_idx =
-    findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position);
+    getClosestSegmentIndex(right_bound, front_pose.position);
   const auto left_start_point =
     calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length);
   const auto right_start_point =
     calcLongitudinalOffsetStartPoint(right_bound, front_pose, front_right_start_idx, -front_length);
   const size_t left_start_idx =
-    findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point);
+    getClosestSegmentIndex(left_bound, left_start_point);
   const size_t right_start_idx =
-    findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point);
+    getClosestSegmentIndex(right_bound, right_start_point);

   // Get Closest segment for the goal point
   const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose;
   const size_t goal_left_start_idx =
-    findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position);
+    getClosestSegmentIndex(left_bound, goal_pose.position);
   const size_t goal_right_start_idx =
-    findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose.position);
+    getClosestSegmentIndex(right_bound, goal_pose.position);
   const auto left_goal_point =
     calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length);
   const auto right_goal_point =
     calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length);
   const size_t left_goal_idx = std::max(
-    goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point));
+    goal_left_start_idx, getClosestSegmentIndex(left_bound, left_goal_point));
   const size_t right_goal_idx = std::max(
     goal_right_start_idx,
-    findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point));
+    getClosestSegmentIndex(right_bound, right_goal_point));

   // Store Data
   path.left_bound.clear();

Or the same with ranges ;-)

auto getClosestSegmentIndex = [](const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Point & target) {
    using behavior_path_planner::util::safety_check::Point2d;
    using behavior_path_planner::util::safety_check::pointToSegment;
    const Point2d p{target.x, target.y};
    const auto distance_to_projected_points = points 
      | ranges::views::transform([](auto &&p) { return Point2d {p.x, p.y}; })
      | ranges::views::sliding(2)
      | ranges::views::transform([=](auto &&segment) { 
        auto projected_point = pointToSegment(p, segment[0], segment[1]);
        return projected_point.distance;
      });
    const auto min_it = ranges::min_element(distance_to_projected_points);
    const size_t best_idx = std::distance(distance_to_projected_points.begin(), min_it);
    return best_idx;
  };
VRichardJP commented 1 year ago

@satoshi-ota @purewater0901 Any idea how to fix this properly?

takayuki5168 commented 1 year ago

@VRichardJP Thank you for your analysis. Now I understand the cause.

just want to refine them

This is right.

we could maybe do something like this: explore left/right points in order and keep going while the next point is closer than the previous. In other words getting the "closest point at the beginning". For the goal point it would be the same story but in reverse.

I agree with this idea. The following function is very similar to what you said, so you can use it. https://github.com/autowarefoundation/autoware.universe/blob/d73c3ae41b53b76589d78a7a019cd065d031fd9d/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp#L1495-L1499

The following is what I though first, but I found it not so a good way to fix. So please ignore the idea.

I was thinking about the idea like

However, the point's orientation on bounds may be weird like this. We'd better not to use the orientation. image

stale[bot] commented 1 year ago

This pull request has been automatically marked as stale because it has not had recent activity.

VRichardJP commented 11 months ago

The outside drivable area issue seems to have disappeared on later version. However lane boundaries are still calculated incorrectly:

weird_lane_boundaries-2023-10-24_10.14.25.webm

stale[bot] commented 9 months ago

This pull request has been automatically marked as stale because it has not had recent activity.

VRichardJP commented 2 months ago

The problem does not exist anymore on release/v1.1-beta