Coordinate offset of ego pose in Closed-loop simulation with Town12

[ChipsICU]

Hi guys, I want to transfer the centerline of map to the ego vehicle coordinate, everything works fine in open loop (which means using dataset of b2d dataset)， but when I use the same code in carla (0.9.15), it looks strange.

Here're the comparison results. (Taking route id="1711" town="Town12" and ParkingCutIn_Town12_Route901_Weather9 for example).

The biggest difference is that the poses provided of ego are different, in route id="1711" town="Town12" is (0.6022555710831057, 3674.5574101156035), while in ParkingCutIn_Town12_Route901_Weather9 is (-69.44846595722188, 3676.610212767409).

I also tested my code in other closed loop maps, it looks ok, here is route id="24224" road_id="12" town="Town02"

If You Want Me To Provide my code, Please Let Me Know, Thks.

[ChipsICU]

Thank you very much for this excellent work, but I think I found the reason in the end is that the code you provided in vad_b2d_agent.py regarding the calculation of lat_ref and lon_ref seems to be wrong for Town12. Here is a function copyed from carla_garage, use this for minor changes in team_code can get correct lat_ref and lon_ref.

Here is fixed result of route id="1711" town="Town12"

[jiaxiaosong1002]

@ChipsICU Thanks for your advice. Your point is correct!

[jayyoung0802]

@ChipsICU Hi, Can you provide the content of the changes? Is it using _get_latlon_ref(world_map)?

[ChipsICU]

@ChipsICU Hi, Can you provide the content of the changes? Is it using _get_latlon_ref(world_map)?
Yes, your're right. The following is my modification, it is not very elegant, but I hope it can help you.

1. Put this function into leaderboard_evaluator.py, then use this to feedback the lon_ref and lat_ref to agents

self.agent_instance.lat_ref = self.lat_ref
self.agent_instance.lon_ref = self.lon_ref

2. Modify the agent code, take vad for example

• add this part

def __init__(self, host, port, debug):
      super(VadAgent, self).__init__(host, port, debug)
      self.lat_ref : float = 42.0
      self.lon_ref : float = 2.0

• comment out this part

def _init(self):
      # try:
      #     locx, locy = self._global_plan_world_coord[0][0].location.x, self._global_plan_world_coord[0][0].location.y
      #     lon, lat = self._global_plan[0][0]['lon'], self._global_plan[0][0]['lat']
      #     EARTH_RADIUS_EQUA = 6378137.0
      #     def equations(vars):
      #         x, y = vars
      #         eq1 = lon * math.cos(x * math.pi / 180) - (locx * x * 180) / (math.pi * EARTH_RADIUS_EQUA) - math.cos(x * math.pi / 180) * y
      #         eq2 = math.log(math.tan((lat + 90) * math.pi / 360)) * EARTH_RADIUS_EQUA * math.cos(x * math.pi / 180) + locy - math.cos(x * math.pi / 180) * EARTH_RADIUS_EQUA * math.log(math.tan((90 + x) * math.pi / 360))
      #         return [eq1, eq2]
      #     initial_guess = [0, 0]
      #     solution = fsolve(equations, initial_guess)
      #     self.lat_ref, self.lon_ref = solution[0], solution[1]
      # except Exception as e:
      #     print("!!!")
      #     print(e, flush=True)
      #     self.lat_ref, self.lon_ref = 0, 0      
      self._route_planner = RoutePlanner(4.0, 50.0, lat_ref=self.lat_ref, lon_ref=self.lon_ref)
      self._route_planner.set_route(self._global_plan, True)
      self.initialized = True

[jayyoung0802]

Of course, you can get the ground truth about lat_ref/lon_ref this way. But when we are in closed-loop evaluation such as carla official leaderboard 2.0, we cannot get agent_instance. Please be aware.

By the way, from the perspective of the coordinate system, only the reference origin is changed, and the relative position does not change.

[ChipsICU]

Thank you for your reminder, but when I need a map, the offset will always have an error effect.