def __init__(self, carla_actor, parent, communication, synchronous_mode, prefix=None): # pylint: disable=too-many-arguments
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param prefix: the topic prefix to be used for this actor
:type prefix: string
"""
if not prefix:
prefix = 'camera'
super(Camera, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
prefix=prefix)
#if self.__class__.__name__ == "Camera":
# rospy.logwarn("Created Unsupported Camera Actor"
# "(id={}, parent_id={}, type={}, attributes={})".format(
# self.get_id(), self.get_parent_id(),
# self.carla_actor.type_id, self.carla_actor.attributes))
#else:
self.pub1=Publisher(self.get_topic_prefix() + '/camera_info')
self.pub2=Publisher(self.get_topic_prefix() + '/' + self.get_image_topic_name())
self.cv_bridge = CvBridge()
self._build_camera_info()
def __init__(self, carla_world, setstart, setgoal):
self.world = carla_world
#only for circles
self.map = carla_world.get_map()
self.ego_vehicle = None
self.role_name = 'ego_vehicle'
self.waypoint_publisher = Publisher(
'carla_ego_vehicle_waypoints'.format(self.role_name))
# set initial goal
self.start = setstart
self.goal = setgoal
self.current_route = None
self.goal_subscriber = Subscriber("/carla/{}/goal".format(
self.role_name))
self._pose_subscriber = Subscriber(
"ego_pose_localization"
) #, RigidBodyStateStamped, self.pose_callback
self._pose = RigidBodyStateStamped()
self.pose_ = PoseStamped()
self._update_lock = threading.Lock()
self.update_loop_thread = Thread(target=self.loop)
self.update_loop_thread.start()
# use callback to wait for ego vehicle
self.world.on_tick(self.find_ego_vehicle_actor)
class ObjectSensor(PseudoActor):
"""
Pseudo object sensor
"""
def __init__(self, parent, communication, actor_list, filtered_id):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param actor_list: current list of actors
:type actor_list: map(carla-actor-id -> python-actor-object)
:param filtered_id: id to filter from actor_list
:type filtered_id: int
"""
super(ObjectSensor, self).__init__(parent=parent,
communication=communication,
prefix='objects')
self.actor_list = actor_list
self.filtered_id = filtered_id
self.pub1 = Publisher(self.get_topic_prefix())
def destroy(self):
"""
Function to destroy this object.
:return:
"""
self.actor_list = None
super(ObjectSensor, self).destroy()
def update(self, frame, timestamp):
"""
Function (override) to update this object.
On update map sends:
- tf global frame
:return:
"""
icv_objects = ObjectArray(header=self.get_msg_header("map"))
for actor_id in self.actor_list.keys():
# currently only Vehicles and Walkers are added to the object array
if self.filtered_id != actor_id:
actor = self.actor_list[actor_id]
if isinstance(actor, Vehicle):
icv_objects.objects.append(actor.get_object_info())
elif isinstance(actor, Walker):
icv_objects.objects.append(actor.get_object_info())
self.pub1.publish(icv_objects)
def __init__(self):
"""
Constructor
"""
self.tf_to_publish = []
self.msgs_to_publish = []
self.timestamp = Time()
#icvTF=TFMessage()
# needed?
self.pubtf = Publisher("tf")
self.pubmarker = Publisher("/carla/marker")
self.pubclock = Publisher("clock")
self.publist = Publisher("/carla/actor_list")
def __init__(self, carla_actor, parent, communication, synchronous_mode):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
"""
super(Lidar, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
prefix='lidar/' + carla_actor.attributes.get('role_name'))
self.pub1=Publisher(self.get_topic_prefix()+ "/point_cloud")
def __init__(self, carla_actor, parent, communication,
vehicle_control_applied_callback):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
"""
super(EgoVehicle,
self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
prefix=carla_actor.attributes.get('role_name'))
self.vehicle_info_published = False
self.vehicle_control_override = False
self._vehicle_control_applied_callback = vehicle_control_applied_callback
self.sub1 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_cmd")
self.sub2 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_cmd_manual")
self.sub3 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_manual_override")
self.sub4 = Subscriber(self.get_topic_prefix() + "/enable_autopilot")
self.sub5 = Subscriber(self.get_topic_prefix() + "/twist_cmd")
self.Sec_loop = 0.02
self.control_subscriber = CarlaEgoVehicleControl()
self.manual_control_subscriber = CarlaEgoVehicleControl()
self.control_override_subscriber = Bool()
self.enable_autopilot_subscriber = Bool()
self.twist_control_subscriber = Twist()
self.pub1 = Publisher(self.get_topic_prefix() + "/vehicle_info")
self.pub2 = Publisher(self.get_topic_prefix() + "/vehicle_status")
self.update_command_thread = Thread(
target=self._update_commands_thread)
self.update_command_thread.start()
class CollisionSensor(Sensor):
"""
Actor implementation details for a collision sensor
"""
def __init__(self, carla_actor, parent, communication, synchronous_mode):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param synchronous_mode: use in synchronous mode?
:type synchronous_mode: bool
"""
super(CollisionSensor,
self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
is_event_sensor=True,
prefix="collision")
self.pub1 = Publisher(self.get_topic_prefix())
# pylint: disable=arguments-differ
def sensor_data_updated(self, collision_event):
"""
Function to wrap the collision event into a icv messsage
:param collision_event: carla collision event object
:type collision_event: carla.CollisionEvent
"""
collision_msg = CarlaCollisionEvent()
collision_msg.header = self.get_msg_header()
collision_msg.other_actor_id = collision_event.other_actor.id
collision_msg.normal_impulse.x = collision_event.normal_impulse.x
collision_msg.normal_impulse.y = collision_event.normal_impulse.y
collision_msg.normal_impulse.z = collision_event.normal_impulse.z
self.pub1.publish(collision_msg)
class Communication(object):
"""
Handle communication of icv topics
"""
def __init__(self):
"""
Constructor
"""
self.tf_to_publish = []
self.msgs_to_publish = []
self.timestamp = Time()
#icvTF=TFMessage()
# needed?
self.pubtf = Publisher("tf")
self.pubmarker = Publisher("/carla/marker")
self.pubclock = Publisher("clock")
self.publist = Publisher("/carla/actor_list")
def update_clock(self, carla_timestamp):
"""
perform the update of the clock
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
time = icvTime()
self.timestamp = time.from_sec(carla_timestamp.elapsed_seconds)
self.pubclock.publish(self.timestamp)
def get_current_time(self):
"""
get the current icv time
:return: the current icv time
:rtype icv.Time
"""
return self.timestamp
def __init__(self, carla_actor, parent, communication, synchronous_mode):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param synchronous_mode: use in synchronous mode?
:type synchronous_mode: bool
"""
super(Gnss, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
prefix="gnss/" +
carla_actor.attributes.get('role_name'))
self.pub1 = Publisher(self.get_topic_prefix() + "/fix")
def __init__(self, carla_world, communication):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
"""
super(WorldInfo, self).__init__(parent=None,
communication=communication,
prefix="world_info")
self.carla_map = carla_world.get_map()
self.map_published = False
self.pub1=Publisher(self.get_topic_prefix() )
self.update_loop_thread = Thread(target=self.loop)
self.update_loop_thread.start()
def __init__(self, carla_actor, parent, communication, synchronous_mode):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param synchronous_mode: use in synchronous mode?
:type synchronous_mode: bool
"""
super(LaneInvasionSensor,
self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
is_event_sensor=True,
prefix="lane_invasion")
self.pub1 = Publisher(self.get_topic_prefix())
def __init__(self, parent, communication, actor_list, filtered_id):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param actor_list: current list of actors
:type actor_list: map(carla-actor-id -> python-actor-object)
:param filtered_id: id to filter from actor_list
:type filtered_id: int
"""
super(ObjectSensor, self).__init__(parent=parent,
communication=communication,
prefix='objects')
self.actor_list = actor_list
self.filtered_id = filtered_id
self.pub1 = Publisher(self.get_topic_prefix())
class Gnss(Sensor):
"""
Actor implementation details for gnss sensor
"""
def __init__(self, carla_actor, parent, communication, synchronous_mode):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param synchronous_mode: use in synchronous mode?
:type synchronous_mode: bool
"""
super(Gnss, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
prefix="gnss/" +
carla_actor.attributes.get('role_name'))
self.pub1 = Publisher(self.get_topic_prefix() + "/fix")
# pylint: disable=arguments-differ
def sensor_data_updated(self, carla_gnss_event):
"""
Function to transform a received gnss event into a icv NavSatFix message
:param carla_gnss_event: carla gnss event object
:type carla_gnss_event: carla.GnssEvent
"""
navsatfix_msg = NavSatFix()
navsatfix_msg.header = self.get_msg_header(
timestamp=carla_gnss_event.timestamp)
navsatfix_msg.latitude = carla_gnss_event.latitude
navsatfix_msg.longitude = carla_gnss_event.longitude
navsatfix_msg.altitude = carla_gnss_event.altitude
self.pub1.publish(navsatfix_msg)
class LaneInvasionSensor(Sensor):
"""
Actor implementation details for a lane invasion sensor
"""
def __init__(self, carla_actor, parent, communication, synchronous_mode):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param synchronous_mode: use in synchronous mode?
:type synchronous_mode: bool
"""
super(LaneInvasionSensor,
self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
is_event_sensor=True,
prefix="lane_invasion")
self.pub1 = Publisher(self.get_topic_prefix())
# pylint: disable=arguments-differ
def sensor_data_updated(self, lane_invasion_event):
"""
Function to wrap the lane invasion event into a icv messsage
:param lane_invasion_event: carla lane invasion event object
:type lane_invasion_event: carla.LaneInvasionEvent
"""
lane_invasion_msg = CarlaLaneInvasionEvent()
lane_invasion_msg.header = self.get_msg_header()
for marking in lane_invasion_event.cicvsed_lane_markings:
lane_invasion_msg.cicvsed_lane_markings.append(marking.type)
self.pub1.publish(lane_invasion_msg)
class EgoVehicle(Vehicle):
"""
Vehicle implementation details for the ego vehicle
"""
def __init__(self, carla_actor, parent, communication,
vehicle_control_applied_callback):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
"""
super(EgoVehicle,
self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
prefix=carla_actor.attributes.get('role_name'))
self.vehicle_info_published = False
self.vehicle_control_override = False
self._vehicle_control_applied_callback = vehicle_control_applied_callback
self.sub1 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_cmd")
self.sub2 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_cmd_manual")
self.sub3 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_manual_override")
self.sub4 = Subscriber(self.get_topic_prefix() + "/enable_autopilot")
self.sub5 = Subscriber(self.get_topic_prefix() + "/twist_cmd")
self.Sec_loop = 0.02
self.control_subscriber = CarlaEgoVehicleControl()
self.manual_control_subscriber = CarlaEgoVehicleControl()
self.control_override_subscriber = Bool()
self.enable_autopilot_subscriber = Bool()
self.twist_control_subscriber = Twist()
self.pub1 = Publisher(self.get_topic_prefix() + "/vehicle_info")
self.pub2 = Publisher(self.get_topic_prefix() + "/vehicle_status")
self.update_command_thread = Thread(
target=self._update_commands_thread)
self.update_command_thread.start()
def get_marker_color(self):
"""
Function (override) to return the color for marker messages.
The ego vehicle uses a different marker color than other vehicles.
:return: the color used by a ego vehicle marker
:rtpye : std_msgs.ColorRGBA
"""
color = ColorRGBA()
color.r = 0
color.g = 255
color.b = 0
return color
def send_vehicle_msgs(self):
"""
send messages related to vehicle status
:return:
"""
vehicle_status = CarlaEgoVehicleStatus(
header=self.get_msg_header("map"))
vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor)
vehicle_status.acceleration.linear = transforms.carla_vector_to_icv_vector_rotated(
self.carla_actor.get_acceleration(),
self.carla_actor.get_transform().rotation)
vehicle_status.orientation = self.get_current_icv_pose().orientation
vehicle_status.control.throttle = self.carla_actor.get_control(
).throttle
vehicle_status.control.steer = self.carla_actor.get_control().steer
vehicle_status.control.brake = self.carla_actor.get_control().brake
vehicle_status.control.hand_brake = self.carla_actor.get_control(
).hand_brake
vehicle_status.control.reverse = self.carla_actor.get_control().reverse
vehicle_status.control.gear = self.carla_actor.get_control().gear
vehicle_status.control.manual_gear_shift = self.carla_actor.get_control(
).manual_gear_shift
self.pub2.publish(vehicle_status)
# only send vehicle once (in latched-mode)
if not self.vehicle_info_published:
self.vehicle_info_published = True
vehicle_info = CarlaEgoVehicleInfo()
vehicle_info.id = self.carla_actor.id
vehicle_info.type = self.carla_actor.type_id
vehicle_info.rolename = self.carla_actor.attributes.get(
'role_name')
vehicle_physics = self.carla_actor.get_physics_control()
for wheel in vehicle_physics.wheels:
wheel_info = CarlaEgoVehicleInfoWheel()
wheel_info.tire_friction = wheel.tire_friction
wheel_info.damping_rate = wheel.damping_rate
wheel_info.max_steer_angle = math.radians(
wheel.max_steer_angle)
vehicle_info.wheels.append(wheel_info)
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.moi = vehicle_physics.moi
vehicle_info.damping_rate_full_throttle = vehicle_physics.damping_rate_full_throttle
vehicle_info.damping_rate_zero_throttle_clutch_engaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_engaged
vehicle_info.damping_rate_zero_throttle_clutch_disengaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_disengaged
vehicle_info.use_gear_autobox = vehicle_physics.use_gear_autobox
vehicle_info.gear_switch_time = vehicle_physics.gear_switch_time
vehicle_info.clutch_strength = vehicle_physics.clutch_strength
vehicle_info.mass = vehicle_physics.mass
vehicle_info.drag_coefficient = vehicle_physics.drag_coefficient
vehicle_info.center_of_mass.x = vehicle_physics.center_of_mass.x
vehicle_info.center_of_mass.y = vehicle_physics.center_of_mass.y
vehicle_info.center_of_mass.z = vehicle_physics.center_of_mass.z
self.pub1.publish(vehicle_info)
def update(self, frame, timestamp):
"""
Function (override) to update this object.
On update ego vehicle calculates and sends the new values for VehicleControl()
:return:
"""
self.send_vehicle_msgs()
super(EgoVehicle, self).update(frame, timestamp)
def _update_commands_thread(self):
time.sleep(self.Sec_loop)
if self.sub4.getstate():
self.sub4.reset()
self.enable_autopilot_updated()
if self.sub3.getstate():
self.sub3.reset()
self.control_command_override()
if self.sub1.getstate() or self.sub2.getstate():
self.sub1.reset()
self.sub2.reset()
self.control_command_updated()
if self.sub5.getstate():
self.sub3.reset()
self.twist_command_updated()
def destroy(self):
"""
Function (override) to destroy this object.
Terminate icv subscriptions
Finally forward call to super class.
:return:
"""
#rospy.logdebug("Destroy Vehicle(id={})".format(self.get_id()))
#self.control_subscriber.unregister()
self.control_subscriber = None
#self.enable_autopilot_subscriber.unregister()
self.enable_autopilot_subscriber = None
#self.twist_control_subscriber.unregister()
self.twist_control_subscriber = None
#self.control_override_subscriber.unregister()
self.control_override_subscriber = None
#self.manual_control_subscriber.unregister()
self.manual_control_subscriber = None
super(EgoVehicle, self).destroy()
def twist_command_updated(self):
"""
Set angular/linear velocity (this does not respect vehicle dynamics)
"""
if not self.vehicle_control_override:
sub5.subscribe(self.twist_control_subscriber)
twist = self.twist_control_subscriber
angular_velocity = Vector3D()
angular_velocity.z = math.degrees(twist.angular.z)
rotation_matrix = transforms.carla_rotation_to_numpy_rotation_matrix(
self.carla_actor.get_transform().rotation)
linear_vector = numpy.array(
[twist.linear.x, twist.linear.y, twist.linear.z])
rotated_linear_vector = rotation_matrix.dot(linear_vector)
linear_velocity = Vector3D()
linear_velocity.x = rotated_linear_vector[0]
linear_velocity.y = -rotated_linear_vector[1]
linear_velocity.z = rotated_linear_vector[2]
# rospy.logdebug("Set velocity linear: {}, angular: {}".format(
# linear_velocity, angular_velocity))
self.carla_actor.set_velocity(linear_velocity)
self.carla_actor.set_angular_velocity(angular_velocity)
def control_command_override(self):
"""
Set the vehicle control mode according to icv topic
"""
self.sub3.subscribe(self.control_override_subscriber)
self.vehicle_control_override
def control_command_updated(self):
"""
Receive a CarlaEgoVehicleControl msg and send to CARLA
This function gets called whenever a icv CarlaEgoVehicleControl is received.
If the mode is valid (either normal or manual), the received icv message is
converted into carla.VehicleControl command and sent to CARLA.
This bridge is not responsible for any restrictions on velocity or steering.
It's just forwarding the icv input to CARLA
:param manual_override: manually override the vehicle control command
:param icv_vehicle_control: current vehicle control input received via icv
:type icv_vehicle_control: carla_msgs.msg.CarlaEgoVehicleControl
:return:
"""
if self.vehicle_control_override:
sub2.subscribe(self.manual_control_subscriber)
icv_vehicle_control = self.manual_control_subscriber
else:
sub1.subscribe(self.control_subscriber)
icv_vehicle_control = self.control_subscriber
vehicle_control = VehicleControl()
vehicle_control.hand_brake = icv_vehicle_control.hand_brake
vehicle_control.brake = icv_vehicle_control.brake
vehicle_control.steer = icv_vehicle_control.steer
vehicle_control.throttle = icv_vehicle_control.throttle
vehicle_control.reverse = icv_vehicle_control.reverse
self.carla_actor.apply_control(vehicle_control)
self._vehicle_control_applied_callback(self.get_id())
def enable_autopilot_updated(self):
"""
Enable/disable auto pilot
:param enable_auto_pilot: should the autopilot be enabled?
:type enable_auto_pilot: std_msgs.Bool
:return:
"""
#rospy.logdebug("Ego vehicle: Set autopilot to {}".format(enable_auto_pilot.data))
self.sub4.subscribe(self.enable_autopilot_subscriber)
self.carla_actor.set_autopilot(self.enable_autopilot_subscriber.data)
@staticmethod
def get_vector_length_squared(carla_vector):
"""
Calculate the squared length of a carla_vector
:param carla_vector: the carla vector
:type carla_vector: carla.Vector3D
:return: squared vector length
:rtype: float64
"""
return carla_vector.x * carla_vector.x + \
carla_vector.y * carla_vector.y + \
carla_vector.z * carla_vector.z
@staticmethod
def get_vehicle_speed_squared(carla_vehicle):
"""
Get the squared speed of a carla vehicle
:param carla_vehicle: the carla vehicle
:type carla_vehicle: carla.Vehicle
:return: squared speed of a carla vehicle [(m/s)^2]
:rtype: float64
"""
return EgoVehicle.get_vector_length_squared(
carla_vehicle.get_velocity())
@staticmethod
def get_vehicle_speed_abs(carla_vehicle):
"""
Get the absolute speed of a carla vehicle
:param carla_vehicle: the carla vehicle
:type carla_vehicle: carla.Vehicle
:return: speed of a carla vehicle [m/s >= 0]
:rtype: float64
"""
speed = math.sqrt(EgoVehicle.get_vehicle_speed_squared(carla_vehicle))
return speed
class WorldInfo(PseudoActor):
"""
Publish the map
"""
def __init__(self, carla_world, communication):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
"""
super(WorldInfo, self).__init__(parent=None,
communication=communication,
prefix="world_info")
self.carla_map = carla_world.get_map()
self.map_published = False
self.pub1=Publisher(self.get_topic_prefix() )
self.update_loop_thread = Thread(target=self.loop)
self.update_loop_thread.start()
def loop(self):
while True:
time.sleep(10)
self.map_published = False
def destroy(self):
"""
Function (override) to destroy this object.
Remove reference to carla.Map object.
Finally forward call to super class.
:return:
"""
#rospy.logdebug("Destroying WorldInfo()")
self.carla_map = None
super(WorldInfo, self).destroy()
def update(self, frame, timestamp):
"""
Function (override) to update this object.
:return:
"""
if not self.map_published:
#print("send map")
open_drive_msg = CarlaWorldInfo()
open_drive_msg.map_name = self.carla_map.name
open_drive_msg.opendrive = self.carla_map.to_opendrive()
self.pub1.publish( open_drive_msg)
self.map_published = True
class Lidar(Sensor):
"""
Actor implementation details for lidars
"""
def __init__(self, carla_actor, parent, communication, synchronous_mode):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
"""
super(Lidar, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
prefix='lidar/' + carla_actor.attributes.get('role_name'))
self.pub1=Publisher(self.get_topic_prefix()+ "/point_cloud")
def get_icv_sensor_transform(self, transform):
"""
Function (override) to modify the tf messages sent by this lidar.
The lidar transformation has to be altered:
for some reasons lidar sends already a rotated cloud,
so herein, we need to ignore pitch and roll
:return: the filled tf message
:rtype: geometry_msgs.msg.TransformStamped
"""
tf_msg = super(Lidar, self).get_icv_sensor_transform(transform)
rotation = tf_msg.transform.rotation
quat = [rotation.x, rotation.y, rotation.z, rotation.w]
dummy_roll, dummy_pitch, yaw = transformations.euler_from_quaternion(
quat)
# set roll and pitch to zero
quat = transformations.quaternion_from_euler(0, 0, yaw)
tf_msg.transform.rotation = trans.numpy_quaternion_to_icv_quaternion(
quat)
return tf_msg
# pylint: disable=arguments-differ
def sensor_data_updated(self, carla_lidar_measurement):
"""
Function to transform the a received lidar measurement into a icv point cloud message
:param carla_lidar_measurement: carla lidar measurement object
:type carla_lidar_measurement: carla.LidarMeasurement
"""
header = self.get_msg_header()
lidar_data = numpy.frombuffer(
carla_lidar_measurement.raw_data, dtype=numpy.float32)
lidar_data = numpy.reshape(
lidar_data, (int(lidar_data.shape[0] / 3), 3))
# we take the oposite of y axis
# (as lidar point are express in left handed coordinate system, and icv need right handed)
# we need a copy here, because the data are read only in carla numpy
# array
lidar_data = -lidar_data
# we also need to permute x and y
lidar_data = lidar_data[..., [1, 0, 2]]
point_cloud_msg = create_cloud_xyz32(header, lidar_data)
self.pub1.publish(point_cloud_msg)
class Camera(Sensor):
"""
Sensor implementation details for cameras
"""
# global cv bridge to convert image between opencv and icv
def __init__(self, carla_actor, parent, communication, synchronous_mode, prefix=None): # pylint: disable=too-many-arguments
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param prefix: the topic prefix to be used for this actor
:type prefix: string
"""
if not prefix:
prefix = 'camera'
super(Camera, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
synchronous_mode=synchronous_mode,
prefix=prefix)
#if self.__class__.__name__ == "Camera":
# rospy.logwarn("Created Unsupported Camera Actor"
# "(id={}, parent_id={}, type={}, attributes={})".format(
# self.get_id(), self.get_parent_id(),
# self.carla_actor.type_id, self.carla_actor.attributes))
#else:
self.pub1=Publisher(self.get_topic_prefix() + '/camera_info')
self.pub2=Publisher(self.get_topic_prefix() + '/' + self.get_image_topic_name())
self.cv_bridge = CvBridge()
self._build_camera_info()
def _build_camera_info(self):
"""
Private function to compute camera info
camera info doesn't change over time
"""
camera_info = CameraInfo()
# store info without header
camera_info.header = None
camera_info.width = int(self.carla_actor.attributes['image_size_x'])
camera_info.height = int(self.carla_actor.attributes['image_size_y'])
camera_info.distortion_model = 'plumb_bob'
cx = camera_info.width / 2.0
cy = camera_info.height / 2.0
fx = camera_info.width / (
2.0 * math.tan(float(self.carla_actor.attributes['fov']) * math.pi / 360.0))
fy = fx
camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
camera_info.D = [0, 0, 0, 0, 0]
camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0]
self._camera_info = camera_info
# pylint: disable=arguments-differ
def sensor_data_updated(self, carla_image):
"""
Function (override) to transform the received carla image data
into a icv image message
:param carla_image: carla image object
:type carla_image: carla.Image
"""
#if ((carla_image.height != self._camera_info.height) or
# (carla_image.width != self._camera_info.width)):
# rospy.logerr(
# "Camera{} received image not matching configuration".format(self.get_prefix()))
image_data_array, encoding = self.get_carla_image_data_array(
carla_image=carla_image)
img_msg = self.cv_bridge.cv2_to_imgmsg(image_data_array, encoding=encoding)
# the camera data is in respect to the camera's own frame
img_msg.header = self.get_msg_header()
cam_info = self._camera_info
cam_info.header = img_msg.header
self.pub1.publish(cam_info)
self.pub2.publish(img_msg)
def get_icv_sensor_transform(self, transform):
"""
Function (override) to modify the tf messages sent by this camera.
The camera transformation has to be altered to look at the same axis
as the opencv projection in order to get easy depth cloud for RGBD camera
:return: the filled tf message
:rtype: geometry_msgs.msg.TransformStamped
"""
tf_msg = super(Camera, self).get_icv_sensor_transform(transform)
rotation = tf_msg.transform.rotation
quat = [rotation.x, rotation.y, rotation.z, rotation.w]
quat_swap = transformations.quaternion_from_matrix(
[[0, 0, 1, 0],
[-1, 0, 0, 0],
[0, -1, 0, 0],
[0, 0, 0, 1]])
quat = transformations.quaternion_multiply(quat, quat_swap)
tf_msg.transform.rotation = trans.numpy_quaternion_to_icv_quaternion(
quat)
return tf_msg
@abstractmethod
def get_carla_image_data_array(self, carla_image):
"""
Virtual function to convert the carla image to a numpy data array
as input for the cv_bridge.cv2_to_imgmsg() function
:param carla_image: carla image object
:type carla_image: carla.Image
:return tuple (numpy data array containing the image information, encoding)
:rtype tuple(numpy.ndarray, string)
"""
raise NotImplementedError(
"This function has to be re-implemented by derived classes")
@abstractmethod
def get_image_topic_name(self):
"""
Virtual function to provide the actual image topic name
:return image topic name
:rtype string
"""
raise NotImplementedError(
"This function has to be re-implemented by derived classes")
class CarlaToRosWaypointConverter(object):
"""
This class generates a plan of waypoints to follow.
The calculation is done whenever:
- the hero vehicle appears
- a new goal is set
"""
WAYPOINT_DISTANCE = 2.0
def __init__(self, carla_world, setstart, setgoal):
self.world = carla_world
#only for circles
self.map = carla_world.get_map()
self.ego_vehicle = None
self.role_name = 'ego_vehicle'
self.waypoint_publisher = Publisher(
'carla_ego_vehicle_waypoints'.format(self.role_name))
# set initial goal
self.start = setstart
self.goal = setgoal
self.current_route = None
self.goal_subscriber = Subscriber("/carla/{}/goal".format(
self.role_name))
self._pose_subscriber = Subscriber(
"ego_pose_localization"
) #, RigidBodyStateStamped, self.pose_callback
self._pose = RigidBodyStateStamped()
self.pose_ = PoseStamped()
self._update_lock = threading.Lock()
self.update_loop_thread = Thread(target=self.loop)
self.update_loop_thread.start()
# use callback to wait for ego vehicle
self.world.on_tick(self.find_ego_vehicle_actor)
def loop(self):
while True:
time.sleep(0.25)
self.publish_waypoints()
'''
if self.goal_subscriber.getstate():
self.goal_subscriber.reset()
print "goal?"
self.goal_subscriber.subscribe(self.pose_)
self.on_goal(self.pose_)'''
'''
if self._pose_subscriber.getstate():
self._pose_subscriber.reset()
self._pose_subscriber.subscribe(self._pose)
self.pose_callback(self._pose)'''
def pose_callback(self, msg):
# Note: Here we actually assume that pose is updating at highest frequency
print("We received pose in carla waypoint publisher")
#self.publish_waypoints()
def on_goal(self, goal):
"""
Callback for /move_base_simple/goal
Receiving a goal (e.g. from RVIZ '2D Nav Goal') triggers a new route calculation.
:return:
"""
carla_goal = carla.Transform()
carla_goal.location.x = goal.pose.position.x
carla_goal.location.y = -goal.pose.position.y
carla_goal.location.z = goal.pose.position.z + 2 # 2m above ground
quaternion = (goal.pose.orientation.x, goal.pose.orientation.y,
goal.pose.orientation.z, goal.pose.orientation.w)
_, _, yaw = euler_from_quaternion(quaternion)
carla_goal.rotation.yaw = -math.degrees(yaw)
self.goal = carla_goal
print("start to reroute")
self.reroute()
def reroute(self):
"""
Triggers a rerouting
"""
if self.ego_vehicle is None or self.goal is None:
# no ego vehicle, remove route if published
self.current_route = None
self.publish_waypoints()
else:
self.current_route = self.calculate_route(self.start, self.goal)
self.publish_waypoints()
def find_ego_vehicle_actor(self, _):
"""
Look for an carla actor with name 'ego_vehicle'
"""
with self._update_lock:
hero = None
for actor in self.world.get_actors():
if actor.attributes.get('role_name') == self.role_name:
hero = actor
break
ego_vehicle_changed = False
if hero is None and self.ego_vehicle is not None:
ego_vehicle_changed = True
if not ego_vehicle_changed and hero is not None and self.ego_vehicle is None:
ego_vehicle_changed = True
if not ego_vehicle_changed and hero is not None and \
self.ego_vehicle is not None and hero.id != self.ego_vehicle.id:
ego_vehicle_changed = True
if ego_vehicle_changed:
#rospy.loginfo("Ego vehicle changed.")
self.ego_vehicle = hero
self.reroute()
def calculate_route(self, start, goal):
"""
Calculate a route from the current location to 'goal'
"""
# rospy.loginfo("Calculating route to x={}, y={}, z={}".format(
# goal.location.x,
# goal.location.y,
# goal.location.z))
dao = GlobalRoutePlannerDAO(self.world.get_map(), 1)
grp = GlobalRoutePlanner(dao)
grp.setup()
route = grp.trace_route(
carla.Location(start.location.x, start.location.y,
start.location.z),
carla.Location(goal.location.x, goal.location.y, goal.location.z))
return route
def publish_waypoints(self):
"""
Publish the ROS message containing the waypoints
"""
msg = Path()
msg.header.frame_id = "map"
msg.header.stamp = icvTime().time_now_o()
if self.current_route is not None:
for wp in self.current_route:
pose = PoseStamped()
pose.pose.position.x = wp[0].transform.location.x
pose.pose.position.y = -wp[0].transform.location.y
pose.pose.position.z = wp[0].transform.location.z
quaternion = transformations.quaternion_from_euler(
0, 0, -math.radians(wp[0].transform.rotation.yaw))
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
msg.poses.append(pose)
self.waypoint_publisher.publish(msg)
print("Published {} waypoints.".format(len(msg.poses)))
