From 4265f4312dd5965c615743131038aece6140fe5f Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Wed, 24 Aug 2022 11:51:26 -0700 Subject: [PATCH 1/9] Update new submodule for carla_msgs --- .gitmodules | 4 ++-- carla_msgs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index 720c0ea7..97f5f726 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,4 @@ [submodule "carla_msgs"] path = carla_msgs - url = https://github.com/carla-simulator/ros-carla-msgs - branch = master + url = https://github.com/hieu-nguyen-tri/ros-carla-msgs + branch = hieu/add-ego-traffic-light-message diff --git a/carla_msgs b/carla_msgs index 081fdcdc..deb98692 160000 --- a/carla_msgs +++ b/carla_msgs @@ -1 +1 @@ -Subproject commit 081fdcdc8a4f8d4ce936b792c4ef13aba3fd7478 +Subproject commit deb98692122d7a79164c8a8684f3b899c58d30be From a42f5b7846f5a718a6024b225452ba7c98cfce67 Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Thu, 25 Aug 2022 16:46:51 -0700 Subject: [PATCH 2/9] Add ego traffic light sensor --- .../src/carla_ros_bridge/actor_factory.py | 13 +- .../ego_traffic_light_sensor.py | 158 ++++++++++++++++++ .../config/object_sensor_only.json | 8 +- 3 files changed, 176 insertions(+), 3 deletions(-) create mode 100644 carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py index a6963556..586d6e55 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py @@ -43,6 +43,7 @@ from carla_ros_bridge.tf_sensor import TFSensor from carla_ros_bridge.traffic import Traffic, TrafficLight from carla_ros_bridge.traffic_lights_sensor import TrafficLightsSensor +from carla_ros_bridge.ego_traffic_light_sensor import EgoTrafficLightSensor from carla_ros_bridge.vehicle import Vehicle from carla_ros_bridge.walker import Walker @@ -106,7 +107,7 @@ def update_available_objects(self): destroyed_actors = self._active_actors - current_actors self._active_actors = current_actors - # Create/destroy actors not managed by the bridge. + # Create/destroy actors not managed by the bridge. self.lock.acquire() for actor_id in spawned_actors: carla_actor = self.world.get_actor(actor_id) @@ -351,6 +352,16 @@ def _create_object(self, uid, type_id, name, attach_to, spawn_pose, carla_actor= parent=parent, node=self.node) + elif type_id == EgoTrafficLightSensor.get_blueprint_name(): + actor = EgoTrafficLightSensor( + uid=uid, + name=name, + parent=parent, + node=self.node, + actor_list=self.actors, + world=self.world + ) + elif carla_actor.type_id.startswith('traffic'): if carla_actor.type_id == "traffic.traffic_light": actor = TrafficLight(uid, name, parent, self.node, carla_actor) diff --git a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py new file mode 100644 index 00000000..d23d65f6 --- /dev/null +++ b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python + +from collections import defaultdict +from ros_compatibility.qos import QoSProfile, DurabilityPolicy + +import carla + +from carla_msgs.msg import CarlaEgoTrafficLightInfo, CarlaTrafficLightStatus +from carla_ros_bridge.ego_vehicle import EgoVehicle +from carla_ros_bridge.pseudo_actor import PseudoActor +from carla_ros_bridge.traffic import TrafficLight + + +# Publish info no less frequently than this. +_PUBLISH_INTERVAL_SECONDS = 5.0 + +def get_stop_line_location(carla_traffic_light): + """Take average of all affected waypoints to get the stop location. + Note: carla has a get_stop_waypoints() but it doesn't seem to return correct location. + """ + stop_location = carla.Location() + points = carla_traffic_light.get_affected_lane_waypoints() + for point in points: + stop_location += point.transform.location + return stop_location / len(points) + +def group_traffic_lights(node, actor_list, previous_road_search_distance=2.0): + results = defaultdict(dict) + for actor in actor_list.values(): + if isinstance(actor, TrafficLight): + carla_actor = actor.carla_actor + affected_points = carla_actor.get_affected_lane_waypoints() + if len(affected_points) == 0: + node.logerr("Unable to find any affected points at") + continue + + for point in affected_points: + if not point.is_junction: + node.logerr(f"The affected waypoint from traffic light is not in a junction. traffic_light_id: {carla_actor.id}, road_id: {point.road_id}, lane_id: {point.lane_id}") + continue + stop_location = point.transform.location + previous_points = point.previous(previous_road_search_distance) + if len(previous_points) == 0: + node.logerr(f"Unable to find previous points for traffic_light_id: {carla_actor.id}, road_id: {point.road_id}, lane_id: {point.lane_id}") + continue + p = previous_points[0] + results[p.road_id][p.lane_id] = StoplineInfo(actor, p.road_id, p.lane_id, stop_location) + return results + +def get_stop_line_info(stop_line_info, road_id, lane_id): + if stop_line_info is None: + return None + if road_id in stop_line_info and lane_id in stop_line_info[road_id]: + return stop_line_info[road_id][lane_id] + return None + + +class StoplineInfo: + def __init__(self, traffic_light_actor, road_id, lane_id, stop_location): + self.traffic_light_actor = traffic_light_actor + self.road_id = road_id + self.lane_id = lane_id + self.stop_location = stop_location + + +class EgoTrafficLightSensor(PseudoActor): + + def __init__(self, uid, name, parent, node, actor_list, world): + super(EgoTrafficLightSensor, self).__init__(uid=uid, name=name, parent=parent, node=node) + + self.pub = node.new_publisher( + CarlaEgoTrafficLightInfo, + self.get_topic_prefix() + "/info", + qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL), + ) + + self._info_published_at = None + + self.map = world.get_map() + self.map_name = self.map.name + self.ego = self.get_ego(actor_list) + self.actor_list = actor_list + self.stop_line_info_map = None + self.cur_sli = None + + self.msg = CarlaEgoTrafficLightInfo() + self.msg.traffic_light_status = CarlaTrafficLightStatus() + self.msg.inside_intersection = False + if self.ego: + self.msg.ego_id = self.ego.id + + def destroy(self): + super(EgoTrafficLightSensor, self).destroy() + self.node.destroy_publisher(self.pub) + self.actor_list = None + self.stop_line_info_map = None + self.node.loginfo("Destroy EgoTrafficLightSensor") + + @staticmethod + def get_blueprint_name(): + return "sensor.pseudo.ego_traffic_light" + + def update(self, frame, timestamp): + if self.ego is None or self.map is None: + return + + if self.stop_line_info_map is None or self.map_name != self.map.name: + self.stop_line_info_map = group_traffic_lights(self.node, self.actor_list) + self.map_name = self.map.name + + ego_location = self.ego.get_location() + wp = self.map.get_waypoint(ego_location) + sli = get_stop_line_info(self.stop_line_info_map, wp.road_id, wp.lane_id) + + if self.has_change(self.cur_sli, sli) \ + or (self._info_published_at is None or timestamp - self._info_published_at > _PUBLISH_INTERVAL_SECONDS): + self.calculate_and_publish_data(ego_location, sli) + + def calculate_and_publish_data(self, ego_location, stop_line_info): + self.cur_sli = stop_line_info + try: + if self.cur_sli: + self.msg.distance_to_stopline = ego_location.distance(self.cur_sli.stop_location) + self.msg.traffic_light_status = self.cur_sli.traffic_light_actor.get_status() + else: + self.msg.distance_to_stopline = -1. + self.msg.traffic_light_status = None + except Exception as e: + self.node.loginfo("Error: {}".format(e)) + + self.msg.inside_intersection = self.is_inside_intersection(self.ego, self.map) + self.pub.publish(self.msg) + + def get_ego(self, actor_list): + """Return a CarlaActor representing the ego car.""" + for actor in actor_list.values(): + if isinstance(actor, EgoVehicle): + return actor.carla_actor + return None + + def is_inside_intersection(self, carla_ego, carla_map): + if carla_ego is None: + return False + waypoint = carla_map.get_waypoint(carla_ego.get_location()) + if waypoint is None: + return False + return waypoint.is_junction + + def has_change(self, cur_sli, new_sli): + if cur_sli is None and new_sli is None: + return False + + if cur_sli != new_sli: + return True + + same_status = cur_sli.traffic_light_actor.get_status() == new_sli.traffic_light_actor.get_status() + return not same_status + diff --git a/carla_spawn_objects/config/object_sensor_only.json b/carla_spawn_objects/config/object_sensor_only.json index 432705ff..46960d3b 100644 --- a/carla_spawn_objects/config/object_sensor_only.json +++ b/carla_spawn_objects/config/object_sensor_only.json @@ -1,5 +1,5 @@ -{ - "objects": +{ + "objects": [ { "type": "sensor.pseudo.objects", @@ -12,6 +12,10 @@ { "type": "sensor.pseudo.traffic_lights", "id": "traffic_lights" + }, + { + "type": "sensor.pseudo.ego_traffic_light", + "id": "ego_traffic_light" } ] } From 396422d7408e9185171048fe905aa77b58885648 Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Thu, 25 Aug 2022 18:36:06 -0700 Subject: [PATCH 3/9] Add comments and clean up --- .../src/carla_ros_bridge/actor_factory.py | 2 +- .../ego_traffic_light_sensor.py | 174 ++++++++++-------- 2 files changed, 96 insertions(+), 80 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py index 586d6e55..e9054899 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py @@ -359,7 +359,7 @@ def _create_object(self, uid, type_id, name, attach_to, spawn_pose, carla_actor= parent=parent, node=self.node, actor_list=self.actors, - world=self.world + carla_map=self.world.get_map() ) elif carla_actor.type_id.startswith('traffic'): diff --git a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py index d23d65f6..bb89818b 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py +++ b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py @@ -1,71 +1,88 @@ #!/usr/bin/env python from collections import defaultdict +from typing import Dict, List from ros_compatibility.qos import QoSProfile, DurabilityPolicy import carla from carla_msgs.msg import CarlaEgoTrafficLightInfo, CarlaTrafficLightStatus +from carla_ros_bridge.actor import Actor from carla_ros_bridge.ego_vehicle import EgoVehicle from carla_ros_bridge.pseudo_actor import PseudoActor from carla_ros_bridge.traffic import TrafficLight - +from ros_compatibility.node import CompatibleNode # Publish info no less frequently than this. _PUBLISH_INTERVAL_SECONDS = 5.0 -def get_stop_line_location(carla_traffic_light): - """Take average of all affected waypoints to get the stop location. - Note: carla has a get_stop_waypoints() but it doesn't seem to return correct location. - """ - stop_location = carla.Location() - points = carla_traffic_light.get_affected_lane_waypoints() - for point in points: - stop_location += point.transform.location - return stop_location / len(points) - -def group_traffic_lights(node, actor_list, previous_road_search_distance=2.0): +class StoplineInfo: + """An util class to keep track of a traffic light and its stop location.""" + + def __init__(self, traffic_light_actor: TrafficLight, approaching_road_id: int, approaching_lane_id: int, stop_location: carla.Location): + self.traffic_light_actor = traffic_light_actor + self.stop_location = stop_location + self.approaching_road_id = approaching_road_id + self.approaching_lane_id = approaching_lane_id + + +def create_stop_line_info_map(node: CompatibleNode, actor_list: Dict[int, Actor], search_distance: float = 2.0): + """ Return a dictionary of stop line info. + + Args: + node: ROS node wrapper for logging purposes + actor_list: A dictionary of spawn actors. Use this to search for traffic light actors + search_distance: the approximate distance where to get the previous waypoints + Return: + A map(road_id, lane_id -> StopLineInfo) + """ results = defaultdict(dict) for actor in actor_list.values(): - if isinstance(actor, TrafficLight): - carla_actor = actor.carla_actor - affected_points = carla_actor.get_affected_lane_waypoints() - if len(affected_points) == 0: - node.logerr("Unable to find any affected points at") + if not isinstance(actor, TrafficLight): + continue + + carla_actor = actor.carla_actor + affected_points = carla_actor.get_affected_lane_waypoints() + if len(affected_points) == 0: + node.logerr(f"Unable to find any affected points for traffic light id: {carla_actor.id}") + continue + + for point in affected_points: + if not point.is_junction: + node.logerr( + f"The affected waypoint from traffic light is not in a junction. Traffic_light_id: {carla_actor.id}, road_id: {point.road_id}, lane_id: {point.lane_id}" + ) + continue + + # Go to previous waypoints to get the road id and lane id on which the ego will be approaching. + previous_points = point.previous(search_distance) + if len(previous_points) == 0: + node.logerr( + f"Unable to find previous points for traffic_light_id: {carla_actor.id}, road_id: {point.road_id}, lane_id: {point.lane_id}" + ) continue - for point in affected_points: - if not point.is_junction: - node.logerr(f"The affected waypoint from traffic light is not in a junction. traffic_light_id: {carla_actor.id}, road_id: {point.road_id}, lane_id: {point.lane_id}") - continue - stop_location = point.transform.location - previous_points = point.previous(previous_road_search_distance) - if len(previous_points) == 0: - node.logerr(f"Unable to find previous points for traffic_light_id: {carla_actor.id}, road_id: {point.road_id}, lane_id: {point.lane_id}") - continue - p = previous_points[0] - results[p.road_id][p.lane_id] = StoplineInfo(actor, p.road_id, p.lane_id, stop_location) + # This stop location is roughly at the stop line. It depends on how the map is created in RoadRunner. + stop_location = point.transform.location + p = previous_points[0] + results[p.road_id][p.lane_id] = StoplineInfo(actor, p.road_id, p.lane_id, stop_location) + return results -def get_stop_line_info(stop_line_info, road_id, lane_id): - if stop_line_info is None: + +def get_stop_line_info(stop_line_infos: Dict[int,Dict[int, StoplineInfo]], road_id: int, lane_id: int): + """Given road_id and lane_id, find and return a StopLineInfo object if any.""" + if stop_line_infos is None: return None - if road_id in stop_line_info and lane_id in stop_line_info[road_id]: - return stop_line_info[road_id][lane_id] + if road_id in stop_line_infos and lane_id in stop_line_infos[road_id]: + return stop_line_infos[road_id][lane_id] return None -class StoplineInfo: - def __init__(self, traffic_light_actor, road_id, lane_id, stop_location): - self.traffic_light_actor = traffic_light_actor - self.road_id = road_id - self.lane_id = lane_id - self.stop_location = stop_location - - class EgoTrafficLightSensor(PseudoActor): + """A sensor to publish CarlaEgoTrafficLightInfo""" - def __init__(self, uid, name, parent, node, actor_list, world): + def __init__(self, uid, name, parent, node, actor_list, carla_map): super(EgoTrafficLightSensor, self).__init__(uid=uid, name=name, parent=parent, node=node) self.pub = node.new_publisher( @@ -76,7 +93,7 @@ def __init__(self, uid, name, parent, node, actor_list, world): self._info_published_at = None - self.map = world.get_map() + self.map = carla_map self.map_name = self.map.name self.ego = self.get_ego(actor_list) self.actor_list = actor_list @@ -101,58 +118,57 @@ def get_blueprint_name(): return "sensor.pseudo.ego_traffic_light" def update(self, frame, timestamp): - if self.ego is None or self.map is None: - return - - if self.stop_line_info_map is None or self.map_name != self.map.name: - self.stop_line_info_map = group_traffic_lights(self.node, self.actor_list) - self.map_name = self.map.name - - ego_location = self.ego.get_location() - wp = self.map.get_waypoint(ego_location) - sli = get_stop_line_info(self.stop_line_info_map, wp.road_id, wp.lane_id) - - if self.has_change(self.cur_sli, sli) \ - or (self._info_published_at is None or timestamp - self._info_published_at > _PUBLISH_INTERVAL_SECONDS): - self.calculate_and_publish_data(ego_location, sli) - - def calculate_and_publish_data(self, ego_location, stop_line_info): - self.cur_sli = stop_line_info try: - if self.cur_sli: - self.msg.distance_to_stopline = ego_location.distance(self.cur_sli.stop_location) - self.msg.traffic_light_status = self.cur_sli.traffic_light_actor.get_status() - else: - self.msg.distance_to_stopline = -1. - self.msg.traffic_light_status = None + if self.ego is None or self.map is None: + return + + if self.stop_line_info_map is None or self.map_name != self.map.name: + self.stop_line_info_map = create_stop_line_info_map(self.node, self.actor_list) + self.map_name = self.map.name + + ego_location = self.ego.get_location() + wp = self.map.get_waypoint(ego_location) + sli = get_stop_line_info(self.stop_line_info_map, wp.road_id, wp.lane_id) + + inside_intersection = wp.is_junction + + if (self.msg.inside_intersection != inside_intersection + or self.has_traffic_light_status_changes(self.cur_sli, sli) + or self._info_published_at is None + or timestamp - self._info_published_at > _PUBLISH_INTERVAL_SECONDS + ): + self.cur_sli = sli + self.calculate_and_publish_data(ego_location, self.cur_sli, inside_intersection) + self._info_published_at = timestamp except Exception as e: self.node.loginfo("Error: {}".format(e)) - self.msg.inside_intersection = self.is_inside_intersection(self.ego, self.map) + def calculate_and_publish_data(self, ego_location: carla.Location, stop_line_info: StoplineInfo, inside_intersection: bool): + """Publish CarlaEgoTrafficLightInfo message.""" + if stop_line_info: + self.msg.distance_to_stopline = ego_location.distance(stop_line_info.stop_location) + self.msg.traffic_light_status = stop_line_info.traffic_light_actor.get_status() + else: + self.msg.distance_to_stopline = -1.0 + self.msg.traffic_light_status = CarlaTrafficLightStatus() + self.msg.inside_intersection = inside_intersection self.pub.publish(self.msg) - def get_ego(self, actor_list): + def get_ego(self, actor_list: Dict[int, Actor]): """Return a CarlaActor representing the ego car.""" for actor in actor_list.values(): if isinstance(actor, EgoVehicle): return actor.carla_actor return None - def is_inside_intersection(self, carla_ego, carla_map): - if carla_ego is None: - return False - waypoint = carla_map.get_waypoint(carla_ego.get_location()) - if waypoint is None: - return False - return waypoint.is_junction - - def has_change(self, cur_sli, new_sli): + def has_traffic_light_status_changes(self, cur_sli: StoplineInfo, new_sli: StoplineInfo): + """Return True if the StopLineInfo object has been changed or the traffic light status has been changed; False otherwise.""" if cur_sli is None and new_sli is None: return False if cur_sli != new_sli: return True - same_status = cur_sli.traffic_light_actor.get_status() == new_sli.traffic_light_actor.get_status() - return not same_status - + cur_status = cur_sli.traffic_light_actor.get_status() + new_status = new_sli.traffic_light_actor.get_status() + return cur_status != new_status From 532414097fb49f6fc12e6fd61d55e5864cf1478d Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Thu, 25 Aug 2022 18:55:16 -0700 Subject: [PATCH 4/9] Keep traffic light id when inside intersection --- .../ego_traffic_light_sensor.py | 30 +++++++++++-------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py index bb89818b..67520998 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py +++ b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py @@ -137,20 +137,27 @@ def update(self, frame, timestamp): or self._info_published_at is None or timestamp - self._info_published_at > _PUBLISH_INTERVAL_SECONDS ): - self.cur_sli = sli - self.calculate_and_publish_data(ego_location, self.cur_sli, inside_intersection) + self.calculate_and_publish_data(ego_location, sli, inside_intersection) self._info_published_at = timestamp except Exception as e: self.node.loginfo("Error: {}".format(e)) - def calculate_and_publish_data(self, ego_location: carla.Location, stop_line_info: StoplineInfo, inside_intersection: bool): + def calculate_and_publish_data(self, ego_location: carla.Location, new_sli: StoplineInfo, inside_intersection: bool): """Publish CarlaEgoTrafficLightInfo message.""" - if stop_line_info: - self.msg.distance_to_stopline = ego_location.distance(stop_line_info.stop_location) - self.msg.traffic_light_status = stop_line_info.traffic_light_actor.get_status() - else: + if inside_intersection: self.msg.distance_to_stopline = -1.0 - self.msg.traffic_light_status = CarlaTrafficLightStatus() + if self.cur_sli: + self.msg.traffic_light_status = self.cur_sli.traffic_light_actor.get_status() + else: + if new_sli: + self.msg.distance_to_stopline = ego_location.distance(new_sli.stop_location) + self.msg.traffic_light_status = new_sli.traffic_light_actor.get_status() + else: + self.msg.distance_to_stopline = -1.0 + self.msg.traffic_light_status = CarlaTrafficLightStatus() + # Store new StopLineInfo. + self.cur_sli = new_sli + self.msg.inside_intersection = inside_intersection self.pub.publish(self.msg) @@ -162,13 +169,10 @@ def get_ego(self, actor_list: Dict[int, Actor]): return None def has_traffic_light_status_changes(self, cur_sli: StoplineInfo, new_sli: StoplineInfo): - """Return True if the StopLineInfo object has been changed or the traffic light status has been changed; False otherwise.""" - if cur_sli is None and new_sli is None: + """Return True if the traffic light status (either new traffic light id or new state) has been changed; False otherwise.""" + if cur_sli is None or new_sli is None: return False - if cur_sli != new_sli: - return True - cur_status = cur_sli.traffic_light_actor.get_status() new_status = new_sli.traffic_light_actor.get_status() return cur_status != new_status From c65090d27ef604e211a88a431d4631d1832fd5fb Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Mon, 29 Aug 2022 14:54:08 -0700 Subject: [PATCH 5/9] Sort import specify args in constructor --- carla_ros_bridge/src/carla_ros_bridge/actor_factory.py | 2 +- .../src/carla_ros_bridge/ego_traffic_light_sensor.py | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py index e9054899..0e11ee9a 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py @@ -25,6 +25,7 @@ from carla_ros_bridge.actor_list_sensor import ActorListSensor from carla_ros_bridge.camera import Camera, RgbCamera, DepthCamera, SemanticSegmentationCamera, DVSCamera from carla_ros_bridge.collision_sensor import CollisionSensor +from carla_ros_bridge.ego_traffic_light_sensor import EgoTrafficLightSensor from carla_ros_bridge.ego_vehicle import EgoVehicle from carla_ros_bridge.gnss import Gnss from carla_ros_bridge.imu import ImuSensor @@ -43,7 +44,6 @@ from carla_ros_bridge.tf_sensor import TFSensor from carla_ros_bridge.traffic import Traffic, TrafficLight from carla_ros_bridge.traffic_lights_sensor import TrafficLightsSensor -from carla_ros_bridge.ego_traffic_light_sensor import EgoTrafficLightSensor from carla_ros_bridge.vehicle import Vehicle from carla_ros_bridge.walker import Walker diff --git a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py index 67520998..8f41dfa5 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py +++ b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py @@ -1,17 +1,17 @@ #!/usr/bin/env python from collections import defaultdict -from typing import Dict, List -from ros_compatibility.qos import QoSProfile, DurabilityPolicy +from typing import Dict import carla - from carla_msgs.msg import CarlaEgoTrafficLightInfo, CarlaTrafficLightStatus +from ros_compatibility.node import CompatibleNode +from ros_compatibility.qos import DurabilityPolicy, QoSProfile + from carla_ros_bridge.actor import Actor from carla_ros_bridge.ego_vehicle import EgoVehicle from carla_ros_bridge.pseudo_actor import PseudoActor from carla_ros_bridge.traffic import TrafficLight -from ros_compatibility.node import CompatibleNode # Publish info no less frequently than this. _PUBLISH_INTERVAL_SECONDS = 5.0 @@ -82,7 +82,7 @@ def get_stop_line_info(stop_line_infos: Dict[int,Dict[int, StoplineInfo]], road_ class EgoTrafficLightSensor(PseudoActor): """A sensor to publish CarlaEgoTrafficLightInfo""" - def __init__(self, uid, name, parent, node, actor_list, carla_map): + def __init__(self, uid: int, name: str, parent: Actor, node: CompatibleNode, actor_list: Dict[int, Actor], carla_map: carla.Map): super(EgoTrafficLightSensor, self).__init__(uid=uid, name=name, parent=parent, node=node) self.pub = node.new_publisher( From b2ff295a4fdb7cebfd7e36435f965da91b0fec63 Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Tue, 30 Aug 2022 21:49:51 -0700 Subject: [PATCH 6/9] Update submodule to use TRI repo --- .gitmodules | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 97f5f726..1f52d7b1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,4 @@ [submodule "carla_msgs"] path = carla_msgs - url = https://github.com/hieu-nguyen-tri/ros-carla-msgs - branch = hieu/add-ego-traffic-light-message + url = https://github.com/ToyotaResearchInstitute/ros-carla-msgs.git + branch = master From 58cf82a489bf8b5a07c826fe5e2924ff2f3c4280 Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Tue, 30 Aug 2022 21:49:51 -0700 Subject: [PATCH 7/9] Update submodule to use TRI repo --- .gitmodules | 4 ++-- carla_msgs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index 97f5f726..1f52d7b1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,4 @@ [submodule "carla_msgs"] path = carla_msgs - url = https://github.com/hieu-nguyen-tri/ros-carla-msgs - branch = hieu/add-ego-traffic-light-message + url = https://github.com/ToyotaResearchInstitute/ros-carla-msgs.git + branch = master diff --git a/carla_msgs b/carla_msgs index deb98692..61bfc947 160000 --- a/carla_msgs +++ b/carla_msgs @@ -1 +1 @@ -Subproject commit deb98692122d7a79164c8a8684f3b899c58d30be +Subproject commit 61bfc9477715568a911e519c64cb472afbf98430 From 69ba3fea02bf9a7fa3e29fae248fdf3754a918ac Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Tue, 30 Aug 2022 21:49:51 -0700 Subject: [PATCH 8/9] Update submodule to use TRI repo --- .gitmodules | 4 ++-- carla_msgs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index 97f5f726..1f52d7b1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,4 @@ [submodule "carla_msgs"] path = carla_msgs - url = https://github.com/hieu-nguyen-tri/ros-carla-msgs - branch = hieu/add-ego-traffic-light-message + url = https://github.com/ToyotaResearchInstitute/ros-carla-msgs.git + branch = master diff --git a/carla_msgs b/carla_msgs index deb98692..61bfc947 160000 --- a/carla_msgs +++ b/carla_msgs @@ -1 +1 @@ -Subproject commit deb98692122d7a79164c8a8684f3b899c58d30be +Subproject commit 61bfc9477715568a911e519c64cb472afbf98430 From 3e97711f61965481b0097d65bc597d44a6b58785 Mon Sep 17 00:00:00 2001 From: Hieu Nguyen Date: Tue, 30 Aug 2022 23:03:18 -0700 Subject: [PATCH 9/9] Address feedback --- .../ego_traffic_light_sensor.py | 43 +++++++++++-------- 1 file changed, 25 insertions(+), 18 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py index 8f41dfa5..ec5550ff 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py +++ b/carla_ros_bridge/src/carla_ros_bridge/ego_traffic_light_sensor.py @@ -70,12 +70,12 @@ def create_stop_line_info_map(node: CompatibleNode, actor_list: Dict[int, Actor] return results -def get_stop_line_info(stop_line_infos: Dict[int,Dict[int, StoplineInfo]], road_id: int, lane_id: int): +def get_stop_line_info(stop_line_info: Dict[int,Dict[int, StoplineInfo]], road_id: int, lane_id: int): """Given road_id and lane_id, find and return a StopLineInfo object if any.""" - if stop_line_infos is None: + if stop_line_info is None: return None - if road_id in stop_line_infos and lane_id in stop_line_infos[road_id]: - return stop_line_infos[road_id][lane_id] + if road_id in stop_line_info and lane_id in stop_line_info[road_id]: + return stop_line_info[road_id][lane_id] return None @@ -95,7 +95,7 @@ def __init__(self, uid: int, name: str, parent: Actor, node: CompatibleNode, act self.map = carla_map self.map_name = self.map.name - self.ego = self.get_ego(actor_list) + self.ego = None self.actor_list = actor_list self.stop_line_info_map = None self.cur_sli = None @@ -103,8 +103,6 @@ def __init__(self, uid: int, name: str, parent: Actor, node: CompatibleNode, act self.msg = CarlaEgoTrafficLightInfo() self.msg.traffic_light_status = CarlaTrafficLightStatus() self.msg.inside_intersection = False - if self.ego: - self.msg.ego_id = self.ego.id def destroy(self): super(EgoTrafficLightSensor, self).destroy() @@ -117,32 +115,40 @@ def destroy(self): def get_blueprint_name(): return "sensor.pseudo.ego_traffic_light" - def update(self, frame, timestamp): + def update(self, frame: int, timestamp: float): try: - if self.ego is None or self.map is None: + if self.map is None: + self.node.logwarn("Carla Map is not assgined") return if self.stop_line_info_map is None or self.map_name != self.map.name: self.stop_line_info_map = create_stop_line_info_map(self.node, self.actor_list) self.map_name = self.map.name + self.ego = self.get_ego(self.actor_list) + + if self.ego is None: + self.node.logwarn("Unable to find ego.") + return ego_location = self.ego.get_location() wp = self.map.get_waypoint(ego_location) sli = get_stop_line_info(self.stop_line_info_map, wp.road_id, wp.lane_id) inside_intersection = wp.is_junction + cur_status = self.cur_sli.traffic_light_actor.get_status() if self.cur_sli else None + new_status = sli.traffic_light_actor.get_status() if sli else None if (self.msg.inside_intersection != inside_intersection - or self.has_traffic_light_status_changes(self.cur_sli, sli) + or self.has_traffic_light_status_changes(cur_status, new_status) or self._info_published_at is None or timestamp - self._info_published_at > _PUBLISH_INTERVAL_SECONDS ): - self.calculate_and_publish_data(ego_location, sli, inside_intersection) + self.calculate_and_publish_data(ego_location, sli, inside_intersection, timestamp) self._info_published_at = timestamp except Exception as e: self.node.loginfo("Error: {}".format(e)) - def calculate_and_publish_data(self, ego_location: carla.Location, new_sli: StoplineInfo, inside_intersection: bool): + def calculate_and_publish_data(self, ego_location: carla.Location, new_sli: StoplineInfo, inside_intersection: bool, timestamp: float): """Publish CarlaEgoTrafficLightInfo message.""" if inside_intersection: self.msg.distance_to_stopline = -1.0 @@ -158,7 +164,9 @@ def calculate_and_publish_data(self, ego_location: carla.Location, new_sli: Stop # Store new StopLineInfo. self.cur_sli = new_sli + self.msg.ego_id = self.ego.id self.msg.inside_intersection = inside_intersection + self.msg.header = self.get_msg_header(timestamp=timestamp) self.pub.publish(self.msg) def get_ego(self, actor_list: Dict[int, Actor]): @@ -168,11 +176,10 @@ def get_ego(self, actor_list: Dict[int, Actor]): return actor.carla_actor return None - def has_traffic_light_status_changes(self, cur_sli: StoplineInfo, new_sli: StoplineInfo): - """Return True if the traffic light status (either new traffic light id or new state) has been changed; False otherwise.""" - if cur_sli is None or new_sli is None: + def has_traffic_light_status_changes(self, cur_traffic_light_status: CarlaTrafficLightStatus, new_traffic_light_status: CarlaTrafficLightStatus): + """Return True if the traffic light status (either traffic light id or state) has been changed; False otherwise.""" + if cur_traffic_light_status is None or new_traffic_light_status is None: + # Don't detect changes when one of the status is None. return False - cur_status = cur_sli.traffic_light_actor.get_status() - new_status = new_sli.traffic_light_actor.get_status() - return cur_status != new_status + return cur_traffic_light_status != new_traffic_light_status