Render traffic lights in Envision

CHANGELOG.md

@@ -12,6 +12,7 @@ Copy and pasting the git commit messages is __NOT__ enough.
 ### Added
 - `visdom` can now be configured through the engine.ini configuration file `visdom:enabled`, `visdom:hostname`, and `visdom:port` (environment variables `SMARTS_VISDOM_ENABLED`, `SMARTS_VISDOM_HOSTNAME`, `SMARTS_VISDOM_PORT`.)
 - Added an install extra that installs the requirements for all optional modules. Use `pip install .[all]`.
+- Traffic light signals are now visualized in Envision.
 ### Changed
 - Changed waypoints in sumo maps to use more incoming lanes into junctions.
 - Increased the cutoff radius for filtering out waypoints that are too far away in junctions in sumo maps.

envision/data_formatter.py

@@ -362,6 +362,7 @@ def _format_signal_state(obj: SignalState, data_formatter: EnvisionDataFormatter
     data_formatter.add(obj.signal_id, op=Operation.REDUCE)
     assert type(obj.signal_light_state) is SignalLightState
     data_formatter.add(obj.signal_light_state)
+    data_formatter.add(obj.signal_position, op=Operation.FLATTEN)
 
 
 def _format_events(obj: Events, data_formatter: EnvisionDataFormatter):

envision/types.py

@@ -77,6 +77,7 @@ class SignalState(NamedTuple):
 
     signal_id: str
     signal_light_state: SignalLightState
+    signal_position: Tuple[float, float, float]
 
 
 class State(NamedTuple):

envision/web/src/components/simulation.js

@@ -53,6 +53,7 @@ import DebugInfoDisplay from "./DebugInfoDisplay.js";
 import earcut from "earcut";
 import { SceneColors } from "../helpers/scene_colors.js";
 import unpack_worldstate from "../helpers/state_unpacker.js";
+import TrafficSignals from "./traffic_signals.js";
 
 // Required by Babylon.js
 window.earcut = earcut;
@@ -63,7 +64,7 @@ export default function Simulation({
   egoView,
   controlModes,
   canvasRef = null,
-  onElapsedTimesChanged = (current, total) => {},
+  onElapsedTimesChanged = (current, total) => { },
   style = {},
   playing = true,
   playingMode,
@@ -330,6 +331,7 @@ export default function Simulation({
         laneDividerPos={laneDividerPos}
         edgeDividerPos={edgeDividerPos}
       />
+      <TrafficSignals scene={scene} worldState={worldState} />
       <div
         style={{
           zIndex: "1",

envision/web/src/components/traffic_signals.js

@@ -0,0 +1,77 @@
+// Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+import {
+  Color3,
+  Color4,
+  MeshBuilder,
+  StandardMaterial,
+  Vector3,
+  Space,
+} from "@babylonjs/core";
+
+import { useEffect, useRef } from "react";
+import { SignalColors } from "../helpers/scene_colors";
+
+export default function TrafficSignals({ scene, worldState }) {
+  if (scene == null || worldState.signals == null) {
+    return null;
+  }
+
+  const signalGeometryRef = useRef([]);
+  const signalColorMap = Object.freeze({
+    0: SignalColors.Unknown,
+    1: SignalColors.Stop,
+    2: SignalColors.Caution,
+    3: SignalColors.Go,
+  });
+
+  useEffect(() => {
+    for (const geom of signalGeometryRef.current) {
+      // doNotRecurse = false, disposeMaterialAndTextures = true
+      geom.dispose(false, true);
+    }
+
+    let newSignalGeometry = Object.keys(worldState.signals).map(signalName => {
+      let state = worldState.signals[signalName].state;
+      let pos = worldState.signals[signalName].position;
+      let point = new Vector3(pos[0], 0.01, pos[1])
+      let mesh = MeshBuilder.CreateDisc(
+        `signal-${signalName}`,
+        { radius: 0.8 },
+        scene
+      );
+      mesh.position = point;
+      let axis = new Vector3(1, 0, 0);
+      mesh.rotate(axis, Math.PI / 2, Space.WORLD);
+
+      let color = signalColorMap[state];
+      let material = new StandardMaterial(`signal-${signalName}-material`, scene);
+      material.diffuseColor = new Color4(...color);
+      material.specularColor = new Color3(0, 0, 0);
+      mesh.material = material;
+      mesh.isVisible = true;
+      return mesh;
+    });
+
+    signalGeometryRef.current = newSignalGeometry;
+  }, [scene, worldState.signals]);
+  return null;
+}

envision/web/src/helpers/scene_colors.js

@@ -27,6 +27,7 @@ export const Yellow = [255 / 255, 190 / 255, 40 / 255, 1];
 export const GreenTransparent = [98 / 255, 178 / 255, 48 / 255, 0.3];
 export const Silver = [192 / 255, 192 / 255, 192 / 255, 1];
 export const Black = [0, 0, 0, 1];
+export const Green = [30 / 255, 210 / 255, 30 / 255, 1];
 
 export const DarkBlue = [5 / 255, 5 / 255, 70 / 255, 1];
 export const Blue = [0, 153 / 255, 1, 1];
@@ -59,3 +60,10 @@ export const SceneColors = Object.freeze({
   LaneDivider: OffWhite,
   EdgeDivider: Yellow,
 });
+
+export const SignalColors = Object.freeze({
+  Unknown: Grey,
+  Stop: Red,
+  Caution: Yellow,
+  Go: Green,
+})

envision/web/src/helpers/state_unpacker.js

@@ -55,6 +55,13 @@ const Traffic = Object.freeze({
   VEHICLE_TYPE: 13,
 });
 
+const TrafficSignal = Object.freeze({
+  SIGNAL_ID: 0,
+  STATE: 1,
+  POSITION_BEGIN: 2,
+  POSITION_END: 5,
+});
+
 const POINT_2D_LENGTH = 2;
 const POINT_3D_LENGTH = 3;
 
@@ -145,6 +152,19 @@ function unpack_traffic(traffic) {
   return mapped_traffic;
 }
 
+function unpack_signals(signals) {
+  let mapped_signals = Object.assign(
+    {},
+    ...signals.map((t) => ({
+      [t[TrafficSignal.SIGNAL_ID]]: {
+        state: t[TrafficSignal.STATE],
+        position: t.slice(TrafficSignal.POSITION_BEGIN, TrafficSignal.POSITION_END),
+      },
+    }))
+  );
+  return mapped_signals;
+}
+
 function get_attribute_map(unpacked_traffic, attr) {
   return Object.fromEntries(
     Object.entries(unpacked_traffic)
@@ -156,8 +176,10 @@ function get_attribute_map(unpacked_traffic, attr) {
 export default function unpack_worldstate(formatted_state) {
   let unpacked_bubbles = unpack_bubbles(formatted_state[WorldState.BUBBLES]);
   let unpacked_traffic = unpack_traffic(formatted_state[WorldState.TRAFFIC]);
+  let unpacked_signals = unpack_signals(formatted_state[WorldState.TRAFFIC_SIGNALS]);
   const worldstate = {
     traffic: unpacked_traffic,
+    signals: unpacked_signals,
     scenario_id: formatted_state[WorldState.SCENARIO_ID],
     scenario_name: formatted_state[WorldState.SCENARIO_NAME],
     bubbles: unpacked_bubbles,

smarts/core/smarts.py

@@ -202,8 +202,8 @@ def __init__(
         # from .utils.bullet import BulletClient
         # self._bullet_client = BulletClient(pybullet.GUI)
         self._bullet_client = bc.BulletClient(
-            pybullet.DIRECT  # pylint: disable=no-member
-        )
+            pybullet.DIRECT
+        )  # pylint: disable=no-member
 
         # Set up indices
         self._sensor_manager = SensorManager()
@@ -813,8 +813,8 @@ def provider_removing_actor(self, provider: Provider, actor_id: str):
     def _setup_bullet_client(self, client: bc.BulletClient):
         client.resetSimulation()
         client.configureDebugVisualizer(
-            pybullet.COV_ENABLE_GUI, 0  # pylint: disable=no-member
-        )
+            pybullet.COV_ENABLE_GUI, 0
+        )  # pylint: disable=no-member
         max_pybullet_freq = config()(
             "physics", "max_pybullet_freq", default=MAX_PYBULLET_FREQ, cast=int
         )
@@ -1537,14 +1537,16 @@ def _try_emit_envision_state(self, provider_state: ProviderState, obs, scores):
                 env_ss = envision_types.SignalLightState.Unknown
                 if v.state == SignalLightState.OFF:
                     env_ss = envision_types.SignalLightState.Off
-                elif v.state | SignalLightState.STOP:
+                elif v.state & SignalLightState.STOP:
                     env_ss = envision_types.SignalLightState.Stop
-                elif v.state | SignalLightState.CAUTION:
+                elif v.state & SignalLightState.CAUTION:
                     env_ss = envision_types.SignalLightState.Caution
-                elif v.state | SignalLightState.GO:
+                elif v.state & SignalLightState.GO:
                     env_ss = envision_types.SignalLightState.Go
                 # TODO: eventually do flashing and arrow states too
-                signals[v.actor_id] = envision_types.SignalState(v.actor_id, env_ss)
+                signals[v.actor_id] = envision_types.SignalState(
+                    v.actor_id, env_ss, tuple(v.stopping_pos.as_np_array)
+                )
                 continue
             if not isinstance(v, VehicleState):
                 continue