Skip to content

Commit ed364e4

Browse files
jdsikajdsika
authored
Merge branch 'main' into dependabot/npm_and_yarn/npm_and_yarn-e04d5d616f
2 parents 1324c25 + 6873cfb commit ed364e4

4 files changed

Lines changed: 189 additions & 38 deletions

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 & 38 deletions
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
};
@@ -596,26 +617,6 @@ export function buildEgoVehicleRearAxleFrameTransform(
596617
};
597618
}
598619

599-
export function buildEgoVehicleRearAxisFrameTransform(
600-
osiGroundTruth: DeepRequired<GroundTruth>,
601-
): FrameTransform {
602-
const hostIdentifier = osiGroundTruth.host_vehicle_id.value;
603-
const hostObject = osiGroundTruth.moving_object.find((obj) => {
604-
return obj.id.value === hostIdentifier;
605-
})!;
606-
return {
607-
timestamp: osiTimestampToTime(osiGroundTruth.timestamp),
608-
parent_frame_id: "ego_vehicle_bb_center",
609-
child_frame_id: "ego_vehicle_rear_axis",
610-
translation: {
611-
x: hostObject.vehicle_attributes.bbcenter_to_rear.x,
612-
y: hostObject.vehicle_attributes.bbcenter_to_rear.y,
613-
z: hostObject.vehicle_attributes.bbcenter_to_rear.z,
614-
},
615-
rotation: eulerToQuaternion(0, 0, 0),
616-
};
617-
}
618-
619620
function buildSensorDataSceneEntities(
620621
osiSensorData: DeepRequired<SensorData>,
621622
): PartialSceneEntity[] {
@@ -1013,23 +1014,6 @@ export function activate(extensionContext: ExtensionContext): void {
10131014
"bbcenter_to_rear not found in ego vehicle attributes. Can not build rear axle FrameTransform.",
10141015
);
10151016
}
1016-
1017-
// Add rear axis FrameTransform if bbcenter_to_rear is set in vehicle attributes of ego vehicle
1018-
if (
1019-
message.moving_object.some(
1020-
(obj) =>
1021-
obj.id?.value === message.host_vehicle_id?.value &&
1022-
obj.vehicle_attributes?.bbcenter_to_rear,
1023-
)
1024-
) {
1025-
transforms.transforms.push(
1026-
buildEgoVehicleRearAxisFrameTransform(message as DeepRequired<GroundTruth>),
1027-
);
1028-
} else {
1029-
console.warn(
1030-
"bbcenter_to_rear not found in ego vehicle attributes. Can not build rear axis FrameTransform.",
1031-
);
1032-
}
10331017
} catch (error) {
10341018
console.error(
10351019
"Error during FrameTransform message conversion:\n%s\nSkipping message! (Input message not compatible?)",

‎src/lightstates/index.ts‎

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

0 commit comments

Comments
 (0)