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 _is_vehicle_hazard(self, ego_vehicle_pose, objects):
"""
Checks whether there is a vehicle hazard.
This method only takes into account vehicles. Pedestrians are other types of obstacles are
ignored.
:param ego_vehicle_pose: current ego vehicle pose
:type ego_vehicle_pose: geometry_msgs/Pose
:param objects: list of objects
:type objects: derived_object_msgs/ObjectArray
:return: a tuple given by (bool_flag, vehicle), where
- bool_flag is True if there is a vehicle ahead blocking us
and False otherwise
- vehicle is the blocker vehicle id
"""
ego_vehicle_waypoint = self.get_waypoint(ego_vehicle_pose.position)
ego_vehicle_transform = trans.ros_pose_to_carla_transform(
ego_vehicle_pose)
for target_vehicle_id, target_vehicle_object in objects.items():
# take into account only vehicles
if target_vehicle_object.classification not in self.OBJECT_VEHICLE_CLASSIFICATION:
continue
# do not account for the ego vehicle
if target_vehicle_id == self._ego_vehicle_id:
continue
target_vehicle_waypoint = self.get_waypoint(
target_vehicle_object.pose.position)
if target_vehicle_waypoint is None:
continue
target_vehicle_transform = trans.ros_pose_to_carla_transform(
target_vehicle_object.pose)
# if the object is not in our lane it's not an obstacle
if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
if is_within_distance_ahead(target_vehicle_transform,
ego_vehicle_transform,
self._proximity_vehicle_threshold):
return (True, target_vehicle_id)
return (False, None)
def _spawn_carla_actor(self, req):
"""
spawns an actor in carla
"""
if "*" in req.type:
blueprint = secure_random.choice(
self.blueprint_lib.filter(req.type))
else:
blueprint = self.blueprint_lib.find(req.type)
blueprint.set_attribute('role_name', req.id)
for attribute in req.attributes:
blueprint.set_attribute(attribute.key, attribute.value)
if req.random_pose is False:
transform = trans.ros_pose_to_carla_transform(req.transform)
else:
# get a random pose
transform = secure_random.choice(
self.spawn_points) if self.spawn_points else carla.Transform()
attach_to = None
if req.attach_to != 0:
attach_to = self.world.get_actor(req.attach_to)
if attach_to is None:
raise IndexError("Parent actor {} not found".format(
req.attach_to))
carla_actor = self.world.spawn_actor(blueprint, transform, attach_to)
return carla_actor.id
def _get_trafficlight_trigger_location(self, light_info): # pylint: disable=no-self-use
def rotate_point(point, radians):
"""
rotate a given point by a given angle
"""
rotated_x = math.cos(radians) * point.x - math.sin(
radians) * point.y
rotated_y = math.sin(radians) * point.x + math.cos(
radians) * point.y
return carla.Vector3D(rotated_x, rotated_y, point.z)
base_transform = trans.ros_pose_to_carla_transform(
light_info.transform)
base_rot = base_transform.rotation.yaw
area_loc = base_transform.transform(
trans.ros_point_to_carla_location(
light_info.trigger_volume.center))
area_ext = light_info.trigger_volume.size
point = rotate_point(carla.Vector3D(0, 0, area_ext.z / 2.0),
math.radians(base_rot))
point_location = area_loc + carla.Location(x=point.x, y=point.y)
return carla.Location(point_location.x, point_location.y,
point_location.z)
def _spawn_actor(self, req):
if "*" in req.type:
blueprint = secure_random.choice(
self.carla_world.get_blueprint_library().filter(req.type))
else:
blueprint = self.carla_world.get_blueprint_library().find(req.type)
blueprint.set_attribute('role_name', req.id)
for attribute in req.attributes:
blueprint.set_attribute(attribute.key, attribute.value)
if req.random_pose is False:
transform = trans.ros_pose_to_carla_transform(req.transform)
else:
# get a random pose
spawn_points = self.carla_world.get_map().get_spawn_points()
transform = secure_random.choice(
spawn_points) if spawn_points else carla.Transform()
attach_to = None
if req.attach_to != 0:
attach_to = self.carla_world.get_actor(req.attach_to)
if attach_to is None:
raise IndexError("Parent actor {} not found".format(req.attach_to))
carla_actor = self.carla_world.spawn_actor(blueprint, transform, attach_to)
actor = self.actor_factory.create(
req.type, req.id, req.attach_to, req.transform, carla_actor)
return actor.uid
def on_goal(self, goal):
"""
Callback for /move_base_simple/goal
Receiving a goal (e.g. from RVIZ '2D Nav Goal') triggers a new route calculation.
:return:
"""
self.loginfo("Received goal, trigger rerouting...")
carla_goal = trans.ros_pose_to_carla_transform(goal.pose)
self.goal = carla_goal
self.reroute()
def on_pose(self, pose):
if self.parent and self.parent.carla_actor.is_alive:
self.parent.carla_actor.set_transform(
trans.ros_pose_to_carla_transform(pose))
if isinstance(self.parent, Sensor):
self.parent.relative_spawn_pose = pose
def on_pose(self, pose):
if self.parent and self.parent.carla_actor.is_alive:
self.parent.carla_actor.set_transform(
trans.ros_pose_to_carla_transform(pose))