Skip to content

Commit 2f6c6d1

Browse files
buraktiryakiburaktiryaki
committed
feat: add brake and indicator lights visualization
1 parent 2ebbcf8 commit 2f6c6d1

3 files changed

Lines changed: 178 additions & 1 deletion

File tree

‎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: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
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 INDICATOR_ON_COLOR: Color = { r: 1.0, g: 0.8, b: 0.0, a: 0.7 };
25+
const INDICATOR_OFF_COLOR: Color = { r: 0.5, g: 0.5, b: 0.0, a: 0.7 };
26+
27+
export const buildBrakeLight = (
28+
moving_obj: DeepRequired<MovingObject>,
29+
side: BrakeLightSide,
30+
): CubePrimitive => {
31+
const brakelightcolor =
32+
BRAKE_LIGHT_COLOR[moving_obj.vehicle_classification.light_state.brake_light_state];
33+
34+
const directionMultiplier = side === BrakeLightSide.Left ? 1 : -1;
35+
const localAxisOffset: Vector3 = {
36+
x: -(moving_obj.base.dimension.length / 2),
37+
y: directionMultiplier * (moving_obj.base.dimension.width / 2) - 0.25 * directionMultiplier,
38+
z: 0.0,
39+
};
40+
const baseOrientation = eulerToQuaternion(
41+
moving_obj.base.orientation.roll,
42+
moving_obj.base.orientation.pitch,
43+
moving_obj.base.orientation.yaw,
44+
);
45+
const globalOffset = pointRotationByQuaternion(localAxisOffset, baseOrientation);
46+
47+
return objectToCubePrimitive(
48+
moving_obj.base.position.x + globalOffset.x,
49+
moving_obj.base.position.y + globalOffset.y,
50+
moving_obj.base.position.z + globalOffset.z,
51+
moving_obj.base.orientation.roll,
52+
moving_obj.base.orientation.pitch,
53+
moving_obj.base.orientation.yaw,
54+
0.5,
55+
0.25,
56+
0.25,
57+
brakelightcolor,
58+
);
59+
};
60+
61+
export const buildIndicatorLight = (
62+
moving_obj: DeepRequired<MovingObject>,
63+
side: IndicatorLightSide,
64+
): CubePrimitive => {
65+
let lightOn = false;
66+
switch (moving_obj.vehicle_classification.light_state.indicator_state) {
67+
case MovingObject_VehicleClassification_LightState_IndicatorState.LEFT:
68+
if (side === IndicatorLightSide.FrontLeft || side === IndicatorLightSide.RearLeft) {
69+
lightOn = true;
70+
} else {
71+
lightOn = false;
72+
}
73+
break;
74+
case MovingObject_VehicleClassification_LightState_IndicatorState.RIGHT:
75+
if (side === IndicatorLightSide.FrontRight || side === IndicatorLightSide.RearRight) {
76+
lightOn = true;
77+
} else {
78+
lightOn = false;
79+
}
80+
break;
81+
case MovingObject_VehicleClassification_LightState_IndicatorState.WARNING:
82+
lightOn = true;
83+
break;
84+
default:
85+
lightOn = false;
86+
break;
87+
}
88+
89+
const localAxisOffset: Vector3 = {
90+
x:
91+
side === IndicatorLightSide.FrontLeft || side === IndicatorLightSide.FrontRight
92+
? moving_obj.base.dimension.length / 2
93+
: -(moving_obj.base.dimension.length / 2),
94+
y:
95+
side === IndicatorLightSide.FrontLeft || side === IndicatorLightSide.RearLeft
96+
? moving_obj.base.dimension.width / 2 - 0.125
97+
: -moving_obj.base.dimension.width / 2 + 0.125,
98+
z: 0.25,
99+
};
100+
const baseOrientation = eulerToQuaternion(
101+
moving_obj.base.orientation.roll,
102+
moving_obj.base.orientation.pitch,
103+
moving_obj.base.orientation.yaw,
104+
);
105+
const globalOffset = pointRotationByQuaternion(localAxisOffset, baseOrientation);
106+
107+
return objectToCubePrimitive(
108+
moving_obj.base.position.x + globalOffset.x,
109+
moving_obj.base.position.y + globalOffset.y,
110+
moving_obj.base.position.z + globalOffset.z,
111+
moving_obj.base.orientation.roll,
112+
moving_obj.base.orientation.pitch,
113+
moving_obj.base.orientation.yaw,
114+
0.25,
115+
0.25,
116+
0.25,
117+
lightOn ? INDICATOR_ON_COLOR : INDICATOR_OFF_COLOR,
118+
);
119+
};

0 commit comments

Comments
 (0)