Refactor client-specific code

Actor.py

@@ -224,7 +224,6 @@ class ActorState:
 
     #the direction the actor is facing
     yaw:float = 0.0
-    yaw_unreal:float = 0.0
 
     #For easy shared memory parsing
     def get_cart_state_vector(self):

SimConfig.py

@@ -77,7 +77,6 @@
 WRITE_TRAJECTORIES = False     #If True, all vehicle trajectories will be saved inside eval/ as csv files
 
 #Client (Unreal or similar)
-CLIENT_METER_UNIT = 100    	#Client unit (Server uses [m], Unreal client uses [cm], Carla uses [m])
 
 #Carla
 CARLA_COSIMULATION = False

SimTraffic.py

@@ -12,7 +12,7 @@
 import glog as log
 from copy import copy
 import csv
-from shm.SimSharedMemory import *
+from shm.SimSharedMemoryServer import *
 from Actor import *
 from sv.Vehicle import Vehicle
 from sp.Pedestrian import *
@@ -84,7 +84,7 @@ def start(self):
         if CARLA_COSIMULATION:
             self.carla_sync = CarlaSync()
             self.carla_sync.create_gs_actors(self.vehicles)
-    
+
         #Creates Shared Memory Blocks to publish all vehicles'state.
         self.create_traffic_state_shm()
         self.write_traffic_state(0 , 0.0, 0.0)
@@ -153,7 +153,7 @@ def tick(self, tick_count, delta_time, sim_time):
                 if self.vehicles[vid].type is Vehicle.EV_TYPE and vid in vstates:
                     if new_client_state:
                         self.vehicles[vid].update_sim_state(vstates[vid], client_delta_time)
-        
+
         #Read Carla socket
         if self.carla_sync:
             vstates, disabled_vehicles = self.carla_sync.read_carla_state(nv)
@@ -192,7 +192,7 @@ def tick(self, tick_count, delta_time, sim_time):
     def create_traffic_state_shm(self):
         #External Sim (Unreal) ShM
         if CLIENT_SHM:
-            self.sim_client_shm = SimSharedMemory()
+            self.sim_client_shm = SimSharedMemoryServer()
 
         #Internal ShM
         nv = len(self.vehicles)
@@ -250,12 +250,12 @@ def write_traffic_state(self, tick_count, delta_time, sim_time):
         #Shm for external Simulator (Unreal)
         #Write out simulator state
         if (self.sim_client_shm):
-            self.sim_client_shm.write_server_state(tick_count, delta_time, self.vehicles, self.pedestrians)
-        
+            self.sim_client_shm.write_server_state(tick_count, sim_time, delta_time, self.vehicles, self.pedestrians)
+
         #Carla socket
         if self.carla_sync:
             self.carla_sync.write_server_state(tick_count, delta_time, self.vehicles)
-     
+
 
 
     def detect_collisions(self,tick_count, delta_time, sim_time):

clients/ros2_client/geoscenario_client/geoscenario_client/geoscenario_client.py

@@ -24,7 +24,7 @@ def timer_callback(self):
             self.get_logger().warn('Waiting for geoscenario server', throttle_duration_sec=2)
             return
 
-        tick_count = header[0]
+        tick_count = header["tick_count"]
 
         # TODO: Consider a better way to keep the client synchronized with the server. Should be possible with semaphores
         if tick_count > self.previous_tick_count + 1:
@@ -34,8 +34,9 @@ def timer_callback(self):
 
 
         tick_msg = Tick()
-        tick_msg.tick_count = header[0]
-        tick_msg.delta_time = header[1]
+        tick_msg.tick_count = tick_count
+        tick_msg.simulation_time = header["simulation_time"]
+        tick_msg.delta_time = header["delta_time"]
 
         for vehicle in vehicles:
             msg = Vehicle()
