obstacle_stop_planner does not revert its avoidance path after the avoided obstacle is gone.

[VRichardJP]

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 have deployed several vehicles near a train station. It is very common for other vehicles to come, stop for a minute to drop off someone, then go away.

Although the avoidance works nicely while the vehicle is parked on the side of the road, autoware still wants to follow the avoidance path after the vehicle is gone. This is very akward for the safety driver, as autoware moves away from its lane for no apparent reason

This can be reproduced with the planning simulator easily:

1. No obstacle -> no avoidance
   

2. A vehicle comes and park -> avoidance path is generated after a few seconds
   

3. The vehicle is gone, but autoware does not revert the path to 1 (even if you wait for a very long time)
   

Expected behavior

If there is no obstacle, there should be no avoidance

Actual behavior

generated avoidance path stays for ever.

Steps to reproduce

See the pictures. You can use the fake bus to emulate a parked vehicle, and delete all obstacles after avoidance path is generated.

Versions

No response

Possible causes

No response

Additional context

No response

[takayuki5168]

FYI: This function is not in obstacle_stop_planner but avoidance module in behavior_path_planner

@satoshi-ota Indeed, this function seems necessary in some cases. You don't have a plan to implement this function for now?

[satoshi-ota]

@VRichardJP There is currently no function to cancel/revert avoidance path when it is no longer necessary, so this is how it would behave in the scene described above. I am currently working on developing a function that takes into account vehicles in adjacent lanes when avoiding, and if it is not safe to follow avoidance path right now, the vehicle stops behind the avoidance target and lets the adjacent lane's vehicle pass over, so I will consider this function together.

Of course, if you would like to implement revert function in avoidance module as soon as possible, your contributions are welcome 😄

[VRichardJP]

@satoshi-ota I am not well versed in the planning logic, but if I find the time I'll try to implement it.

[ismetatabay]

@satoshi-ota @VRichardJP This issue had caught my attention before, and I did a little research on it. I think the problem is that the avoidance module in the Behavior path planner tree, after detecting the object once and running, stays in the running state until the vehicle reach its goal. This is how I solved this problem:

autoware.universe/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

Lines 97 to 114 in 29e8b43

	BT::NodeStatus AvoidanceModule::updateState()
	{
	const auto is_plan_running = isAvoidancePlanRunning();
	DebugData debug;
	const auto avoid_data = calcAvoidancePlanningData(debug);
	const bool has_avoidance_target = !avoid_data.objects.empty();
	if (!is_plan_running && !has_avoidance_target) {
	current_state_ = BT::NodeStatus::SUCCESS;
	} else {
	current_state_ = BT::NodeStatus::RUNNING;
	}
	DEBUG_PRINT(
	"is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target);
	return current_state_;
	}

I'm checking the object data from the planner_data_ by adding an if statement to above method. If there is no object, I make the module complete the task by making the avoidance module current_state_ = SUCCESS. If you think this solution is a suitable solution, I can create a PR on this issue by adding this check option as a parameter. Here is the video:

2022-11-17.13-39-55.mp4

[VRichardJP]

That looks perfect!