METS-R New Interface Progress

Author: Eric-Vin

src/scenic/simulators/metsr/client.py

@@ -93,29 +93,45 @@ def tick(self):
 
         self.current_tick = res["TICK"]
 
-    def generate_trip(self, vehID, origin, destination):
-        msg = {
-            "TYPE": "CTRL_generateTrip",
-            "vehID": vehID,
-            "origin": origin,
-            "destination": destination,
-        }
+    def generate_trip(self, vehID, origin=-1, destination=-1):
+        msg = {"TYPE": "CTRL_generateTrip", "DATA": []}
+        if not isinstance(vehID, list):
+            vehID = [vehID]
+        if not isinstance(origin, list):
+            origin = [origin] * len(vehID)
+        if not isinstance(destination, list):
+            destination = [destination] * len(vehID)
+
+        assert (
+            len(vehID) == len(origin) == len(destination)
+        ), "Length of vehID, origin, and destination must be the same"
+        for vehID, origin, destination in zip(vehID, origin, destination):
+            msg["DATA"].append(
+                {"vehID": vehID, "vehType": True, "orig": origin, "dest": destination}
+            )
 
         res = self.send_receive_msg(msg, ignore_heartbeats=True)
 
         assert res["TYPE"] == "CTRL_generateTrip", res["TYPE"]
         assert res["CODE"] == "OK", res["CODE"]
+        return res
 
-    def query_vehicle(self, vehID, private_veh=False, transform_coords=False):
-        msg = {
-            "TYPE": "QUERY_vehicle",
-            "ID": vehID,
-            "PRV": private_veh,
-            "TRAN": transform_coords,
-        }
+    def query_vehicle(self, id=None, private_veh=False, transform_coords=False):
+        msg = {"TYPE": "QUERY_vehicle"}
+        if id is not None:
+            msg["DATA"] = []
+            if not isinstance(id, list):
+                id = [id]
+            if not isinstance(private_veh, list):
+                private_veh = [private_veh] * len(id)
+            if not isinstance(transform_coords, list):
+                transform_coords = [transform_coords] * len(id)
+            for veh_id, prv, tran in zip(id, private_veh, transform_coords):
+                msg["DATA"].append(
+                    {"vehID": veh_id, "vehType": prv, "transformCoord": tran}
+                )
 
         res = self.send_receive_msg(msg, ignore_heartbeats=True)
-
         assert res["TYPE"] == "ANS_vehicle", res["TYPE"]
         return res
 

src/scenic/simulators/metsr/model.scenic

@@ -14,6 +14,7 @@ simulator METSRSimulator(
     map_name="Data.properties.CARLA",
     timestep=globalParameters.timestep,
     sim_timestep=globalParameters.simTimestep,
+    verbose=True
     )
 
 _DAY_MOD = 24*60*60

src/scenic/simulators/metsr/simulator.py

@@ -8,9 +8,9 @@
 
 
 class METSRSimulator(Simulator):
-    def __init__(self, host, port, map_name, timestep, sim_timestep):
+    def __init__(self, host, port, map_name, timestep, sim_timestep, verbose=False):
         super().__init__()
-        self.client = METSRClient(host=host, port=port)
+        self.client = METSRClient(host=host, port=port, verbose=verbose)
 
         self.map_name = map_name
         self.timestep = timestep
@@ -71,14 +71,21 @@ def step(self):
         for _ in range(self.sim_ticks_per):
             self.client.tick()
 
+    def updateObjects(self):
+        obj_veh_ids = [self.getPrivateVehId(obj) for obj in self.objects]
+        raw_veh_data = self.client.query_vehicle(obj_veh_ids, True, True)
+        self.obj_data_cache = {obj: raw_veh_data[i] for i, obj in enumerate(self.objects)}
+        super().updateObjects()
+        self.obj_data_cache = None
+
     def getProperties(self, obj, properties):
         call_kwargs = {
             "vehID": self.getPrivateVehId(obj),
             "private_veh": True,
             "transform_coords": True,
         }
 
-        raw_data = self.client.query_vehicle(**call_kwargs)
+        raw_data = self.obj_data_cache[obj]
 
         position = Vector(raw_data["x"], raw_data["y"], 0)
         speed = raw_data["speed"]