@@ -46,7 +47,7 @@ def timer_callback(self):
             msg.position.z = vehicle["z"]
             msg.velocity.x = vehicle["vx"]
             msg.velocity.y = vehicle["vy"]
-            msg.yaw = vehicle["yaw"] * math.pi / 180
+            msg.yaw = vehicle["yaw"]
             msg.steering_angle = vehicle["steering_angle"]
             tick_msg.vehicles.append(msg)
 
@@ -59,7 +60,7 @@ def timer_callback(self):
             msg.position.z = pedestrian["z"]
             msg.velocity.x = pedestrian["vx"]
             msg.velocity.y = pedestrian["vy"]
-            msg.yaw = pedestrian["yaw"] * math.pi / 180
+            msg.yaw = pedestrian["yaw"]
             tick_msg.pedestrians.append(msg)
 
         self.tick_pub.publish(tick_msg)

clients/ros2_client/geoscenario_msgs/msg/Tick.msg

@@ -1,7 +1,8 @@
 std_msgs/Header header
 
 uint32 tick_count
-float64 delta_time  # Time passed since last tick (s)
+float64 simulation_time  # Time passed since the beginning of the simulation (s)
+float64 delta_time       # Time passed since last tick (s)
 
 geoscenario_msgs/Vehicle[] vehicles
 geoscenario_msgs/Pedestrian[] pedestrians

clients/unreal/GeoScenarioModule/GSClient.cpp

@@ -6,6 +6,7 @@
 #include "Misc/DateTime.h"
 #include <sstream>
 #include <string>
+#include <cmath>
 
 const key_t  SHM_KEY = 123456;
 const key_t  SEM_KEY = 346565;
@@ -148,9 +149,10 @@ void AGSClient::ReadServerState(float deltaTime)
 		return;
 	}
 	//parse frame stats
+	float server_simulation_time;
 	float server_delta_time;
 	int server_tick_count, nvehicles{0}, npedestrians{0}, vid{0}, pid{0};
-	iss >> server_tick_count >> server_delta_time >> nvehicles >> npedestrians;
+	iss >> server_tick_count >> server_simulation_time >> server_delta_time >> nvehicles >> npedestrians;
 
 	// parse vehicles
 	int vehicles_read{0};
@@ -163,11 +165,18 @@ void AGSClient::ReadServerState(float deltaTime)
 		int v_type;
 		float x, y, z, x_vel, y_vel, yaw, steer;
 		iss >> v_type >> x >> y >> z >> x_vel >> y_vel >> yaw >> steer;
+		// Unreal uses cm instead of m, and y is flipped
+		x *= 100.0f;
+		y *= -100.0f;
+		z *= 100.0f;
+		x_vel *= 100.0f;
+		y_vel *= -100.0f;
+		// Unreal uses degrees instead of radians
+		yaw = yaw * 180.0f / M_PI;
+
 		// Unreal's y axis is inverted from GS server's.
-		y *= -1;
-		y_vel *= -1;
 		yaw -= 90;
-	    // GEngine->AddOnScreenDebugMessage(-1, 0, FColor::Red, FString::Printf(TEXT("Yaw %f"), yaw));
+		// GEngine->AddOnScreenDebugMessage(-1, 0, FColor::Red, FString::Printf(TEXT("Yaw %f"), yaw));
 
 		FVector loc = {x, y, z};
 		FRotator rot = {0.0f, yaw, 0.0f};
@@ -219,11 +228,17 @@ void AGSClient::ReadServerState(float deltaTime)
 		int p_type;
 		float x, y, z, x_vel, y_vel, yaw;
 		iss >> p_type >> x >> y >> z >> x_vel >> y_vel >> yaw;
-		// Unreal's y axis is inverted from GS server's.
-		y *= -1;
-		y_vel *= -1;
+		// Unreal uses cm instead of m, and y is flipped
+		x *= 100.0f;
+		y *= -100.0f;
+		z *= 100.0f;
+		x_vel *= 100.0f;
+		y_vel *= -100.0f;
+		// Unreal uses degrees instead of radians
+		yaw = yaw * 180.0f / M_PI;
+
 		yaw -= 90;
