[Bug] Replan not triggered if the current rollouts reach the goal

[blabla-my]

Describtion

If the current rollouts can reach the goal, replanning will not be triggered. Thus some collision will happen while there are obstacles between ego and goal.

Required infomation:

• ROS melodic (Ubuntu 18.04)
• Autoware 1.15 + Openplanner 2.5
• Simluator: LGSVL
• Planner: op_global_planner + op_local_planner
• Controller: pure_pursuit + twist_filter

Possible Root Cause and Solution

Root Cause

In src/autoware/common/op_planner/src/DecisionMaker.cpp, preCalcPrams->bFinalLocalTrajectory is used to judge if current local path can reach the goal, and while preCalcPrams->bFinalLocalTrajectory is true, replan won't be triggered.

Possible Solution

Disable preCalcPrams->bFinalLocalTrajectory, making the replan continue to be triggered after the point where the local path could reach the goal.
To be specific, update line 380-387 in DecisionMaker.cpp to:

if(m_Path.size() > 0 && m_TotalOriginalPaths.size() > 0)
{
	double d_between_ends = hypot(m_TotalOriginalPaths.at(m_iCurrentTotalPathId).back().pos.y - m_Path.back().pos.y, m_TotalOriginalPaths.at(m_iCurrentTotalPathId).back().pos.x - m_Path.back().pos.x);
	if(d_between_ends < m_params.pathDensity)
	{
		// pValues->bFinalLocalTrajectory = true;
	}
}

Demostration Video

Video to demonstrate this hazard: https://youtu.be/F5UjRHmLVUE

In this scenario, ego won't select a better trajectory to avoid the truck since the replan is not triggered. This scenario was tested more than 10 times and the collision always happened.

After applying the patch above, the collision won't happend (tested more than 10 times and none of them involved the collision). Video: https://youtu.be/6uezdQ3w2FU

@hatem-darweesh

[hatem-darweesh]

Thank you for contributing to OpenPlanner.
This a know design decision, for some projects we want to disable avoidance in the last section. But follow should work.
In your video the ego vehicle stopped, but it got hit by the truck.
You can fix it in your version.
Or add a hyper parameter in op_common_param that enable and disable this feature. If so, please submit a pull request.

[blabla-my]

Thanks for your reply !

I will try to find a better solution.

[hatem-darweesh]

@blabla-my
consider this scenario:
The goal position is actually a bus stop, so you want to bus to stop exactly in the stopping location.
if the avoidance is enabled the bus may stop in the middle of the road if there is another bus or car stopping before or at the bus stop.

[blabla-my]

Thanks!

This is a very nice case to understand why you say that no replan in last section is a design choice.

[blabla-my]

However, I think it's better for ego to still have some lightly ability to avoid, but not too strong ability.

For your example, I think the second bus should preserve its ability to slightly adjust its pose for avoidance, but not go over the first bus to reach goal.

[hatem-darweesh]

It is an application dependent feature. So everyone can customize accordingly

[AV-Security]

It is an application dependent feature. So everyone can customize accordingly
@hatem-darweesh Hi Hatem, I've introduced a new hyperparameter that offers users greater flexibility in configuring this feature. I've submitted a pull request for your review and eagerly await your response. Thank you!
hatem-darweesh/common#18
hatem-darweesh/core_planning#17