Planning Fails when driving to a lanelet that is "behind" the current lanelet (Doing a loop). Cannot plan new paths afterwards.

[danielsanchezaran]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

I am trying to make my vehicle go around a loop conformed by 4 lanelets in Psim. When I set a goal pose that happens to be on the lanelet that is behind the ego vehicle's current lanelet, planning fails and it is no longer possible to select another start and goal. The only way to fix the issue is to restart Autoware. I have confirmed that both halves of the road are drivable and doing "half loops" is possible without problems.

In the attached video, I make the vehicle do a loop by setting multiple goals (to prove all the areas are drive able). But, when I try to set a goal that is on the "previous" lanelet (See steps to reproduce for example), planning fails and it is not possible to continue moving the vehicle with no warnings. I can provide a ros2 bag if necessary.

cap-.2023-10-30-13-34-18.mp4

The only log messages I get are these (Using RelWithDebInfo compilation):

planning_failling.txt
I also get the message that some node died but there is no stack trace but I did notice these messages:

[scenario_selector-26] [WARN] [1698645501.368634568] [route_handler]: [Route Handler] getRouteUuid: Route has not been set yet
[ERROR] [component_container_mt-29]: process has died [pid 77740, exit code -9, cmd '/opt/ros/humble/lib/rclcpp_components/component_container_mt --ros-args -r __node:=behavior_planning_container -r __ns:=/planning/scenario_planning/lane_driving/behavior_planning -p use_sim_time:=False -p wheel_radius:=0.1878 -p wheel_width:=0.037 -p wheel_base:=0.4 -p wheel_tread:=0.405 -p front_overhang:=0.23 -p rear_overhang:=0.21 -p left_overhang:=0.06 -p right_overhang:=0.06 -p vehicle_height:=0.52 -p max_steer_angle:=0.7'].

Expected behavior

The car should be able to do the full path and path planning should not fail. It should be possible to set new goals or vehicle position without problems.

Actual behavior

Planning fails, "Auto" cannot be selected; there is no path, and it is not possible to generate a new path even by changing start and goal pose of the vehicle. It feels like something is stuck on a loop.

Steps to reproduce

1. Open PSim with a loop-like set of lanelets.
2. Set a start pose in a given lanelet
3. Try to set a goal pose on a "previous" lanelet

In the given example, I am trying to make the car go from a start pose in lanelet "3" to a goal in lanelet "2" which is "behind" the current lanelet .

Versions

Ubuntu 22
ROS2 Humble
Autoware: Latest

Possible causes

It looks like a problem with the route handler that makes the behavior_path_planner die.

Additional context

It looks similar (but worse) than the issue presented in #1715. But I believe it is not the same issue. In this case, the problem happens when trying to set a goal to the "previous" lanelet in a loop. Also, there is no mention of planning failing to work afterwards, which I am reporting here.

Here is the lanelet map I used for the examples (it happens with other maps too though):
lanelet2_map.zip

[kosuke55]

thanks for the report!

I also get the message that some node died but there is no stack trace but I did notice these messages:

This is a bit odd, since I would expect to see a stack trace when a node dies:thinking:

If the node is not dead, it may be in a infinity loop. I saw the issue that *extend* function sometimes loop endless
(like this function https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/src/utils/utils.cpp#L3176)
Adding print may help identify where the loop is occurring.

[kosuke55]

and disabling some behavior path module(start_planner, goal_planner etc..) may be help to determine the cause.

[danielsanchezaran]

and disabling some behavior path module(start_planner, goal_planner etc..) may be help to determine the cause.

@kosuke55 Thanks for your input. I tried setting some prints on the extend function like:

But I did not see anything. I also disabled the goal_planner and start_planner, but I get the same behavior.

I believe there is a problem with the route_handler (?) I get this warning:
[scenario_selector-26] [WARN] [1698645501.368634568] [route_handler]: [Route Handler] getRouteUuid: Route has not been set yet.

[kosuke55]

this is caused by endless loop in turn_signal_decider.cpp
I sent PR to fix that #5444

[danielsanchezaran]

this is caused by endless loop in turn_signal_decider.cpp I sent PR to fix that #5444

Thank you @kosuke55. I cherry-picked the commit and it fixes the issue. I will close this issue once the PR is merged.
Video of the fixed issue:

cap-.2023-10-31-08-16-55.mp4

[danielsanchezaran]

Issue was solved with this PR #5444