What should the ordering of the roadblock IDs in the simulation be like?

[MTDzi]

I'm looking at the MLPlanner._initialization attribute and I'm trying to extract a (rough) route based on that. I extended the MLPlanner class, and added a method:

...
    def _get_route_from_roadblocks(self) -> npt.NDArray:
        road_level_route = []
        map_api = self._initialization.map_api
        for roadblock_id in self._initialization.route_roadblock_ids:
            potential_result = map_api._get_roadblock(roadblock_id)
            if potential_result is None:
                potential_result = map_api._get_roadblock_connector(roadblock_id)
                centroid = potential_result._get_roadblock_connector().geometry.centroid
            else:
                centroid = potential_result._get_roadblock().geometry.centroid

            if centroid: road_level_route.append([centroid.x, centroid.y])
        road_level_route = np.array(road_level_route)
        road_level_route = np.unique(road_level_route, axis=0)  # This is because some of the points were identical

        return road_level_route
...

but I noticed the roadblocks don't come in the order that complies with the expert (GT) trajectory.

I made a plot in which we can see the expert trajectory (orange) and the route extracted with the function above (blue) with the points connected so that it's clear which point follows which:

(This is for the 2021.10.05.07.10.04_veh-52_01442_01802 log file, but there are many such cases.)

I assumed that the roadblocks are in an order that "tells" the planner how to go. (I guess the reason I thought so is that the mission goal and the last roadblock match.)

Is this assumption wrong? Or should the roadblocks be in an order that complies with the GT trajectory?

---

Addendum: I suspected the roadblock connectors might cause some issues but even if I use the following version of that method:

...
    def _get_route_from_roadblocks(self) -> npt.NDArray:
        road_level_route = []
        map_api = self._initialization.map_api
        for roadblock_id in self._initialization.route_roadblock_ids:
            potential_result = map_api._get_roadblock(roadblock_id)
            if potential_result is None:
                centroid = None
                # potential_result = map_api._get_roadblock_connector(roadblock_id)
                # centroid = potential_result._get_roadblock_connector().geometry.centroid
            else:
                centroid = potential_result._get_roadblock().geometry.centroid

            if centroid: road_level_route.append([centroid.x, centroid.y])
        road_level_route = np.array(road_level_route)
        road_level_route = np.unique(road_level_route, axis=0)  # This is because some of the points were identical

        return road_level_route
...

but even then this is what I get:

[MTDzi]

I'm not sure if I did a good job at explaining what the issue is, please let me know if I should clarify / expand what I mean. Also, if the plots are not clear, let me know.

[gianmarco-motional]

Hey @MTDzi, it all looks good except one part, np.unique returns a sorted array, so if you notice your plots are always sorted on the x-axis. Without it it should be fine, otherwise let me know :slightly_smiling_face:

[MTDzi]

Kudos for spotting this bug. Thanks!