def _is_light_red(self, ego_vehicle_pose, lights_status, lights_info):
"""
Checks if there is a red light affecting us. This version of the method is compatible with
both European and US style traffic lights.
:param ego_vehicle_pose: current ego vehicle pose
:type ego_vehicle_pose: geometry_msgs/Pose
:param lights_status: list containing all traffic light status.
:type lights_status: carla_msgs/CarlaTrafficLightStatusList
:param lights_info: list containing all traffic light info.
:type lights_info: lis.
:return: a tuple given by (bool_flag, traffic_light), where
- bool_flag is True if there is a traffic light in RED
affecting us and False otherwise
- traffic_light is the traffic light id or None if there is no
red traffic light affecting us.
"""
ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_pose.position)
ego_vehicle_transform = trans.ros_pose_to_carla_transform(
ego_vehicle_pose)
for light_id in lights_status.keys():
object_location = self._get_trafficlight_trigger_location(
lights_info[light_id])
object_location = trans.carla_location_to_ros_point(
object_location)
object_waypoint = self.get_waypoint(object_location)
if object_waypoint is None:
continue
object_transform = trans.ros_pose_to_carla_transform(
object_waypoint.pose)
if object_waypoint.road_id != ego_vehicle_waypoint.road_id:
continue
ve_dir = ego_vehicle_transform.get_forward_vector()
wp_dir = object_transform.get_forward_vector()
dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z
if dot_ve_wp < 0:
continue
if is_within_distance_ahead(object_transform,
ego_vehicle_transform,
self._proximity_tlight_threshold):
if lights_status[light_id].state == CarlaTrafficLightStatus.RED or \
lights_status[light_id].state == CarlaTrafficLightStatus.YELLOW:
return (True, light_id)
return (False, None)
def get_info(self):
"""
Get the info of the traffic light
"""
info = CarlaTrafficLightInfo()
info.id = self.get_id()
info.transform = self.get_current_ros_pose()
info.trigger_volume.center = trans.carla_location_to_ros_point(
self.carla_actor.trigger_volume.location)
info.trigger_volume.size.x = self.carla_actor.trigger_volume.extent.x * 2.0
info.trigger_volume.size.y = self.carla_actor.trigger_volume.extent.y * 2.0
info.trigger_volume.size.z = self.carla_actor.trigger_volume.extent.z * 2.0
return info
def _get_marker_from_environment_object(self, environment_object):
marker = Marker(header=self.get_msg_header(frame_id="map"))
marker.ns = str(environment_object.type)
marker.id = next(self.static_id_gen)
box = environment_object.bounding_box
marker.pose.position = carla_location_to_ros_point(box.location)
marker.pose.orientation = carla_rotation_to_ros_quaternion(
box.rotation)
marker.scale.x = max(0.1, 2 * box.extent.x)
marker.scale.y = max(0.1, 2 * box.extent.y)
marker.scale.z = max(0.1, 2 * box.extent.z)
marker.type = Marker.CUBE
marker.color = OBJECT_LABELS.get(environment_object.type,
ColorRGBA(r=1.0, g=0.0, b=0.0, a=0.8))
return marker