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 __init__(self, carla_actor, parent, communication, prefix=None):
"""
Constructor
:param carla_actor: carla walker actor object
:type carla_actor: carla.Walker
: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 = "walker/{:03}".format(carla_actor.id)
super(Walker, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
prefix=prefix)
self.control_subscriber = CarlaWalkerControl()
self.sub1 = Subscriber(self.get_topic_prefix() + "/walker_control_cmd")
self.Sec_loop = 0.02
self.update_command_thread = Thread(
target=self._update_commands_thread)
self.update_command_thread.start()
def __init__(self, carla_debug_helper):
"""
Constructor
:param carla_debug_helper: carla debug helper
:type carla_debug_helper: carla.DebugHelper
"""
self.debug = carla_debug_helper
self.MarkerArray2=MarkerArray()
self.sub1=Subscriber("/carla/debug_marker")
self.Sec_loop=0.02
self.update_command_thread = Thread(target=self._update_commands_thread)
self.update_command_thread.start()
def __init__(self):
self.host = "localhost" #rospy.get_param('/carla/host', '127.0.0.1')
self.port = 2000 #rospy.get_param('/carla/port', '2000')
self.sensor_definition_file = './config/sensors.json'
self.world = None
self.player = None
self.player_created = False
self.sensor_actors = []
self.actor_filter = 'vehicle'
self.actor_spawnpoint = None
self.role_name = 'ego_vehicle'
self._rate = 4
# check argument and set spawn_point
self.spawn_point_param = ''
self.spawn_ego_vehicle = True
if self.spawn_point_param and self.spawn_ego_vehicle:
#rospy.loginfo("Using ros parameter for spawnpoint: {}".format(spawn_point_param))
spawn_point = self.spawn_point_param.split(',')
if len(spawn_point) != 6:
raise ValueError("Invalid spawnpoint '{}'".format(
self.spawn_point_param))
pose = Pose()
pose.position.x = float(spawn_point[0])
pose.position.y = -float(spawn_point[1])
pose.position.z = float(spawn_point[2])
quat = quaternion_from_euler(math.radians(float(spawn_point[3])),
math.radians(float(spawn_point[4])),
math.radians(float(spawn_point[5])))
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
self.actor_spawnpoint = pose
self.initialpose_subscriber = Subscriber(
"/carla/{}/initialpose".format(self.role_name)
) # PoseWithCovarianceStamped, self.on_initialpose
self.posestamp = PoseWithCovarianceStamped()
#rospy.loginfo('listening to server %s:%s', self.host, self.port)
#rospy.loginfo('using vehicle filter: %s', self.actor_filter)
self.update_loop_thread = Thread(target=self.loop)
self.update_loop_thread.start()
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 run(self):
"""
main loop
"""
# wait for ros-bridge to set up CARLA world
#rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
#rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
sub1 = Subscriber("/carla/world_info")
worldinfo = CarlaWorldInfo()
sub1.enable()
sub1.subscribe(worldinfo)
client = carla.Client(host=self.host, port=self.port)
client.set_timeout(2.0)
self.world = client.get_world()
self.restart()
while True:
time.sleep(10)
def main():
"""
main function
"""
first_goal = Transform()
first_goal.location.x = 32
first_goal.location.y = -20
first_goal.location.z = 0
first_goal.rotation.pitch = 0
first_goal.rotation.yaw = 0
first_goal.rotation.roll = 0
first_start = Transform()
first_start.location.x = -10
first_start.location.y = -96
first_start.location.z = 0
first_start.rotation.pitch = 0
first_start.rotation.yaw = 180
first_start.rotation.roll = 0
sub1 = Subscriber("/carla/world_info")
worldinfo = CarlaWorldInfo()
sub1.enable()
sub1.subscribe(worldinfo)
host = "127.0.0.1"
port = 2000
carla_client = carla.Client(host=host, port=port)
carla_client.set_timeout(2)
carla_world = carla_client.get_world()
waypointConverter = CarlaToRosWaypointConverter(carla_world, first_start,
first_goal)
class Walker(TrafficParticipant):
"""
Actor implementation details for pedestrians
"""
def __init__(self, carla_actor, parent, communication, prefix=None):
"""
Constructor
:param carla_actor: carla walker actor object
:type carla_actor: carla.Walker
: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 = "walker/{:03}".format(carla_actor.id)
super(Walker, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
prefix=prefix)
self.control_subscriber = CarlaWalkerControl()
self.sub1 = Subscriber(self.get_topic_prefix() + "/walker_control_cmd")
self.Sec_loop = 0.02
self.update_command_thread = Thread(
target=self._update_commands_thread)
self.update_command_thread.start()
def update(self, frame, timestamp):
super(Walker, self).update(frame, timestamp)
def control_command_updated(self):
"""
Receive a CarlaWalkerControl msg and send to CARLA
This function gets called whenever a icv message is received via
'/carla/<role name>/walker_control_cmd' topic.
The received icv message is converted into carla.WalkerControl command and
sent to CARLA.
:param icv_walker_control: current walker control input received via icv
:type self.info.output: carla_icv_bridge.msg.CarlaWalkerControl
:return:
"""
self.sub1.subscribe(self.control_subscriber)
walker_control = WalkerControl()
walker_control.direction.x = self.control_subscriber.direction.x
walker_control.direction.y = -self.control_subscriber.direction.y
walker_control.direction.z = self.control_subscriber.direction.z
walker_control.speed = self.control_subscriber.speed
walker_control.jump = self.control_subscriber.jump
self.carla_actor.apply_control(walker_control)
def _update_commands_thread(self):
if self.sub1.getstate():
self.sub1.reset()
self.control_command_updated()
def get_classification(self):
"""
Function (override) to get classification
:return:
"""
return Object.CLASSIFICATION_PEDESTRIAN
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
#@Deprecated
#CARLA_VERSION = "0.9.9"
# check CARLA version
#dist = pkg_resources.get_distribution("carla")
#if LooseVersion(dist.version) < LooseVersion(self.CARLA_VERSION):
# raise ImportError(
# "CARLA version {} or newer required. CARLA version found: {}".format(
# self.CARLA_VERSION, dist))
self.parameters = params
self.actors = {}
self.actors_list = []
self.pseudo_actors = []
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
self.shutdown = Event()
# set carla world settings
self.carla_settings = carla_world.get_settings()
self.sub1=Subscriber("/carla/control")
self.carla_control_data = CarlaControl()
print("before setting")
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
self.carla_settings.synchronous_mode = self.parameters["synchronous_mode"]
self.carla_settings.fixed_delta_seconds = self.parameters["fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self._rate=self.carla_settings.fixed_delta_seconds
self.update_command_thread = Thread(target=self._update_commands_thread)
self.update_command_thread.start()
# workaround: settings can only applied within non-sync mode
self.comm = Communication()
self.update_lock = Lock()
self.carla_control_queue = queue.Queue()
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.carla_settings.synchronous_mode:
self.carla_run_state = CarlaControl.PLAY
self.synchronous_mode_update_thread = Thread(target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.update_actors_queue = queue.Queue(maxsize=1)
# start thread to update actors
self.update_actor_thread = Thread(target=self._update_actors_thread)
self.update_actor_thread.start()
# create initially existing actors
self.update_actors_queue.put(set([x.id for x in self.carla_world.get_snapshot()]))
# wait for first actors creation to be finished
self.update_actors_queue.join()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# add world info
self.pseudo_actors.append(WorldInfo(carla_world=self.carla_world,
communication=self.comm))
# add global object sensor
self.pseudo_actors.append(ObjectSensor(parent=None,
communication=self.comm,
actor_list=self.actors,
filtered_id=None))
class CarlaICVBridge(object):
"""
Carla icv bridge
"""
CARLA_VERSION = "0.9.9"
def __init__(self, carla_world, params):
"""
Constructor
:param carla_world: carla world object
:type carla_world: carla.World
:param params: dict of parameters, see settings.yaml
:type params: dict
"""
#@Deprecated
#CARLA_VERSION = "0.9.9"
# check CARLA version
#dist = pkg_resources.get_distribution("carla")
#if LooseVersion(dist.version) < LooseVersion(self.CARLA_VERSION):
# raise ImportError(
# "CARLA version {} or newer required. CARLA version found: {}".format(
# self.CARLA_VERSION, dist))
self.parameters = params
self.actors = {}
self.actors_list = []
self.pseudo_actors = []
self.carla_world = carla_world
self.synchronous_mode_update_thread = None
self.shutdown = Event()
# set carla world settings
self.carla_settings = carla_world.get_settings()
self.sub1=Subscriber("/carla/control")
self.carla_control_data = CarlaControl()
print("before setting")
if self.carla_settings.synchronous_mode:
self.carla_settings.synchronous_mode = False
carla_world.apply_settings(self.carla_settings)
self.carla_settings.synchronous_mode = self.parameters["synchronous_mode"]
self.carla_settings.fixed_delta_seconds = self.parameters["fixed_delta_seconds"]
carla_world.apply_settings(self.carla_settings)
self._rate=self.carla_settings.fixed_delta_seconds
self.update_command_thread = Thread(target=self._update_commands_thread)
self.update_command_thread.start()
# workaround: settings can only applied within non-sync mode
self.comm = Communication()
self.update_lock = Lock()
self.carla_control_queue = queue.Queue()
self.status_publisher = CarlaStatusPublisher(
self.carla_settings.synchronous_mode,
self.carla_settings.fixed_delta_seconds)
# for waiting for ego vehicle control commands in synchronous mode,
# their ids are maintained in a list.
# Before tick(), the list is filled and the loop waits until the list is empty.
self._all_vehicle_control_commands_received = Event()
self._expected_ego_vehicle_control_command_ids = []
self._expected_ego_vehicle_control_command_ids_lock = Lock()
if self.carla_settings.synchronous_mode:
self.carla_run_state = CarlaControl.PLAY
self.synchronous_mode_update_thread = Thread(target=self._synchronous_mode_update)
self.synchronous_mode_update_thread.start()
else:
self.timestamp_last_run = 0.0
self.update_actors_queue = queue.Queue(maxsize=1)
# start thread to update actors
self.update_actor_thread = Thread(target=self._update_actors_thread)
self.update_actor_thread.start()
# create initially existing actors
self.update_actors_queue.put(set([x.id for x in self.carla_world.get_snapshot()]))
# wait for first actors creation to be finished
self.update_actors_queue.join()
# register callback to update actors
self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)
# add world info
self.pseudo_actors.append(WorldInfo(carla_world=self.carla_world,
communication=self.comm))
# add global object sensor
self.pseudo_actors.append(ObjectSensor(parent=None,
communication=self.comm,
actor_list=self.actors,
filtered_id=None))
#self.debug_helper = DebugHelper(carla_world.debug)
def destroy(self):
"""
Function to destroy this object.
:return:
"""
#rospy.signal_shutdown("")
self.debug_helper.destroy()
self.shutdown.set()
self.carla_control_queue.put(CarlaControl.STEP_ONCE)
if not self.carla_settings.synchronous_mode:
if self.on_tick_id:
self.carla_world.remove_on_tick(self.on_tick_id)
self.update_actor_thread.join()
self._update_actors(set())
#rospy.loginfo("Exiting Bridge")
def process_run_state(self):
"""
process state changes
"""
command = None
# get last command
while not self.carla_control_queue.empty():
command = self.carla_control_queue.get()
while not command is None: #and not rospy.is_shutdown():
self.carla_run_state = command
if self.carla_run_state == CarlaControl.PAUSE:
# wait for next command
#rospy.loginfo("State set to PAUSED")
self.status_publisher.set_synchronous_mode_running(False)
command = self.carla_control_queue.get()
elif self.carla_run_state == CarlaControl.PLAY:
#rospy.loginfo("State set to PLAY")
self.status_publisher.set_synchronous_mode_running(True)
return
elif self.carla_run_state == CarlaControl.STEP_ONCE:
#rospy.loginfo("Execute single step.")
self.status_publisher.set_synchronous_mode_running(True)
self.carla_control_queue.put(CarlaControl.PAUSE)
return
def _update_commands_thread (self):
time.sleep(self.carla_settings.fixed_delta_seconds)
if self.sub1.getstate():
self.sub1.reset()
self.sub1.subscribe( self.carla_control_data)
self.carla_control_queue.put(self.carla_control_data.command)
def _synchronous_mode_update(self):
"""
execution loop for synchronous mode
"""
print("synchronous mode")
while not self.shutdown.is_set():
self.process_run_state()
time.sleep(self._rate)
if self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
# fill list of available ego vehicles
self._expected_ego_vehicle_control_command_ids = []
with self._expected_ego_vehicle_control_command_ids_lock:
for actor_id, actor in self.actors.iteritems():
if isinstance(actor, EgoVehicle):
self._expected_ego_vehicle_control_command_ids.append(actor_id)
frame = self.carla_world.tick()
world_snapshot = self.carla_world.get_snapshot()
self.status_publisher.set_frame(frame)
self.comm.update_clock(world_snapshot.timestamp)
#rospy.logdebug("Tick for frame {} returned. Waiting for sensor data...".format(
# frame))
self._update(frame, world_snapshot.timestamp.elapsed_seconds)
#rospy.logdebug("Waiting for sensor data finished.")
#self.comm.send_msgs()
# print ("check actors")
# for tactor in self.carla_world.get_actors(list(previous_actors)):
# print(tactor.type_id)
idset=set([x.id for x in world_snapshot])
#print("number of actor :%s" % len(idset))
self._update_actors(idset)
if self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
# wait for all ego vehicles to send a vehicle control command
if self._expected_ego_vehicle_control_command_ids:
if not self._all_vehicle_control_commands_received.wait(1):
print("control not received")
# rospy.logwarn("Timeout (1s) while waiting for vehicle control commands. "
# "Missing command from actor ids {}".format(
# self._expected_ego_vehicle_control_command_ids))
self._all_vehicle_control_commands_received.clear()
def _carla_time_tick(self, carla_snapshot):
"""
Private callback registered at carla.World.on_tick()
to trigger cyclic updates.
After successful locking the update mutex
(only perform trylock to respect bridge processing time)
the clock and the children are updated.
Finally the icv messages collected to be published are sent out.
:param carla_timestamp: the current carla time
:type carla_timestamp: carla.Timestamp
:return:
"""
if not self.shutdown.is_set():
if self.update_lock.acquire(False):
if self.timestamp_last_run < carla_snapshot.timestamp.elapsed_seconds:
self.timestamp_last_run = carla_snapshot.timestamp.elapsed_seconds
self.comm.update_clock(carla_snapshot.timestamp)
self.status_publisher.set_frame(carla_snapshot.frame)
self._update(carla_snapshot.frame, carla_snapshot.timestamp.elapsed_seconds)
#self.comm.send_msgs()
self.update_lock.release()
# if possible push current snapshot to update-actors-thread
try:
self.update_actors_queue.put_nowait(set([x.id for x in carla_snapshot]))
except queue.Full:
pass
def _update_actors_thread(self):
"""
execution loop for async mode actor list updates
"""
while not self.shutdown.is_set():
try:
current_actors = self.update_actors_queue.get(timeout=1)
if current_actors:
self._update_actors(current_actors)
self.update_actors_queue.task_done()
except queue.Empty:
pass
def _update_actors(self, current_actors):
"""
update the available actors
"""
previous_actors = set(self.actors)
new_actors = current_actors - previous_actors
deleted_actors = previous_actors - current_actors
if new_actors:
for carla_actor in self.carla_world.get_actors(list(new_actors)):
self._create_actor(carla_actor)
if deleted_actors:
for id_to_delete in deleted_actors:
# remove actor
actor = self.actors[id_to_delete]
with self.update_lock:
# rospy.loginfo("Remove {}(id={}, parent_id={}, prefix={})".format(
# actor.__class__.__name__, actor.get_id(),
# actor.get_parent_id(),
# actor.get_prefix()))
actor.destroy()
del self.actors[id_to_delete]
# remove pseudo-actors that have actor as parent
updated_pseudo_actors = []
for pseudo_actor in self.pseudo_actors:
if pseudo_actor.get_parent_id() == id_to_delete:
# rospy.loginfo("Remove {}(parent_id={}, prefix={})".format(
# pseudo_actor.__class__.__name__,
# pseudo_actor.get_parent_id(),
# pseudo_actor.get_prefix()))
pseudo_actor.destroy()
del pseudo_actor
else:
updated_pseudo_actors.append(pseudo_actor)
self.pseudo_actors = updated_pseudo_actors
# publish actor list on change
if new_actors or deleted_actors:
self.publish_actor_list()
def publish_actor_list(self):
"""
publish list of carla actors
:return:
"""
icv_actor_list = CarlaActorList()
for actor_id in self.actors:
actor = self.actors[actor_id].carla_actor
icv_actor = CarlaActorInfo()
icv_actor.id = actor.id
icv_actor.type = actor.type_id
try:
icv_actor.rolename = str(actor.attributes.get('role_name'))
except ValueError:
pass
if actor.parent:
icv_actor.parent_id = actor.parent.id
else:
icv_actor.parent_id = 0
icv_actor_list.actors.append(icv_actor)
self.comm.publist.publish( icv_actor_list)
def _create_actor(self, carla_actor): # pylint: disable=too-many-branches,too-many-statements
"""
create an actor
"""
parent = None
if carla_actor.parent:
if carla_actor.parent.id in self.actors:
parent = self.actors[carla_actor.parent.id]
else:
parent = self._create_actor(carla_actor.parent)
actor = None
pseudo_actors = []
#print("typeid")
#print(carla_actor.type_id)
if carla_actor.type_id.startswith('traffic'):
if carla_actor.type_id == "traffic.traffic_light":
actor = TrafficLight(carla_actor, parent, self.comm)
else:
actor = Traffic(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("vehicle"):
print(carla_actor.attributes.get('role_name'))
print(carla_actor.type_id)
print(carla_actor.id)
if carla_actor.attributes.get('role_name')\
in self.parameters['ego_vehicle']['role_name']:
actor = EgoVehicle(
carla_actor, parent, self.comm, self._ego_vehicle_control_applied_callback)
pseudo_actors.append(ObjectSensor(parent=actor,
communication=self.comm,
actor_list=self.actors,
filtered_id=carla_actor.id))
else:
actor = Vehicle(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("sensor"):
if carla_actor.type_id.startswith("sensor.camera"):
if carla_actor.type_id.startswith("sensor.camera.rgb"):
actor = RgbCamera(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.camera.depth"):
actor = DepthCamera(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.camera.semantic_segmentation"):
actor = SemanticSegmentationCamera(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
else:
actor = Camera(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.lidar"):
actor = Lidar(carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.gnss"):
actor = Gnss(carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.collision"):
actor = CollisionSensor(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.lane_invasion"):
actor = LaneInvasionSensor(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
else:
actor = Sensor(carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("spectator"):
actor = Spectator(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("walker"):
actor = Walker(carla_actor, parent, self.comm)
else:
actor = Actor(carla_actor, parent, self.comm)
with self.update_lock:
self.actors[carla_actor.id] = actor
for pseudo_actor in pseudo_actors:
with self.update_lock:
self.pseudo_actors.append(pseudo_actor)
return actor
def run(self):
"""
Run the bridge functionality.
Registers on shutdown callback at rospy and spins icv.
:return:
"""
while not self.shutdown.is_set():
time.sleep(5)
def on_shutdown(self):
"""
Function to be called on shutdown.
This function is registered at rospy as shutdown handler.
"""
# rospy.loginfo("Shutdown requested")
self.destroy()
def _update(self, frame_id, timestamp):
"""
update all actors
:return:
"""
# update all pseudo actors
for actor in self.pseudo_actors:
actor.update(frame_id, timestamp)
# update all carla actors
for actor_id in self.actors:
try:
self.actors[actor_id].update(frame_id, timestamp)
except RuntimeError as e:
#rospy.logwarn("Update actor {}({}) failed: {}".format(
# self.actors[actor_id].__class__.__name__, actor_id, e))
continue
def _ego_vehicle_control_applied_callback(self, ego_vehicle_id):
if not self.carla_settings.synchronous_mode or \
not self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
return
with self._expected_ego_vehicle_control_command_ids_lock:
if ego_vehicle_id in self._expected_ego_vehicle_control_command_ids:
self._expected_ego_vehicle_control_command_ids.remove(ego_vehicle_id)
else:
print("unexpected control")
if not self._expected_ego_vehicle_control_command_ids:
self._all_vehicle_control_commands_received.set()
class DebugHelper(object):
"""
Helper to draw markers in CARLA
"""
def __init__(self, carla_debug_helper):
"""
Constructor
:param carla_debug_helper: carla debug helper
:type carla_debug_helper: carla.DebugHelper
"""
self.debug = carla_debug_helper
self.MarkerArray2=MarkerArray()
self.sub1=Subscriber("/carla/debug_marker")
self.Sec_loop=0.02
self.update_command_thread = Thread(target=self._update_commands_thread)
self.update_command_thread.start()
def destroy(self):
"""
Function (override) to destroy this object.
Terminate icv subscriptions
:return:
"""
#rospy.logdebug("Destroy DebugHelper")
self.debug = None
self.marker_subscriber = None
def _update_commands_thread (self):
if self.sub1.getstate():
self.sub1.reset()
self.on_marker()
def on_marker(self, marker_array):
"""
Receive markers from icv and apply in CARLA
"""
self.sub1.subscribe(self.MarkerArray2)
marker_array=self.MarkerArray2
for marker in marker_array.markers:
if marker.header.frame_id != "map":
#rospy.logwarn(
# "Could not draw marker in frame '{}'. Only 'map' supported.".format(
# marker.header.frame_id))
continue
lifetime = -1.
if marker.lifetime:
lifetime = marker.lifetime.to_sec()
color = carla.Color(int(marker.color.r * 255),
int(marker.color.g * 255),
int(marker.color.b * 255),
int(marker.color.a * 255))
if marker.type == Marker.POINTS:
self.draw_points(marker, lifetime, color)
elif marker.type == Marker.LINE_STRIP:
self.draw_line_strips(marker, lifetime, color)
elif marker.type == Marker.ARROW:
self.draw_arrow(marker, lifetime, color)
elif marker.type == Marker.CUBE:
self.draw_box(marker, lifetime, color)
#else:
# rospy.logwarn("Marker type '{}' not supported.".format(marker.type))
def draw_arrow(self, marker, lifetime, color):
"""
draw arrow from icv marker
"""
if marker.points:
#if not len(marker.points) == 2:
# rospy.logwarn(
# "Drawing arrow from points requires two points. Received {}".format(
# len(marker.points)))
# return
thickness = marker.scale.x
arrow_size = marker.scale.y
start = carla.Location(
x=marker.points[0].x, y=-marker.points[0].y, z=marker.points[0].z)
end = carla.Location(
x=marker.points[1].x, y=-marker.points[1].y, z=marker.points[1].z)
#rospy.loginfo("Draw Arrow from {} to {} (color: {}, lifetime: {}, "
# "thickness: {}, arrow_size: {})".format(
# start, end, color, lifetime, thickness, arrow_size))
self.debug.draw_arrow(
start,
end,
thickness=thickness,
arrow_size=arrow_size,
color=color,
life_time=lifetime)
#else:
# rospy.logwarn(
# "Drawing arrow from Position/Orientation not yet supported. "
# "Please use points.")
def draw_points(self, marker, lifetime, color):
"""
draw points from icv marker
"""
for point in marker.points:
location = carla.Location(x=point.x, y=-point.y, z=point.z)
size = marker.scale.x
# rospy.loginfo("Draw Point {} (color: {}, lifetime: {}, size: {})".format(
# location, color, lifetime, size))
self.debug.draw_point(location, size=size, color=color, life_time=lifetime)
def draw_line_strips(self, marker, lifetime, color):
"""
draw lines from icv marker
"""
#if len(marker.points) < 2:
# rospy.logwarn(
# "Drawing line-strip requires at least two points. Received {}".format(
# len(marker.points)))
# return
last_point = None
thickness = marker.scale.x
for point in marker.points:
if last_point:
start = carla.Location(x=last_point.x, y=-last_point.y, z=last_point.z)
end = carla.Location(x=point.x, y=-point.y, z=point.z)
# rospy.loginfo(
# "Draw Line from {} to {} (color: {}, lifetime: {}, "
# "thickness: {})".format(
# start, end, color, lifetime, thickness))
self.debug.draw_line(start,
end,
thickness=thickness,
color=color,
life_time=lifetime)
last_point = point
def draw_box(self, marker, lifetime, color):
"""
draw box from icv marker
"""
box = carla.BoundingBox()
box.location.x = marker.pose.position.x
box.location.y = -marker.pose.position.y
box.location.z = marker.pose.position.z
box.extent.x = marker.scale.x / 2
box.extent.y = marker.scale.y / 2
box.extent.z = marker.scale.z / 2
roll, pitch, yaw = euler_from_quaternion([
marker.pose.orientation.x,
marker.pose.orientation.y,
marker.pose.orientation.z,
marker.pose.orientation.w
])
rotation = carla.Rotation()
rotation.roll = math.degrees(roll)
rotation.pitch = math.degrees(pitch)
rotation.yaw = -math.degrees(yaw)
#rospy.loginfo("Draw Box {} (rotation: {}, color: {}, lifetime: {})".format(
# box, rotation, color, lifetime))
self.debug.draw_box(box, rotation, thickness=0.1, color=color, life_time=lifetime)
class CarlaEgoVehicle(object):
"""
Handles the spawning of the ego vehicle and its sensors
Derive from this class and implement method sensors()
"""
def __init__(self):
self.host = "localhost" #rospy.get_param('/carla/host', '127.0.0.1')
self.port = 2000 #rospy.get_param('/carla/port', '2000')
self.sensor_definition_file = './config/sensors.json'
self.world = None
self.player = None
self.player_created = False
self.sensor_actors = []
self.actor_filter = 'vehicle'
self.actor_spawnpoint = None
self.role_name = 'ego_vehicle'
self._rate = 4
# check argument and set spawn_point
self.spawn_point_param = ''
self.spawn_ego_vehicle = True
if self.spawn_point_param and self.spawn_ego_vehicle:
#rospy.loginfo("Using ros parameter for spawnpoint: {}".format(spawn_point_param))
spawn_point = self.spawn_point_param.split(',')
if len(spawn_point) != 6:
raise ValueError("Invalid spawnpoint '{}'".format(
self.spawn_point_param))
pose = Pose()
pose.position.x = float(spawn_point[0])
pose.position.y = -float(spawn_point[1])
pose.position.z = float(spawn_point[2])
quat = quaternion_from_euler(math.radians(float(spawn_point[3])),
math.radians(float(spawn_point[4])),
math.radians(float(spawn_point[5])))
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
self.actor_spawnpoint = pose
self.initialpose_subscriber = Subscriber(
"/carla/{}/initialpose".format(self.role_name)
) # PoseWithCovarianceStamped, self.on_initialpose
self.posestamp = PoseWithCovarianceStamped()
#rospy.loginfo('listening to server %s:%s', self.host, self.port)
#rospy.loginfo('using vehicle filter: %s', self.actor_filter)
self.update_loop_thread = Thread(target=self.loop)
self.update_loop_thread.start()
def loop(self):
while True:
time.sleep(self._rate)
if self.initialpose_subscriber.getstate():
self.initialpose_subscriber.reset()
self.initialpose_subscriber.subscribe(self.posestamp)
self.on_initialpose(self.posestamp)
def on_initialpose(self, initial_pose):
"""
Callback for /initialpose
Receiving an initial pose (e.g. from RVIZ '2D Pose estimate') triggers a respawn.
:return:
"""
self.actor_spawnpoint = initial_pose.pose.pose
self.restart()
def restart(self):
"""
(Re)spawns the vehicle
Either at a given actor_spawnpoint or at a random Carla spawnpoint
:return:
"""
# Get vehicle blueprint.
blueprint = secure_random.choice(
self.world.get_blueprint_library().filter(self.actor_filter))
blueprint.set_attribute('role_name', "{}".format(self.role_name))
if blueprint.has_attribute('color'):
color = secure_random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
# Spawn the vehicle.
actors = self.world.get_actors().filter(self.actor_filter)
self.exist = False
for actor in actors:
if actor.attributes['role_name'] == self.role_name:
self.player = actor
self.exist = True
# print(actor.attributes['role_name'])
# print(self.role_name)
# print(actor.type_id)
break
if self.exist:
print(self.role_name + " exist")
else:
if self.actor_spawnpoint:
spawn_point = carla.Transform()
spawn_point.location.x = self.actor_spawnpoint.position.x
spawn_point.location.y = -self.actor_spawnpoint.position.y
spawn_point.location.z = self.actor_spawnpoint.position.z + \
2 # spawn 2m above ground
quaternion = (self.actor_spawnpoint.orientation.x,
self.actor_spawnpoint.orientation.y,
self.actor_spawnpoint.orientation.z,
self.actor_spawnpoint.orientation.w)
_, _, yaw = euler_from_quaternion(quaternion)
spawn_point.rotation.yaw = -math.degrees(yaw)
# rospy.loginfo("Spawn {} at x={} y={} z={} yaw={}".format(self.role_name,
# spawn_point.location.x,
# spawn_point.location.y,
# spawn_point.location.z,
# spawn_point.rotation.yaw))
if self.player is not None:
self.player.set_transform(spawn_point)
while self.player is None:
self.player = self.world.spawn_actor(
blueprint, spawn_point)
self.player_created = True
else:
if self.player is not None:
spawn_point = self.player.get_transform()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.player.set_transform(spawn_point)
while self.player is None:
spawn_point = random.choice(
self.world.get_map().get_spawn_points())
# spawn_points = self.world.get_map().get_spawn_points()
# spawn_point = secure_random.choice(
# spawn_points) if spawn_points else carla.Transform()
self.player = self.world.try_spawn_actor(
blueprint, spawn_point)
self.player.set_autopilot(True)
self.player_created = True
print("new vehicle generated")
#print(self.player.type_id)
if self.player_created:
# Read sensors from file
if not os.path.exists(self.sensor_definition_file):
raise RuntimeError(
"Could not read sensor-definition from {}".format(
self.sensor_definition_file))
json_sensors = None
with open(self.sensor_definition_file) as handle:
json_sensors = json.loads(handle.read())
# Set up the sensors
self.sensor_actors = self.setup_sensors(json_sensors["sensors"])
self.player_created = False
def setup_sensors(self, sensors):
"""
Create the sensors defined by the user and attach them to the ego-vehicle
:param sensors: list of sensors
:return:
"""
actors = []
bp_library = self.world.get_blueprint_library()
# bps=[bp for bp in bp_library.filter('sensor')]
# for bpsex in bps:
# print(" - {}".format(bpsex.id))
sensor_names = []
for sensor_spec in sensors:
try:
sensor_name = str(sensor_spec['type']) + "/" + str(
sensor_spec['id'])
if sensor_name in sensor_names:
print("sensor role is only allowed to be used once")
# rospy.logfatal(
# "Sensor rolename '{}' is only allowed to be used once.".format(
# sensor_spec['id']))
raise NameError(
"Sensor rolename '{}' is only allowed to be used once."
.format(sensor_spec['id']))
sensor_names.append(sensor_name)
bp = bp_library.find(str(sensor_spec['type']))
bp.set_attribute('role_name', str(sensor_spec['id']))
if sensor_spec['type'].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(sensor_spec['width']))
bp.set_attribute('image_size_y',
str(sensor_spec['height']))
bp.set_attribute('fov', str(sensor_spec['fov']))
try:
bp.set_attribute('sensor_tick',
str(sensor_spec['sensor_tick']))
except KeyError:
pass
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(
pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
if sensor_spec['type'].startswith('sensor.camera.rgb'):
bp.set_attribute('gamma', str(sensor_spec['gamma']))
#bp.set_attribute('shutter_speed', str(sensor_spec['shutter_speed']))
#bp.set_attribute('iso', str(sensor_spec['iso']))
#bp.set_attribute('fstop', str(sensor_spec['fstop']))
#bp.set_attribute('min_fstop', str(sensor_spec['min_fstop']))
#bp.set_attribute('blade_count', str(sensor_spec['blade_count']))
#bp.set_attribute('exposure_mode', str(sensor_spec['exposure_mode']))
#bp.set_attribute('exposure_compensation', str(
# sensor_spec['exposure_compensation']))
#bp.set_attribute('exposure_min_bright', str(
# sensor_spec['exposure_min_bright']))
# bp.set_attribute('exposure_max_bright', str(
# sensor_spec['exposure_max_bright']))
# bp.set_attribute('exposure_speed_up', str(sensor_spec['exposure_speed_up']))
# bp.set_attribute('exposure_speed_down', str(
# sensor_spec['exposure_speed_down']))
# bp.set_attribute('calibration_constant', str(
# sensor_spec['calibration_constant']))
# bp.set_attribute('focal_distance', str(sensor_spec['focal_distance']))
# bp.set_attribute('blur_amount', str(sensor_spec['blur_amount']))
# bp.set_attribute('blur_radius', str(sensor_spec['blur_radius']))
# bp.set_attribute('motion_blur_intensity', str(
# sensor_spec['motion_blur_intensity']))
# bp.set_attribute('motion_blur_max_distortion', str(
# sensor_spec['motion_blur_max_distortion']))
# bp.set_attribute('motion_blur_min_object_screen_size', str(
# sensor_spec['motion_blur_min_object_screen_size']))
# bp.set_attribute('slope', str(sensor_spec['slope']))
# bp.set_attribute('toe', str(sensor_spec['toe']))
# bp.set_attribute('shoulder', str(sensor_spec['shoulder']))
# bp.set_attribute('black_clip', str(sensor_spec['black_clip']))
# bp.set_attribute('white_clip', str(sensor_spec['white_clip']))
# bp.set_attribute('temp', str(sensor_spec['temp']))
# bp.set_attribute('tint', str(sensor_spec['tint']))
# bp.set_attribute('chromatic_aberration_intensity', str(
# sensor_spec['chromatic_aberration_intensity']))
# bp.set_attribute('chromatic_aberration_offset', str(
# sensor_spec['chromatic_aberration_offset']))
# bp.set_attribute('enable_postprocess_effects', str(
# sensor_spec['enable_postprocess_effects']))
# bp.set_attribute('lens_circle_falloff', str(
# sensor_spec['lens_circle_falloff']))
# bp.set_attribute('lens_circle_multiplier', str(
# sensor_spec['lens_circle_multiplier']))
# bp.set_attribute('lens_k', str(sensor_spec['lens_k']))
# bp.set_attribute('lens_kcube', str(sensor_spec['lens_kcube']))
# bp.set_attribute('lens_x_size', str(sensor_spec['lens_x_size']))
# bp.set_attribute('lens_y_size', str(sensor_spec['lens_y_size']))
elif sensor_spec['type'].startswith('sensor.lidar'):
# print(bp.id)
# for attr in bp:
# print(" - {}".format(attr))
bp.set_attribute('range', str(sensor_spec['range']))
bp.set_attribute('rotation_frequency',
str(sensor_spec['rotation_frequency']))
bp.set_attribute('channels', str(sensor_spec['channels']))
bp.set_attribute('upper_fov',
str(sensor_spec['upper_fov']))
bp.set_attribute('lower_fov',
str(sensor_spec['lower_fov']))
bp.set_attribute('points_per_second',
str(sensor_spec['points_per_second']))
try:
bp.set_attribute('sensor_tick',
str(sensor_spec['sensor_tick']))
except KeyError:
pass
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(
pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
elif sensor_spec['type'].startswith('sensor.other.gnss'):
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation()
# bp.set_attribute('noise_alt_stddev', str(sensor_spec['noise_alt_stddev']))
# bp.set_attribute('noise_lat_stddev', str(sensor_spec['noise_lat_stddev']))
# bp.set_attribute('noise_lon_stddev', str(sensor_spec['noise_lon_stddev']))
# bp.set_attribute('noise_alt_bias', str(sensor_spec['noise_alt_bias']))
# bp.set_attribute('noise_lat_bias', str(sensor_spec['noise_lat_bias']))
# bp.set_attribute('noise_lon_bias', str(sensor_spec['noise_lon_bias']))
elif sensor_spec['type'].startswith('sensor.other.imu'):
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(
pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
# print(bp.id)
# for attr in bp:
# print(" - {}".format(attr))
bp.set_attribute('noise_accel_stddev_x',
str(sensor_spec['noise_accel_stddev_x']))
bp.set_attribute('noise_accel_stddev_y',
str(sensor_spec['noise_accel_stddev_y']))
bp.set_attribute('noise_accel_stddev_z',
str(sensor_spec['noise_accel_stddev_z']))
bp.set_attribute('noise_gyro_stddev_x',
str(sensor_spec['noise_gyro_stddev_x']))
bp.set_attribute('noise_gyro_stddev_y',
str(sensor_spec['noise_gyro_stddev_y']))
bp.set_attribute('noise_gyro_stddev_z',
str(sensor_spec['noise_gyro_stddev_z']))
bp.set_attribute('noise_gyro_bias_x',
str(sensor_spec['noise_gyro_bias_x']))
bp.set_attribute('noise_gyro_bias_y',
str(sensor_spec['noise_gyro_bias_y']))
bp.set_attribute('noise_gyro_bias_z',
str(sensor_spec['noise_gyro_bias_z']))
elif sensor_spec['type'].startswith('sensor.other.radar'):
sensor_location = carla.Location(x=sensor_spec['x'],
y=sensor_spec['y'],
z=sensor_spec['z'])
sensor_rotation = carla.Rotation(
pitch=sensor_spec['pitch'],
roll=sensor_spec['roll'],
yaw=sensor_spec['yaw'])
bp.set_attribute('horizontal_fov',
str(sensor_spec['horizontal_fov']))
bp.set_attribute('vertical_fov',
str(sensor_spec['vertical_fov']))
bp.set_attribute('points_per_second',
str(sensor_spec['points_per_second']))
bp.set_attribute('range', str(sensor_spec['range']))
except KeyError as e:
# rospy.logfatal(
# "Sensor will not be spawned, because sensor spec is invalid: '{}'".format(e))
raise e
# create sensor
sensor_transform = carla.Transform(sensor_location,
sensor_rotation)
sensor = self.world.spawn_actor(bp,
sensor_transform,
attach_to=self.player)
actors.append(sensor)
return actors
@abstractmethod
def sensors(self):
"""
return a list of sensors attached
"""
return []
def destroy(self):
"""
destroy the current ego vehicle and its sensors
"""
for i, _ in enumerate(self.sensor_actors):
if self.sensor_actors[i] is not None:
self.sensor_actors[i].destroy()
self.sensor_actors[i] = None
self.sensor_actors = []
if self.player and self.player.is_alive:
self.player.destroy()
self.player = None
def run(self):
"""
main loop
"""
# wait for ros-bridge to set up CARLA world
#rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
#rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
sub1 = Subscriber("/carla/world_info")
worldinfo = CarlaWorldInfo()
sub1.enable()
sub1.subscribe(worldinfo)
client = carla.Client(host=self.host, port=self.port)
client.set_timeout(2.0)
self.world = client.get_world()
self.restart()
while True:
time.sleep(10)
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