Skip to content
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

added option to change pose of camera at runtime by publishing on topic #443

Closed
wants to merge 1 commit into from
Closed

added option to change pose of camera at runtime by publishing on topic #443

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
17 changes: 9 additions & 8 deletions carla_ros_bridge/src/carla_ros_bridge/actor_factory.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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))
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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,
Expand Down
6 changes: 5 additions & 1 deletion carla_ros_bridge/src/carla_ros_bridge/bridge.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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):
Expand Down
65 changes: 56 additions & 9 deletions carla_ros_bridge/src/carla_ros_bridge/camera.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -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

Expand Down Expand Up @@ -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,
Expand All @@ -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
Expand Down Expand Up @@ -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

Expand All @@ -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()

Expand Down Expand Up @@ -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

Expand All @@ -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()

Expand Down Expand Up @@ -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

Expand All @@ -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()

Expand Down Expand Up @@ -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

Expand All @@ -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() +
Expand Down
3 changes: 2 additions & 1 deletion carla_spawn_objects/config/objects.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down