-			// GEngine->AddOnScreenDebugMessage(-1, 0, FColor::Red, FString::Printf(TEXT("Yaw %f"), yaw));
+		// GEngine->AddOnScreenDebugMessage(-1, 0, FColor::Red, FString::Printf(TEXT("Yaw %f"), yaw));
 
 		FVector loc = {x, y, z};
 		FRotator rot = {0.0f, yaw, 0.0f};
@@ -495,9 +510,10 @@ void AGSClient::WriteClientState(int tickCount, float deltaTime)
 			loc[2] = 0.0f;
 			ASimVehicle *sv = Cast<ASimVehicle>(gsv.actor);
 			int active = sv != nullptr ? (int)(sv->GetActive()) : 1;
+			// Unreal uses cm instead of m, and y is flipped
 			oss << Elem.Key << " "
-				<< gsv.vehicle_state.x << " " << gsv.vehicle_state.y << " " << gsv.vehicle_state.z << " "
-				<< gsv.vehicle_state.x_vel << " " << gsv.vehicle_state.y_vel << " " /* TODO: gsv.vehicle_state.yaw << " " */
+				<< gsv.vehicle_state.x / 100.0f << " " << -gsv.vehicle_state.y / 100.0f << " " << gsv.vehicle_state.z / 100.0f << " "
+				<< gsv.vehicle_state.x_vel / 100.0f << " " << -gsv.vehicle_state.y_vel / 100.0f << " " /* TODO: gsv.vehicle_state.yaw << " " */
 				<< active << '\n';
 		}
 		else
@@ -517,9 +533,10 @@ void AGSClient::WriteClientState(int tickCount, float deltaTime)
 			loc[2] = 0.0f;
 			ASimPedestrian *sp = Cast<ASimPedestrian>(gsp.actor);
 			int active = sp != nullptr ? (int)(sp->GetActive()) : 1;
+			// Unreal uses cm instead of m, and y is flipped
 			oss << Elem.Key << " "
-				<< gsp.pedestrian_state.x << " " << gsp.pedestrian_state.y << " " << gsp.pedestrian_state.z << " "
-				<< gsp.pedestrian_state.x_vel << " " << gsp.pedestrian_state.y_vel << " " /* TODO: gsp.pedestrian_state.yaw << " " */
+				<< gsp.pedestrian_state.x / 100.0f << " " << -gsp.pedestrian_state.y / 100.0f << " " << gsp.pedestrian_state.z / 100.0f << " "
+				<< gsp.pedestrian_state.x_vel / 100.0f << " " << -gsp.pedestrian_state.y_vel / 100.0f << " " /* TODO: gsp.pedestrian_state.yaw << " " */
 				<< active << '\n';
 		}
 		else

shm/README.md

@@ -1,6 +1,20 @@
 # Shared Memory
 
-`SimSharedMemory.py` defines a class that the server uses to manage shared memory.
+`SimSharedMemoryServer.py` defines a class that the server uses to manage shared memory.
 
 `SimSharedMemoryClient.py` defines a class that can be used by client applications to read from the shared memory.
 It purposely does not contain any GeoScenario dependencies.
+
+## Shared Memory Layout
+
+The shared memory written by the server is one large space-separated UTF8-encoded string with the following layout:
+
+```
+tick_count simulation_time delta_time n_vehicles n_pedestrians
+vid v_type x y z vx vy yaw steering_angle
+...
+pid p_type x y z vx vy yaw
+...
+```
+
+All data is in base SI units (m, m/s, s, radians), and position data is ENU with positive yaw indicating counter-clockwise from east. 

shm/SimSharedMemoryClient.py

