Skip to content

Commit 4dd0b57

Browse files
buraktiryakirmessaou
buraktiryaki
and
rmessaou
authored
Visualize brake and indicator lights (#115)
* feat: add brake and indicator lights visualization * fix: fix review comments * fix: add example mcap file --------- Co-authored-by: Rayene Messaoud <48479151+rmessaou@users.noreply.github.com>
1 parent 8803754 commit 4dd0b57

4 files changed

Lines changed: 186 additions & 1 deletion

File tree

‎example-data/testBrakeAndIndicatorLights.mcap‎

112 KB
Binary file not shown.

‎src/config.ts‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ import {
1010
TrafficLight_Classification_Color,
1111
Lane_Classification_Type,
1212
RoadMarking_Classification_Color,
13+
MovingObject_VehicleClassification_LightState_BrakeLightState,
1314
} from "@lichtblick/asam-osi-types";
1415
import { ColorCode, ColorCodeName } from "@utils/helper";
1516

@@ -128,6 +129,42 @@ export const ROAD_MARKING_COLOR: Record<RoadMarking_Classification_Color, Color>
128129
[RoadMarking_Classification_Color.VIOLET]: { r: 0.9, g: 0.5, b: 0.9, a: 1 },
129130
};
130131

132+
export const BRAKE_LIGHT_COLOR: Record<
133+
MovingObject_VehicleClassification_LightState_BrakeLightState,
134+
Color
135+
> = {
136+
[MovingObject_VehicleClassification_LightState_BrakeLightState.OFF]: {
137+
r: 0.3,
138+
g: 0.0,
139+
b: 0.0,
140+
a: 0.7,
141+
},
142+
[MovingObject_VehicleClassification_LightState_BrakeLightState.OTHER]: {
143+
r: 0.3,
144+
g: 0.0,
145+
b: 0.0,
146+
a: 0.7,
147+
},
148+
[MovingObject_VehicleClassification_LightState_BrakeLightState.UNKNOWN]: {
149+
r: 0.3,
150+
g: 0.0,
151+
b: 0.0,
152+
a: 0.7,
153+
},
154+
[MovingObject_VehicleClassification_LightState_BrakeLightState.NORMAL]: {
155+
r: 0.7,
156+
g: 0.0,
157+
b: 0.0,
158+
a: 0.7,
159+
},
160+
[MovingObject_VehicleClassification_LightState_BrakeLightState.STRONG]: {
161+
r: 0.9,
162+
g: 0.0,
163+
b: 0.0,
164+
a: 0.7,
165+
},
166+
};
167+
131168
//// STATIONARY OBJECT MAPPING ////
132169

133170
export const STATIONARY_OBJECT_COLOR: Record<StationaryObject_Classification_Color, ColorCodeName> =

‎src/index.ts‎

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,12 @@ import {
5959
PREFIX_LANE,
6060
PREFIX_LANE_BOUNDARY,
6161
} from "./lanes";
62+
import {
63+
buildBrakeLight,
64+
BrakeLightSide,
65+
buildIndicatorLight,
66+
IndicatorLightSide,
67+
} from "./lightstates";
6268
import {
6369
buildLogicalLaneEntity,
6470
buildLogicalLaneBoundaryEntity,
@@ -148,13 +154,28 @@ function buildObjectEntity(
148154
];
149155
}
150156

157+
function buildVehicleLights() {
158+
if ("vehicle_classification" in osiObject) {
159+
return [
160+
buildBrakeLight(osiObject, BrakeLightSide.Left),
161+
buildBrakeLight(osiObject, BrakeLightSide.Right),
162+
buildIndicatorLight(osiObject, IndicatorLightSide.FrontLeft),
163+
buildIndicatorLight(osiObject, IndicatorLightSide.FrontRight),
164+
buildIndicatorLight(osiObject, IndicatorLightSide.RearLeft),
165+
buildIndicatorLight(osiObject, IndicatorLightSide.RearRight),
166+
];
167+
} else {
168+
return [];
169+
}
170+
}
171+
151172
return {
152173
timestamp: time,
153174
frame_id,
154175
id: generateSceneEntityId(id_prefix, osiObject.id.value),
155176
lifetime: { sec: 0, nsec: 0 },
156177
frame_locked: true,
157-
cubes: [cube],
178+
cubes: [cube, ...buildVehicleLights()],
158179
arrows: buildAxes(),
159180
metadata,
160181
};

‎src/lightstates/index.ts‎

