Skip to content

Add selective collision detection for different agent types #404

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
daphne-cornelisse wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Add selective collision detection for different agent types #404

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d2c4848
Update collision detection rules
daphne-cornelisse Apr 4, 2025
4b5aa1f
Register collisions pedestrians <> other agents
daphne-cornelisse Apr 4, 2025
e7e100a
filter pedestrian collisions at 1st timestep
nadarenator Apr 8, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 8 additions & 6 deletions data_utils/detect_3d.ipynb
View file
Open in desktop

Large diffs are not rendered by default.

85 changes: 46 additions & 39 deletions data_utils/process_waymo_files.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -400,16 +400,17 @@ def waymo_to_scenario(
# Create collision managers
road_collision_manager = trimesh.collision.CollisionManager()
road_collision_manager.add_object("road_edges", edge_mesh)
agent_collision_manager = trimesh.collision.CollisionManager()
agent_collision_manager = trimesh.collision.CollisionManager() # All agents
trajectory_collision_manager = trimesh.collision.CollisionManager()

# Non-pedestrian collision managers for road edge collisions
non_ped_agent_collision_manager = trimesh.collision.CollisionManager()

# Construct object states
objects = []
for track in protobuf.tracks:
obj = _init_object(track)
if obj is not None:
if obj["type"] not in ["vehicle", "cyclist"]:
if obj["type"] not in ["vehicle", "cyclist", "pedestrian"]:
obj["mark_as_expert"] = False
objects.append(obj)
continue
Expand All @@ -431,49 +432,55 @@ def waymo_to_scenario(
obj["width"],
obj["height"]
)

# Add to general agent collision manager
agent_collision_manager.add_object(str(obj["id"]), initial_box)

# Create trajectory mesh
if False in obj["valid"]:
# Create trajectory segments of only valid positions
trajectory_segments = []
for i in range(len(obj["position"]) - 1):
if obj["valid"][i] and obj["valid"][i + 1]:
trajectory_segments.append(
[
# Add to non-pedestrian collision manager if not a pedestrian
if obj["type"] != "pedestrian":
non_ped_agent_collision_manager.add_object(str(obj["id"]), initial_box)

# Create trajectory mesh for non-pedestrians
if False in obj["valid"]:
# Create trajectory segments of only valid positions
trajectory_segments = []
for i in range(len(obj["position"]) - 1):
if obj["valid"][i] and obj["valid"][i + 1]:
trajectory_segments.append(
[
obj["position"][i]["x"],
obj["position"][i]["y"],
obj["position"][i]["z"],
],
[
obj["position"][i + 1]["x"],
obj["position"][i + 1]["y"],
obj["position"][i + 1]["z"],
],
]
)
else:
obj_vertices = [
[pos["x"], pos["y"], pos["z"]] for pos in obj["position"]
]
trajectory_segments = [
[obj_vertices[i], obj_vertices[i + 1]]
for i in range(len(obj_vertices) - 1)
]

trajectory_segments = _filter_small_segments(trajectory_segments)
if len(trajectory_segments) > 0:
trajectory_mesh = _generate_mesh(trajectory_segments)
trajectory_collision_manager.add_object(str(obj["id"]), trajectory_mesh)

[
obj["position"][i]["x"],
obj["position"][i]["y"],
obj["position"][i]["z"],
],
[
obj["position"][i + 1]["x"],
obj["position"][i + 1]["y"],
obj["position"][i + 1]["z"],
],
]
)
else:
obj_vertices = [
[pos["x"], pos["y"], pos["z"]] for pos in obj["position"]
]
trajectory_segments = [
[obj_vertices[i], obj_vertices[i + 1]]
for i in range(len(obj_vertices) - 1)
]

trajectory_segments = _filter_small_segments(trajectory_segments)
if len(trajectory_segments) > 0:
trajectory_mesh = _generate_mesh(trajectory_segments)
trajectory_collision_manager.add_object(str(obj["id"]), trajectory_mesh)

objects.append(obj)

# Check collisions between all init agent positions
_, agent_collision_pairs = agent_collision_manager.in_collision_internal(return_names=True)

# Check collisions between init agent positions and road edges
_, road_collision_pairs = agent_collision_manager.in_collision_other(
# Check collisions between init agent positions and road edges (vehicles, cyclists)
_, road_collision_pairs = non_ped_agent_collision_manager.in_collision_other(
road_collision_manager, return_names=True
)

Expand All @@ -500,7 +507,7 @@ def waymo_to_scenario(

# Update mark_as_expert based on initial collisions
for index, obj in enumerate(objects):
if obj["type"] in ["vehicle", "cyclist"]:
if obj["type"] in ["vehicle", "cyclist", "pedestrian"]:
if str(obj["id"]) in colliding_agents:
objects[index]["mark_as_expert"] = True
else:
Expand Down
78 changes: 51 additions & 27 deletions src/sim.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -619,7 +619,7 @@ inline void doneSystem(Engine &ctx,
}

void collisionDetectionSystem(Engine &ctx,
const CandidateCollision &candidateCollision) {
const CandidateCollision &candidateCollision) {

auto isInvalidExpertOrDone = [&](const Loc &candidate) -> bool
{
Expand Down Expand Up @@ -656,7 +656,6 @@ void collisionDetectionSystem(Engine &ctx,

if (isInvalidExpertOrDone(candidateCollision.a) ||
isInvalidExpertOrDone(candidateCollision.b)) {

return;
}

Expand Down Expand Up @@ -689,54 +688,79 @@ void collisionDetectionSystem(Engine &ctx,
EntityType aEntityType = ctx.get<EntityType>(candidateCollision.a);
EntityType bEntityType = ctx.get<EntityType>(candidateCollision.b);

// Check for existing collision pairs to ignore
for(auto &pair : ctx.data().collisionPairs)
{
if((pair.first == aEntityType && pair.second == bEntityType) ||
(pair.first == bEntityType && pair.second == aEntityType))
(pair.first == bEntityType && pair.second == aEntityType))
{
return;
}
}

auto maybeCollisionDetectionEventA =
ctx.getCheck<CollisionDetectionEvent>(candidateCollision.a);
if (maybeCollisionDetectionEventA.valid()) {
// Implement the specific collision logic
// Use existing logic for road element detection:
// Road elements have EntityType > None && EntityType <= StopSign

// Check collisions based on entity types
bool shouldRegisterCollisionA = false;
bool shouldRegisterCollisionB = false;

// Process collisions for Entity A
if (aEntityType == EntityType::Vehicle || aEntityType == EntityType::Cyclist) {
// Vehicles and Cyclists always register collisions
shouldRegisterCollisionA = true;
} else if (aEntityType == EntityType::Pedestrian) {
// Pedestrians don't register collisions with road elements but can collide with other agents
if (!(bEntityType > EntityType::None && bEntityType <= EntityType::StopSign)) {
shouldRegisterCollisionA = true;
}
} else {
// Non-agent entities (like Road elements) don't need special handling
shouldRegisterCollisionA = true;
}

// Process collisions for Entity B
if (bEntityType == EntityType::Vehicle || bEntityType == EntityType::Cyclist) {
// Vehicles and Cyclists always register collisions
shouldRegisterCollisionB = true;
} else if (bEntityType == EntityType::Pedestrian) {
// Pedestrians don't register collisions with road elements but can collide with other agents
if (!(aEntityType > EntityType::None && aEntityType <= EntityType::StopSign)) {
shouldRegisterCollisionB = true;
}
} else {
// Non-agent entities (like Road elements) don't need special handling
shouldRegisterCollisionB = true;
}

// Register collision for entity A if applicable
auto maybeCollisionDetectionEventA = ctx.getCheck<CollisionDetectionEvent>(candidateCollision.a);
if (maybeCollisionDetectionEventA.valid() && shouldRegisterCollisionA) {
maybeCollisionDetectionEventA.value().hasCollided.store_relaxed(1);
auto agent_iface = ctx.get<AgentInterfaceEntity>(candidateCollision.a).e;
if(bEntityType > EntityType::None && bEntityType <= EntityType::StopSign)
{
if (bEntityType > EntityType::None && bEntityType <= EntityType::StopSign) {
ctx.get<Info>(agent_iface).collidedWithRoad = 1;
}
else if(bEntityType == EntityType::Vehicle)
{
} else if (bEntityType == EntityType::Vehicle) {
ctx.get<Info>(agent_iface).collidedWithVehicle = 1;
}
else if(bEntityType <= EntityType::Cyclist)
{
} else if (bEntityType == EntityType::Pedestrian || bEntityType == EntityType::Cyclist) {
ctx.get<Info>(agent_iface).collidedWithNonVehicle = 1;
}
}

auto maybeCollisionDetectionEventB =
ctx.getCheck<CollisionDetectionEvent>(candidateCollision.b);
if (maybeCollisionDetectionEventB.valid()) {
// Register collision for entity B if applicable
auto maybeCollisionDetectionEventB = ctx.getCheck<CollisionDetectionEvent>(candidateCollision.b);
if (maybeCollisionDetectionEventB.valid() && shouldRegisterCollisionB) {
maybeCollisionDetectionEventB.value().hasCollided.store_relaxed(1);
auto agent_iface = ctx.get<AgentInterfaceEntity>(candidateCollision.b).e;
if(aEntityType > EntityType::None && aEntityType <= EntityType::StopSign)
{
if (aEntityType > EntityType::None && aEntityType <= EntityType::StopSign) {
ctx.get<Info>(agent_iface).collidedWithRoad = 1;
}
else if(aEntityType == EntityType::Vehicle)
{
} else if (aEntityType == EntityType::Vehicle) {
ctx.get<Info>(agent_iface).collidedWithVehicle = 1;
}
else if(aEntityType <= EntityType::Cyclist)
{
} else if (aEntityType == EntityType::Pedestrian || aEntityType == EntityType::Cyclist) {
ctx.get<Info>(agent_iface).collidedWithNonVehicle = 1;
}
}


}

// Helper function for sorting nodes in the taskgraph.
Expand Down