@@ -54,12 +54,12 @@ def connect_to_shared_memory(self):
     def read_server_state(self):
         """ Reads from shared memory pose data for each agent.
             Shared memory format:
-                tick_count delta_time n_vehicles n_pedestrians
+                tick_count simulation_time delta_time n_vehicles n_pedestrians
                 vid v_type x y z vx vy yaw steering_angle
                 pid p_type x y z vx vy yaw
                 ...
         """
-        header = None
+        header = {}
         vehicles = []
         pedestrians = []
 
@@ -92,16 +92,18 @@ def read_server_state(self):
 
         try:
             header_str = data_arr[0].split(' ')
-            header = [int(header_str[0]), float(header_str[1]), int(header_str[2]), int(header_str[3])]
-            num_vehicles = header[2]
-            num_pedestrians = header[3]
+            header["tick_count"] = int(header_str[0])
+            header["simulation_time"] = float(header_str[1])
+            header["delta_time"] = float(header_str[2])
+            header["n_vehicles"] = int(header_str[3])
+            header["n_pedestrians"] = int(header_str[4])
         except Exception as e:
             print("Header parsing exception")
             print("data_arr[0]: %s ", data_arr[0])
             print(e)
 
         try:
-            for ri in range(1, num_vehicles + 1):
+            for ri in range(1, header["n_vehicles"] + 1):
                 vehicle = {}
                 id, type, x, y, z, vx, vy, yaw, str_angle = data_arr[ri].split()
                 vehicle["id"] = int(id)
@@ -114,13 +116,12 @@ def read_server_state(self):
                 vehicle["yaw"] = float(yaw)
                 vehicle["steering_angle"] = float(str_angle)
                 vehicles.append(vehicle)
-
         except Exception as e:
             print("VehicleState parsing exception")
             print(e)
 
         try:
-            for ri in range(num_vehicles + 1, num_vehicles + 1 + num_pedestrians):
+            for ri in range(header["n_vehicles"] + 1, header["n_vehicles"] + 1 + header["n_pedestrians"]):
                 pedestrian = {}
                 id, x, y, z, vx, vy, yaw = data_arr[ri].split()
                 pedestrian["id"] = int(id)
@@ -131,7 +132,6 @@ def read_server_state(self):
                 pedestrian["vy"] = float(vy)
                 pedestrian["yaw"] = float(yaw)
                 pedestrians.append(pedestrian)
-
         except Exception as e:
             print("PedestrianState parsing exception")
             print(e)

shm/SimSharedMemory.py → shm/SimSharedMemoryServer.py

@@ -5,7 +5,7 @@
 
 # Class defining shared memory structure used to sync with
 # an external simulator (client)
-class SimSharedMemory(object):
+class SimSharedMemoryServer(object):
 
     def __init__(self, ss_shm_key=SHM_KEY, ss_sem_key=SEM_KEY, cs_shm_key=CS_SHM_KEY, cs_sem_key=CS_SEM_KEY):
         # Server state shared memory
