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