Lines changed: 127 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,127 @@
1+
import { CubePrimitive, Vector3, type Color } from "@foxglove/schemas";
2+
import {
3+
MovingObject,
4+
MovingObject_VehicleClassification_LightState_IndicatorState,
5+
} from "@lichtblick/asam-osi-types";
6+
import { eulerToQuaternion, pointRotationByQuaternion } from "@utils/geometry";
7+
import { objectToCubePrimitive } from "@utils/marker";
8+
import { DeepRequired } from "ts-essentials";
9+
10+
import { BRAKE_LIGHT_COLOR } from "../config";
11+
12+
export enum BrakeLightSide {
13+
Left,
14+
Right,
15+
}
16+
17+
export enum IndicatorLightSide {
18+
FrontLeft,
19+
FrontRight,
20+
RearLeft,
21+
RearRight,
22+
}
23+
24+
const BRAKE_LIGHT_DIMENSIONS: Vector3 = { x: 0.5, y: 0.25, z: 0.25 };
25+
const BRAKE_LIGHT_POSITION_Y_OFFSET = 0.25;
26+
27+
const INDICATOR_ON_COLOR: Color = { r: 1.0, g: 0.8, b: 0.0, a: 0.7 };
28+
const INDICATOR_OFF_COLOR: Color = { r: 0.5, g: 0.5, b: 0.0, a: 0.7 };
29+
const INDICATOR_LIGHT_DIMENSIONS: Vector3 = { x: 0.25, y: 0.25, z: 0.25 };
30+
const INDICATOR_LIGHT_POSITION_Y_OFFSET = 0.125;
31+
const INDICATOR_LIGHT_POSITION_Z_OFFSET = 0.25;
32+
33+
export const buildBrakeLight = (
34+
moving_obj: DeepRequired<MovingObject>,
35+
side: BrakeLightSide,
36+
): CubePrimitive => {
37+
const brakeLightColor =
38+
BRAKE_LIGHT_COLOR[moving_obj.vehicle_classification.light_state.brake_light_state];
39+
40+
const directionMultiplier = side === BrakeLightSide.Left ? 1 : -1;
41+
const localAxisOffset: Vector3 = {
42+
x: -(moving_obj.base.dimension.length / 2),
43+
y:
44+
directionMultiplier * (moving_obj.base.dimension.width / 2) -
45+
BRAKE_LIGHT_POSITION_Y_OFFSET * directionMultiplier,
46+
z: 0.0,
47+
};
48+
const baseOrientation = eulerToQuaternion(
49+
moving_obj.base.orientation.roll,
50+
moving_obj.base.orientation.pitch,
51+
moving_obj.base.orientation.yaw,
52+
);
53+
const globalOffset = pointRotationByQuaternion(localAxisOffset, baseOrientation);
54+
55+
return objectToCubePrimitive(
56+
moving_obj.base.position.x + globalOffset.x,
57+
moving_obj.base.position.y + globalOffset.y,
58+
moving_obj.base.position.z + globalOffset.z,
59+
moving_obj.base.orientation.roll,
60+
moving_obj.base.orientation.pitch,
61+
moving_obj.base.orientation.yaw,
62+
BRAKE_LIGHT_DIMENSIONS.x,
63+
BRAKE_LIGHT_DIMENSIONS.y,
64+
BRAKE_LIGHT_DIMENSIONS.z,
65+
brakeLightColor,
66+
);
67+
};
68+
69+
export const buildIndicatorLight = (
70+
moving_obj: DeepRequired<MovingObject>,
71+
side: IndicatorLightSide,
72+
): CubePrimitive => {
73+
let lightOn = false;
74+
switch (moving_obj.vehicle_classification.light_state.indicator_state) {
75+
case MovingObject_VehicleClassification_LightState_IndicatorState.LEFT:
76+
if (side === IndicatorLightSide.FrontLeft || side === IndicatorLightSide.RearLeft) {
77+
lightOn = true;
78+
} else {
79+
lightOn = false;
80+
}
81+
break;
82+
case MovingObject_VehicleClassification_LightState_IndicatorState.RIGHT:
83+
if (side === IndicatorLightSide.FrontRight || side === IndicatorLightSide.RearRight) {
84+
lightOn = true;
85+
} else {
86+
lightOn = false;
87+
}
88+
break;
89+
case MovingObject_VehicleClassification_LightState_IndicatorState.WARNING:
90+
lightOn = true;
91+
break;
92+
default:
93+
lightOn = false;
94+
break;
95+
}
96+
97+
const localAxisOffset: Vector3 = {
98+
x:
99+
side === IndicatorLightSide.FrontLeft || side === IndicatorLightSide.FrontRight
100+
? moving_obj.base.dimension.length / 2
101+
: -(moving_obj.base.dimension.length / 2),
102+
y:
103+
side === IndicatorLightSide.FrontLeft || side === IndicatorLightSide.RearLeft
104+
? moving_obj.base.dimension.width / 2 - INDICATOR_LIGHT_POSITION_Y_OFFSET
105+
: -moving_obj.base.dimension.width / 2 + INDICATOR_LIGHT_POSITION_Y_OFFSET,
106+
z: INDICATOR_LIGHT_POSITION_Z_OFFSET,
107+
};
108+
const baseOrientation = eulerToQuaternion(
109+
moving_obj.base.orientation.roll,
110+
moving_obj.base.orientation.pitch,
111+
moving_obj.base.orientation.yaw,
112+
);
113+
const globalOffset = pointRotationByQuaternion(localAxisOffset, baseOrientation);
114+
115+
return objectToCubePrimitive(
116+
moving_obj.base.position.x + globalOffset.x,
117+
moving_obj.base.position.y + globalOffset.y,
118+
moving_obj.base.position.z + globalOffset.z,
119+
moving_obj.base.orientation.roll,
120+
moving_obj.base.orientation.pitch,
121+
moving_obj.base.orientation.yaw,
122+
INDICATOR_LIGHT_DIMENSIONS.x,
123+
INDICATOR_LIGHT_DIMENSIONS.y,
124+
INDICATOR_LIGHT_DIMENSIONS.z,
125+
lightOn ? INDICATOR_ON_COLOR : INDICATOR_OFF_COLOR,
126+
);
127+
};

0 commit comments

Comments
 (0)