@@ -30,12 +30,12 @@ def __init__(self, ss_shm_key=SHM_KEY, ss_sem_key=SEM_KEY, cs_shm_key=CS_SHM_KEY
             log.error("Error creating Shared Memory")
             self.is_connected = False
 
-    def write_server_state(self, tick_count, delta_time, vehicles, pedestrians):
+    def write_server_state(self, tick_count, sim_time, delta_time, vehicles, pedestrians):
         """ Writes to shared memory pose data for each agent.
             @param vehicles:      dictionary of type <int, Vehicle>
             @param pedestrians:      dictionary of type <int, Pedestrian>
             Shared memory format:
-                tick_count delta_time n_vehicles n_pedestrians
+                tick_count simulation_time delta_time n_vehicles n_pedestrians
                 vid v_type x y z vx vy yaw steering_angle
                 pid p_type x y z vx vy yaw
                 ...
@@ -44,19 +44,33 @@ def write_server_state(self, tick_count, delta_time, vehicles, pedestrians):
             return
 
         # write tick count, deltatime, numbers of vehicles and pedestrians
-        write_str = "{} {} {} {}\n".format(int(tick_count), delta_time, len(vehicles), len(pedestrians))
-        # write vehicle states
+        write_str = "{} {} {} {} {}\n".format(int(tick_count), sim_time, delta_time, len(vehicles), len(pedestrians))
+        # write vehicle states, rounding the numerical data to reasonable significant figures
         for svid in vehicles:
-            vid, v_type, position, velocity, yaw, steering_angle = vehicles[svid].get_full_state_for_client()
+            vid, v_type, position, velocity, yaw, steering_angle = vehicles[svid].get_sim_state()
             write_str += "{} {} {} {} {} {} {} {} {}\n".format(
-                vid, v_type, position[0], position[1], position[2],
-                velocity[0], velocity[1], yaw, steering_angle)
-
+                vid, v_type,
+                round(position[0], 4),
+                round(position[1], 4),
+                round(position[2], 4),
+                round(velocity[0], 4),
+                round(velocity[1], 4),
+                round(yaw * math.pi / 180, 6),
+                round(steering_angle, 6)
+            )
+
+        # write pedestrian states, rounding the numerical data to reasonable significant figures
         for spid in pedestrians:
             pid, p_type, position, velocity, yaw = pedestrians[spid].get_sim_state()
             write_str += "{} {} {} {} {} {} {} {}\n".format(
-                pid, p_type, position[0], position[1], position[2],
-                velocity[0], velocity[1], yaw)
+                pid, p_type,
+                round(position[0], 4),
+                round(position[1], 4),
+                round(position[2], 4),
+                round(velocity[0], 4),
+                round(velocity[1], 4),
+                round(yaw * math.pi / 180, 6)
+            )
 
         # sysv_ipc.BusyError needs to be caught
         try:
@@ -128,11 +142,11 @@ def read_client_state(self, nvehicles, npedestrians):
                     vid, x, y, z, x_vel, y_vel, is_active = data_arr[ri].split()
                     vs = VehicleState()
                     vid = int(vid)
-                    vs.x = float(x) / CLIENT_METER_UNIT
-                    vs.y = -float(y) / CLIENT_METER_UNIT
-                    vs.z = float(z) / CLIENT_METER_UNIT
-                    vs.x_vel = float(x_vel) / CLIENT_METER_UNIT
-                    vs.y_vel = -float(y_vel) / CLIENT_METER_UNIT
+                    vs.x = float(x)
+                    vs.y = float(y)
+                    vs.z = float(z)
+                    vs.x_vel = float(x_vel)
+                    vs.y_vel = float(y_vel)
                     #Estimating yaw because is not being published by client.
                     #We use the velocity vectors and only if vehicle is moving at least 10cm/s to avoid noise
                     if (abs(vs.y_vel) > 0.01) or (abs(vs.x_vel) > 0.01):
@@ -157,11 +171,11 @@ def read_client_state(self, nvehicles, npedestrians):
                     pid, x, y, z, x_vel, y_vel, is_active = data_arr[ri].split()
                     ps = PedestrianState()
                     pid = int(pid)
-                    ps.x = float(x) / CLIENT_METER_UNIT
-                    ps.y = -float(y) / CLIENT_METER_UNIT
-                    ps.z = float(z) / CLIENT_METER_UNIT
-                    ps.x_vel = float(x_vel) / CLIENT_METER_UNIT
-                    ps.y_vel = -float(y_vel) / CLIENT_METER_UNIT
+                    ps.x = float(x)
+                    ps.y = float(y)
+                    ps.z = float(z)
+                    ps.x_vel = float(x_vel)
+                    ps.y_vel = float(y_vel)
                     pstates[pid] = ps
                     if not int(is_active):
                         disabled_pedestrians.append(pid)