A solution to filter the rollouts that are outside of the drivable areas in TrajectoryEvaluator.

[blabla-my]

Problem describtion and previous solution

Sometimes the ego car will drive out of the road since there are no definitions of curbs in the map which can block rollouts as obstacles.

This problem has been described in this issue: https://github.com/autowarefoundation/autoware_ai_planning/issues/54 . And there has been one solution that adds the curb definitions to the map: https://github.com/hatem-darweesh/autoware.ai.openplanner/issues/5

Our solution

We implement another solution which dynamically filters the rollouts that are outside of the drivable area. Our patch method is:

1. Enable load_wayarea in VectorMapLoader to load wayareas from wayarea.csv.
2. Use PlanningHelpers::PointInsidePolygon to judge whether a specific rollout waypoint is inside of some wayareas.
3. If there exist such a rollout i and one of its trajectory point p, that p is not inside of any wayareas, the rollout i will be marked as blocked.

Demonstration videos

To demonstrate that the patch is working correctly, we made four videos as below:

1. Single ego car scenario with no patch. the rollouts that are outside of the map are not blocked ( colored green in rviz ).

https://user-images.githubusercontent.com/55621094/192413599-89c3e7ee-602f-4d8d-b8b8-b205fb771831.mp4

2. Single ego car scenario with patch. the rollouts are blocked ( colored red ).

https://user-images.githubusercontent.com/55621094/192414432-c42a18c4-bd29-4644-9773-a85af0642797.mp4

3. Multi-NPC scenario with no patch. the ego drives outside of the road due to one npc ahead of ego and the unblocked rollouts.

https://user-images.githubusercontent.com/55621094/192414531-25dc9603-9e07-48df-8228-e8dcefba81db.mp4

4. Muliti-NPC scenario with patch. The ego keeps driving on the lane since the rollouts are blocked correctly.

https://user-images.githubusercontent.com/55621094/192414553-349a7bd7-c262-4bb2-a46a-c6a228914d12.mp4

Patch details

A function to judge if a given waypoint is inside of the map using PlanningHelpers:

bool MappingHelpers::PointInMap(RoadNetwork& map,const WayPoint& pos)
{   
    for(unsigned int i=0; i < map.boundaries.size(); i++)
    {
        if ( PlanningHelpers::PointInsidePolygon(map.boundaries.at(i).points, pos) )
        {
            return true;
        }
    }
    return false;
}

A function to filter the rollouts in TrajectoryEvaluator.cpp, this function is called in TrajectoryEvaluator::doOneStep:

void TrajectoryEvaluator::blockTrajectoryOutOfRoad(const std::vector<std::vector<WayPoint> >& roll_outs, std::vector<TrajectoryCost>& trajectory_costs)
{
  for(unsigned int i=0; i<roll_outs.size(); i++)
  {
    for(unsigned int j=0; j<roll_outs.at(i).size(); j++)
    {
      if( ! MappingHelpers::PointInMap(m_Map,roll_outs.at(i).at(j)) )
      {
        trajectory_costs.at(i).bBlocked = true;
        break;
      }
    }
  }
}

@hatem-darweesh Hello Hatem :) , Can you help to check if this solution is reasonable ?

[hatem-darweesh]

@blabla-my I will check in the weekend and let you know.

[blabla-my]

@hatem-darweesh Thank you!

There are two more details:

1. TrajectoryEvaluator::blockTrajectoryOutOfRoad() is invoked in TrajectoryEvaluator::doOneStep()
2. I add a RoadNetwork m_Map in class TrajectoryEvaluator, which is used in blockTrajectoryOutOfRoad. And the map data is loaded into m_Map in op_trajectory_evaluator_core.cpp.

I will upload some diff files later.

Thanks again :D

[hatem-darweesh]

Hi @blabla-my , Sorry for my late reply.

This is a very nice solution, it show good understanding to the OpenPlanner library. One suggestion to improve performance:

• Move this test to trajectory generator. When the trajectory is outside the area, remove it from the generated rollouts list. If this solution causes an issue, your proposal is fine, please make a pull request.

[blabla-my]

Thanks @hatem-darweesh, I will try to do some optimization. if the results are good, I will try to send a pull request for this solution.