forked from casper-auto/carla-autoware
-
Notifications
You must be signed in to change notification settings - Fork 0
/
update_ros_bridge.patch
63 lines (59 loc) · 3.54 KB
/
update_ros_bridge.patch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py
index e3b0667..91bd78b 100755
--- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py
+++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py
@@ -112,7 +112,7 @@ class ActorFactory(object):
for actor_id in new_actors:
carla_actor = self.world.get_actor(actor_id)
if self.node.parameters["register_all_sensors"] or not isinstance(carla_actor, carla.Sensor):
- self._create_object_from_actor(carla_actor)
+ self._create_object_from_actor(carla_actor, None)
for actor_id in deleted_actors:
self._destroy_object(actor_id, delete_actor=False)
@@ -123,7 +123,8 @@ class ActorFactory(object):
task = self._task_queue.get()
if task[0] == ActorFactory.TaskType.SPAWN_ACTOR and not self.node.shutdown.is_set():
carla_actor = self.world.get_actor(task[1][0])
- self._create_object_from_actor(carla_actor)
+ relative_transform = task[1][1].transform
+ self._create_object_from_actor(carla_actor, relative_transform)
elif task[0] == ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR and not self.node.shutdown.is_set():
pseudo_object = task[1]
self._create_object(pseudo_object[0], pseudo_object[1].type, pseudo_object[1].id,
@@ -169,7 +170,7 @@ class ActorFactory(object):
self._task_queue.put((ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR, (id_, req)))
else:
id_ = self._spawn_carla_actor(req)
- self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, None)))
+ self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, req)))
self._known_actor_ids.append(id_)
return id_
@@ -222,26 +223,20 @@ class ActorFactory(object):
carla_actor = self.world.spawn_actor(blueprint, transform, attach_to)
return carla_actor.id
- def _create_object_from_actor(self, carla_actor):
+ def _create_object_from_actor(self, carla_actor, relative_transform):
"""
create a object for a given carla actor
Creates also the object for its parent, if not yet existing
"""
parent = None
# the transform relative to the map
- relative_transform = trans.carla_transform_to_ros_pose(carla_actor.get_transform())
+ if relative_transform == None:
+ relative_transform = trans.carla_transform_to_ros_pose(carla_actor.get_transform())
if carla_actor.parent:
if carla_actor.parent.id in self.actors:
parent = self.actors[carla_actor.parent.id]
else:
- parent = self._create_object_from_actor(carla_actor.parent)
- # calculate relative transform to the parent
- actor_transform_matrix = trans.ros_pose_to_transform_matrix(relative_transform)
- parent_transform_matrix = trans.ros_pose_to_transform_matrix(
- trans.carla_transform_to_ros_pose(carla_actor.parent.get_transform()))
- relative_transform_matrix = np.matrix(
- parent_transform_matrix).getI() * np.matrix(actor_transform_matrix)
- relative_transform = trans.transform_matrix_to_ros_pose(relative_transform_matrix)
+ parent = self._create_object_from_actor(carla_actor.parent, None)
parent_id = 0
if parent is not None: