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://youtu.be/LpveSclVLXQ

2. Single ego car scenario with patch. the rollouts are blocked ( colored red ) https://youtu.be/vrKy87u0f4g

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://youtu.be/PK15sREkdog

4. Muliti-NPC scenario with patch. The ego keeps driving on the lane since the rollouts are blocked correctly. https://youtu.be/p5G8eTCMnYE

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 Can you help to check if this solution is correct ?