From a8848acbe74178e03936a42c899b839614b0b687 Mon Sep 17 00:00:00 2001 From: jmagnin Date: Fri, 18 Dec 2020 13:49:45 -0500 Subject: [PATCH] added option to change pose of camera at runtime by publishing on topic --- .../src/carla_ros_bridge/actor_factory.py | 17 ++--- .../src/carla_ros_bridge/bridge.py | 6 +- .../src/carla_ros_bridge/camera.py | 65 ++++++++++++++++--- carla_spawn_objects/config/objects.json | 3 +- 4 files changed, 72 insertions(+), 19 deletions(-) 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 59e1a2bb..23f9436b 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py @@ -121,7 +121,7 @@ def _create_carla_actor(self, carla_actor): name = str(carla_actor.id) return self.create(carla_actor.type_id, name, parent_id, None, carla_actor) - def create(self, type_id, name, attach_to, spawn_pose, carla_actor=None): + def create(self, type_id, name, attach_to, spawn_pose, carla_actor=None, allow_transform=False): with self.lock: # check that the actor is not already created. if carla_actor is not None and carla_actor.id in self.actors: @@ -139,7 +139,8 @@ def create(self, type_id, name, attach_to, spawn_pose, carla_actor=None): else: uid = next(self.id_gen) - actor = self._create_object(uid, type_id, name, parent, spawn_pose, carla_actor) + actor = self._create_object(uid, type_id, name, parent, + spawn_pose, carla_actor, allow_transform=allow_transform) self.actors[actor.uid] = actor rospy.loginfo("Created {}(id={})".format(actor.__class__.__name__, actor.uid)) @@ -169,7 +170,7 @@ def get_pseudo_sensor_types(self): pseudo_sensors.append(cls.get_blueprint_name()) return pseudo_sensors - def _create_object(self, uid, type_id, name, parent, spawn_pose, carla_actor=None): + def _create_object(self, uid, type_id, name, parent, spawn_pose, carla_actor=None, allow_transform=False): if type_id == TFSensor.get_blueprint_name(): actor = TFSensor(uid=uid, name=name, parent=parent, node=self.node) @@ -248,22 +249,22 @@ def _create_object(self, uid, type_id, name, parent, spawn_pose, carla_actor=Non if carla_actor.type_id.startswith("sensor.camera"): if carla_actor.type_id.startswith("sensor.camera.rgb"): actor = RgbCamera(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) + carla_actor, self.sync_mode, allow_transform=allow_transform) elif carla_actor.type_id.startswith("sensor.camera.depth"): actor = DepthCamera(uid, name, parent, spawn_pose, - self.node, carla_actor, self.sync_mode) + self.node, carla_actor, self.sync_mode, allow_transform=allow_transform) elif carla_actor.type_id.startswith( "sensor.camera.semantic_segmentation"): actor = SemanticSegmentationCamera(uid, name, parent, spawn_pose, self.node, carla_actor, - self.sync_mode) + self.sync_mode, allow_transform=allow_transform) elif carla_actor.type_id.startswith("sensor.camera.dvs"): actor = DVSCamera(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) + carla_actor, self.sync_mode, allow_transform=allow_transform) else: actor = Camera(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) + carla_actor, self.sync_mode, allow_transform=allow_transform) elif carla_actor.type_id.startswith("sensor.lidar"): if carla_actor.type_id.endswith("sensor.lidar.ray_cast"): actor = Lidar(uid, name, parent, spawn_pose, self.node, diff --git a/carla_ros_bridge/src/carla_ros_bridge/bridge.py b/carla_ros_bridge/src/carla_ros_bridge/bridge.py index ac49929c..b20f0cdf 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/bridge.py +++ b/carla_ros_bridge/src/carla_ros_bridge/bridge.py @@ -153,7 +153,11 @@ def _spawn_actor(self, req): else: blueprint = self.carla_world.get_blueprint_library().find(req.type) blueprint.set_attribute('role_name', req.id) + allow_transform = False for attribute in req.attributes: + if attribute.key == "allow_transform": + allow_transform = attribute.value + continue blueprint.set_attribute(attribute.key, attribute.value) if req.random_pose is False: transform = trans.ros_pose_to_carla_transform(req.transform) @@ -171,7 +175,7 @@ def _spawn_actor(self, req): carla_actor = self.carla_world.spawn_actor(blueprint, transform, attach_to) actor = self.actor_factory.create( - req.type, req.id, req.attach_to, req.transform, carla_actor) + req.type, req.id, req.attach_to, req.transform, carla_actor, allow_transform=allow_transform) return actor.uid def _spawn_pseudo_actor(self, req): diff --git a/carla_ros_bridge/src/carla_ros_bridge/camera.py b/carla_ros_bridge/src/carla_ros_bridge/camera.py index 9affa6e0..e560db1f 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/camera.py +++ b/carla_ros_bridge/src/carla_ros_bridge/camera.py @@ -19,10 +19,13 @@ from cv_bridge import CvBridge from sensor_msgs.point_cloud2 import create_cloud from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField +from geometry_msgs.msg import PoseStamped import carla from carla_ros_bridge.sensor import Sensor import carla_common.transforms as trans +from math import degrees +from tf.transformations import euler_from_quaternion class Camera(Sensor): @@ -34,7 +37,7 @@ class Camera(Sensor): # global cv bridge to convert image between opencv and ros cv_bridge = CvBridge() - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): # pylint: disable=too-many-arguments + def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode, allow_transform=False): # pylint: disable=too-many-arguments """ Constructor @@ -69,6 +72,8 @@ def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, sy else: self._build_camera_info() + self.carla_actor = carla_actor + self.camera_info_publisher = rospy.Publisher(self.get_topic_prefix() + '/camera_info', CameraInfo, @@ -78,6 +83,44 @@ def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, sy Image, queue_size=10) + if allow_transform: + rospy.Subscriber(self.get_topic_prefix() + '/' + 'update_pose', + PoseStamped, self.new_pose_received) + + def new_pose_received(self, pose): + """ + Move the sensor + """ + if self.relative_spawn_pose != pose.pose: + rospy.logdebug("Camera pose changed. Updating carla camera") + self.relative_spawn_pose = pose.pose + transform = self.get_sensor_carla_transform() + if transform and self.carla_actor: + self.carla_actor.set_transform(transform) + + def get_sensor_carla_transform(self): + """ + Calculate the CARLA sensor transform + """ + if not self.relative_spawn_pose: + rospy.loginfo("no pose!") + return None + sensor_location = carla.Location(x=self.relative_spawn_pose.position.x, + y=-self.relative_spawn_pose.position.y, + z=self.relative_spawn_pose.position.z) + quaternion = ( + self.relative_spawn_pose.orientation.x, + self.relative_spawn_pose.orientation.y, + self.relative_spawn_pose.orientation.z, + self.relative_spawn_pose.orientation.w + ) + roll, pitch, yaw = euler_from_quaternion(quaternion) + # rotate to CARLA + sensor_rotation = carla.Rotation(pitch=degrees(roll)-90, + roll=degrees(pitch), + yaw=-degrees(yaw)-90) + return carla.Transform(sensor_location, sensor_rotation) + def _build_camera_info(self): """ Private function to compute camera info @@ -174,7 +217,7 @@ class RgbCamera(Camera): Camera implementation details for rgb camera """ - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): + def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode, allow_transform=False): """ Constructor @@ -199,7 +242,8 @@ def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, sy relative_spawn_pose=relative_spawn_pose, node=node, carla_actor=carla_actor, - synchronous_mode=synchronous_mode) + synchronous_mode=synchronous_mode, + allow_transform=allow_transform) self.listen() @@ -229,7 +273,7 @@ class DepthCamera(Camera): Camera implementation details for depth camera """ - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): + def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode, allow_transform=False): """ Constructor @@ -254,7 +298,8 @@ def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, sy relative_spawn_pose=relative_spawn_pose, node=node, carla_actor=carla_actor, - synchronous_mode=synchronous_mode) + synchronous_mode=synchronous_mode, + allow_transform=allow_transform) self.listen() @@ -306,7 +351,7 @@ class SemanticSegmentationCamera(Camera): Camera implementation details for segmentation camera """ - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): + def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode, allow_transform=False): """ Constructor @@ -332,7 +377,8 @@ def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, sy relative_spawn_pose=relative_spawn_pose, node=node, synchronous_mode=synchronous_mode, - carla_actor=carla_actor) + carla_actor=carla_actor, + allow_transform=allow_transform) self.listen() @@ -363,7 +409,7 @@ class DVSCamera(Camera): Sensor implementation details for dvs cameras """ - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): # pylint: disable=too-many-arguments + def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode, allow_transform=False): # pylint: disable=too-many-arguments """ Constructor @@ -388,7 +434,8 @@ def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, sy relative_spawn_pose=relative_spawn_pose, node=node, carla_actor=carla_actor, - synchronous_mode=synchronous_mode) + synchronous_mode=synchronous_mode, + allow_transform=allow_transform) self._dvs_events = None self.dvs_camera_publisher = rospy.Publisher(self.get_topic_prefix() + diff --git a/carla_spawn_objects/config/objects.json b/carla_spawn_objects/config/objects.json index 717a7981..a2507d14 100644 --- a/carla_spawn_objects/config/objects.json +++ b/carla_spawn_objects/config/objects.json @@ -42,7 +42,8 @@ "image_size_x": 800, "image_size_y": 600, "fov": 90.0, - "sensor_tick": 0.05 + "sensor_tick": 0.05, + "allow_transform": true }, { "type": "sensor.lidar.ray_cast",