def test_group(self):
tvo2 = pycrcc.TimeVariantCollisionObject(4)
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 0)) # time step 4
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 2)) # time step 5
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 3)) # time step 6
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 4)) # time step 7
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 5)) # time step 8
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 6)) # time step 9
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 7)) # time step 10
cc = pycrcc.CollisionChecker()
cc.add_collision_object(tvo2)
obb1 = pycrcc.RectOBB(2, 1, 1.5, 6.0, 0)
obb2 = pycrcc.RectOBB(2, 1, 1.5, 6.0, 20)
obb3 = pycrcc.RectOBB(2.1, 1, 1.5, 6.0, 20)
obb4 = pycrcc.RectOBB(2, 1, 1.5, 6.0, 0)
sg = pycrcc.ShapeGroup()
sg.add_shape(obb1)
sg.add_shape(obb2)
sg2 = pycrcc.ShapeGroup()
sg2.add_shape(obb3)
sg2.add_shape(obb4)
self.assertEqual(sg.collide(sg2), 1)
self.assertEqual(cc.collide(sg), 1)
def create_collision_object_shape_group(shape_group,
params=None,
collision_object_func=None):
sg = pycrcc.ShapeGroup()
for shape in shape_group.shapes:
sg.add_shape(
commonroad_cc.collision_detection.pycrcc_collision_dispatch.
create_collision_object(shape, params, collision_object_func))
return sg
def lane_rep_setup(lanelet_network):
lane_rep = []
for lane_polyline in lanelet_bounds.lane_hull(lanelet_network):
triangles = pycrcc.ShapeGroup()
triangle_builder.triangulate(
lanelet_bounds.polyline_edges(lane_polyline.tolist()), [],
None, triangles, {})
polygon = Polygon.Polygon(lane_polyline)
lane_rep.append((polygon, triangles))
return lane_rep
def lanelet_rep_setup(lanelets):
lanelet_rep = []
for lanelet in lanelets:
triangles = pycrcc.ShapeGroup()
triangle_builder.build_simple_triangles([lanelet], triangles)
polyline = np.concatenate(
(lanelet.left_vertices, np.flipud(lanelet.right_vertices)))
polygon = Polygon.Polygon(polyline)
lanelet_rep.append((polygon, triangles))
return lanelet_rep
def create_random_shape_group(self, shape_count=-1):
if shape_count == -1:
shape_count = np.random.choice(range(1, 50))
assert ((shape_count > 0) and (shape_count == int(shape_count)))
shapes = list()
for i in range(shape_count):
new_shape = self.create_random_shape()
shapes.append(new_shape)
sg = pycrcc.ShapeGroup()
for shape in shapes:
sg.add_shape(shape)
return sg
def create_collision_checker_scenario(scenario: Scenario,
params=None,
collision_object_func=None):
cc = pycrcc.CollisionChecker()
for co in scenario.dynamic_obstacles:
cc.add_collision_object(
commonroad_cc.collision_detection.pycrcc_collision_dispatch.
create_collision_object(co, params, collision_object_func))
shape_group = pycrcc.ShapeGroup()
for co in scenario.static_obstacles:
collision_object = commonroad_cc.collision_detection.pycrcc_collision_dispatch.create_collision_object(
co, params, collision_object_func)
if isinstance(collision_object, pycrcc.ShapeGroup):
for shape in collision_object.unpack():
shape_group.add_shape(shape)
else:
shape_group.add_shape(collision_object)
cc.add_collision_object(shape_group)
return cc
def init():
return pycrcc.ShapeGroup()
loopc = loopc + 1
if (loopc % 10 == 0):
print(loopc)
sleep(0.05)
if (loopc % 1000 == 0):
sleep(0.05)
cc1 = pycrcc.CollisionChecker()
objs = list()
for i in range(30):
obj = creat.create_random_object()
objs.append(obj)
for obj in objs:
cc1.add_collision_object(obj)
obj2 = creat.create_random_object()
sg_new = pycrcc.ShapeGroup()
sg_new.add_shape(obj_0)
cc1.collide(obj2)
obstacles = cc1.find_all_colliding_objects(obj2)
#snapshot = tracemalloc.take_snapshot()
#top_stats = snapshot.statistics('lineno')
#for stat in top_stats[:10]:
# print(stat)
#cc1.find_all_colliding_objects(obj2)
print("done")