def check_traffic_light_infraction(self):
transform = self._car_agent.get_transform()
location = transform.location
veh_extent = self._car_agent.bounding_box.extent.x
tail_close_pt = self.rotate_point(
carla.Vector3D(-0.8 * veh_extent, 0.0, location.z),
transform.rotation.yaw)
tail_close_pt = location + carla.Location(tail_close_pt)
tail_far_pt = self.rotate_point(
carla.Vector3D(-veh_extent - 1, 0.0, location.z),
transform.rotation.yaw)
tail_far_pt = location + carla.Location(tail_far_pt)
for traffic_light, center, waypoints in self._list_traffic_lights:
center_loc = carla.Location(center)
if self._last_red_light_id and self._last_red_light_id == traffic_light.id:
continue
if center_loc.distance(location) > self.DISTANCE_LIGHT:
continue
if traffic_light.state != carla.TrafficLightState.Red:
continue
for wp in waypoints:
tail_wp = self._map.get_waypoint(tail_far_pt)
# Calculate the dot product (Might be unscaled, as only its sign is important)
ve_dir = self._car_agent.get_transform().get_forward_vector()
wp_dir = wp.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
# Check the lane until all the "tail" has passed
if tail_wp.road_id == wp.road_id and tail_wp.lane_id == wp.lane_id and dot_ve_wp > 0:
# This light is red and is affecting our lane
yaw_wp = wp.transform.rotation.yaw
lane_width = wp.lane_width
location_wp = wp.transform.location
lft_lane_wp = self.rotate_point(
carla.Vector3D(0.4 * lane_width, 0.0, location_wp.z),
yaw_wp + 90)
lft_lane_wp = location_wp + carla.Location(lft_lane_wp)
rgt_lane_wp = self.rotate_point(
carla.Vector3D(0.4 * lane_width, 0.0, location_wp.z),
yaw_wp - 90)
rgt_lane_wp = location_wp + carla.Location(rgt_lane_wp)
# Is the vehicle traversing the stop line?
if self.is_vehicle_crossing_line(
(tail_close_pt, tail_far_pt),
(lft_lane_wp, rgt_lane_wp)):
self.light_actual_value += 1
# location = traffic_light.get_transform().location
#red_light_event = TrafficEvent(event_type=TrafficEventType.TRAFFIC_LIGHT_INFRACTION)
self.events.append(
[TrafficEventType.TRAFFIC_LIGHT_INFRACTION])
self._last_red_light_id = traffic_light.id
self.n_tafficlight_violations += 1
break
def process_depth_images(msg,
depth_camera_setup,
ego_vehicle,
speed,
csv,
surface,
visualize=False):
print("Received a message for the time: {}".format(msg.timestamp))
# If we are in distance to the destination, stop and exit with success.
if ego_vehicle.get_location().distance(VEHICLE_DESTINATION) <= 5:
ego_vehicle.set_velocity(carla.Vector3D())
CLEANUP_FUNCTION()
sys.exit(0)
# Get the RGB image corresponding to the given depth image timestamp.
rgb_image = retrieve_rgb_image(msg.timestamp)
# Visualize the image and the bounding boxes if needed.
if visualize:
draw_image(rgb_image, surface)
# Transform the pedestrians into the detected objects relative to the
# current frame.
bb_surface = None
resolution = (depth_camera_setup.width, depth_camera_setup.height)
if visualize:
bb_surface = pygame.Surface(resolution)
bb_surface.set_colorkey((0, 0, 0))
detected_pedestrians = []
for pedestrian in ego_vehicle.get_world().get_actors().filter('walker.*'):
bbox = get_2d_bbox_from_3d_box(
depth_to_array(msg),
to_pylot_transform(ego_vehicle.get_transform()),
to_pylot_transform(pedestrian.get_transform()),
BoundingBox(pedestrian.bounding_box),
depth_camera_setup.get_unreal_transform(),
depth_camera_setup.get_intrinsic(), resolution, 1.0, 3.0)
if bbox is not None:
detected_pedestrians.append(bbox)
if visualize:
draw_bounding_box(bbox, bb_surface)
# We have drawn all the bounding boxes on the bb_surface, now put it on
# the RGB image surface.
if visualize:
surface.blit(bb_surface, (0, 0))
pygame.display.flip()
# Compute the mAP.
print("We detected a total of {} pedestrians.".format(len(detected_pedestrians)))
compute_and_log_map(detected_pedestrians, msg.timestamp, csv)
# Move the ego_vehicle according to the given speed.
ego_vehicle.set_velocity(carla.Vector3D(x=-speed))
ego_vehicle.get_world().tick()
def reset(self,pos):
VehicleController.set_control(self.control)
VehicleController.set_control(self.prev_control)
id_ =self.vehicle.id
self.simulator.client.apply_batch([carla.command.ApplyVelocity(id_, carla.Vector3D()),
carla.command.ApplyTransform(id_,pos),
carla.command.ApplyAngularVelocity(id_, carla.Vector3D()) ])
def _on_detect(weak_self, event, num):
self = weak_self()
if not self:
return
# If other part is already detected
if self.detected or not event.other_actor:
return
# Only track other dynamic objects
if "vehicle" not in event.other_actor.type_id or event.other_actor.id == self._parent.id:
self.obstacle = None
self.distance = None
return
self.detected = True
self.obstacle = event.other_actor
self.distance = event.distance
# relative position
veh_tran = self._parent.get_transform()
obs_pos = self.obstacle.get_location()
rel_pos = transform_to_world(veh_tran, obs_pos, inverse=True)
self.rel_pos = [rel_pos.x, rel_pos.y, rel_pos.z]
# velocity
sensor_rotation = carla.Rotation(yaw=veh_tran.rotation.yaw +
self.rotation.yaw + self.fov * num)
actor_vel = self.obstacle.get_velocity()
vel_vec = carla.Vector3D(x=actor_vel.x, y=actor_vel.y, z=actor_vel.z)
rel_vel = transform_to_world(carla.Transform(carla.Location(),
sensor_rotation),
vel_vec,
inverse=True)
self.rel_vel = [rel_vel.x, rel_vel.y, rel_vel.z]
if self._debug:
draw_vec = carla.Vector3D(x=obs_pos.x,
y=obs_pos.y,
z=obs_pos.z + 1)
veh_vel = self._parent.get_velocity()
veh_vel_vec = carla.Vector3D(x=veh_vel.x, y=veh_vel.y, z=veh_vel.z)
rel_veh_vel = transform_to_world(carla.Transform(
carla.Location(), sensor_rotation),
veh_vel_vec,
inverse=True)
norm_velocity = (self.rel_vel[0] - rel_veh_vel.x
) / self._velocity_range # range [-1, 1]
r = int(self.clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0)
g = int(self.clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0)
b = int(abs(self.clamp(-1.0, 0.0, -1.0 - norm_velocity)) * 255.0)
self._draw.draw_point(draw_vec,
size=0.1,
life_time=0.5,
persistent_lines=False,
color=carla.Color(r, g, b))
def init_scene(self, prefix, settings=None, spectator_tr=None):
super(SpawnAllRaycastSensors, self).init_scene(prefix, settings,
spectator_tr)
blueprint_library = self.world.get_blueprint_library()
vehicle00_tr = carla.Transform(carla.Location(140, -205, 0.1),
carla.Rotation(yaw=181.5))
vehicle00 = self.world.spawn_actor(
blueprint_library.filter("tt")[0], vehicle00_tr)
vehicle00.set_target_velocity(carla.Vector3D(-25, 0, 0))
vehicle01_tr = carla.Transform(carla.Location(50, -200, 0.1),
carla.Rotation(yaw=1.5))
vehicle01 = self.world.spawn_actor(
blueprint_library.filter("lincoln")[0], vehicle01_tr)
vehicle01.set_target_velocity(carla.Vector3D(25, 0, 0))
radar_bp = self.world.get_blueprint_library().find(
'sensor.other.radar')
radar_bp.set_attribute('noise_seed', '54283')
radar_tr = carla.Transform(carla.Location(z=2))
radar = self.world.spawn_actor(radar_bp, radar_tr, attach_to=vehicle00)
lidar01_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
lidar01_bp.set_attribute('noise_seed', '12134')
lidar01_tr = carla.Transform(carla.Location(x=1, z=2))
lidar01 = self.world.spawn_actor(lidar01_bp,
lidar01_tr,
attach_to=vehicle00)
lidar02_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast_semantic')
lidar02_tr = carla.Transform(carla.Location(x=1, z=2))
lidar02 = self.world.spawn_actor(lidar02_bp,
lidar02_tr,
attach_to=vehicle01)
lidar03_bp = self.world.get_blueprint_library().find(
'sensor.lidar.ray_cast')
lidar03_bp.set_attribute('noise_seed', '23135')
lidar03_tr = carla.Transform(carla.Location(z=2))
lidar03 = self.world.spawn_actor(lidar03_bp,
lidar03_tr,
attach_to=vehicle01)
self.add_sensor(radar, "Radar")
self.add_sensor(lidar01, "LiDAR")
self.add_sensor(lidar02, "SemLiDAR")
self.add_sensor(lidar03, "LiDAR")
self.add_actor(vehicle00, "Car")
self.add_actor(vehicle01, "Car")
self.wait(1)
def reset_vehicle(self):
start_lane = random.choice([-1, -2, -3, -4])
self.vehicle_start_pose = self.map.get_waypoint_xodr(road_id=45, lane_id=start_lane, s=100.).transform
if self.vehicle is None:
# create vehicle
blueprint_library = self.world.get_blueprint_library()
vehicle_blueprint = blueprint_library.find('vehicle.audi.a2')
self.vehicle = self.world.spawn_actor(vehicle_blueprint, self.vehicle_start_pose)
# self.vehicle.set_light_state(carla.libcarla.VehicleLightState.HighBeam) # HighBeam # LowBeam # All
else:
self.vehicle.set_transform(self.vehicle_start_pose)
self.vehicle.set_velocity(carla.Vector3D())
self.vehicle.set_angular_velocity(carla.Vector3D())
def reset(self):
# start_point = random.choice(self.spawn_points)
# self.destination = random.choice(self.spawn_points)
# yujiyu
junctions = get_junctions(self.waypoint_tuple_list)
start_point = random.choice(junctions).transform
# start_point = junctions[12].transform #11路口直行 12右转
self.vehicle.set_transform(start_point)
self.vehicle.set_velocity(carla.Vector3D(x=0.0, y=0.0, z=0.0))
for i in range(10):
self.world.tick()
ref_route = get_reference_route(self.world_map, self.vehicle, 50, 0.04)
self.destination = ref_route[-1][0].transform
self.global_dict['plan_map'], self.destination, ref_route = replan(
self.world_map, self.vehicle, self.agent, self.destination,
copy.deepcopy(self.origin_map), self.spawn_points)
# show_plan = cv2.cvtColor(np.asarray(self.global_dict['plan_map']), cv2.COLOR_BGR2RGB)
# cv2.namedWindow('plan_map', 0)
# cv2.resizeWindow('plan_map', 600, 600) # 自己设定窗口图片的大小
# cv2.imshow('plan_map', show_plan)
# cv2.waitKey(1)
self.global_dict['collision'] = False
# start_waypoint = self.agent._map.get_waypoint(self.agent._vehicle.get_location())
# end_waypoint = self.agent._map.get_waypoint(self.destination.location)
# self.route_trace = self.agent._trace_route(start_waypoint, end_waypoint)
self.route_trace = ref_route
start_point.rotation = self.route_trace[0][0].transform.rotation
self.vehicle.set_transform(start_point)
for i in range(10):
self.vehicle.set_velocity(carla.Vector3D(x=0.0, y=0.0, z=0.0))
self.world.tick()
self.state['img_nav'] = copy.deepcopy(self.global_dict['img_nav'])
self.state[
'v0'] = self.global_dict['v0'] if self.global_dict['v0'] > 4 else 4
# if self.state == None:
# print("None State!")
self.done = False
self.reward = 0.0
# print('RESET !!!!!!!!')
return self.state
def set_vehicle_control(self, ego, npc, ego_action, npc_action, c_tau,
step):
if step == 0:
# 初始速度设定
ego_target_speed = carla.Vector3D(16.5, 0, 0)
npc_target_speed = carla.Vector3D(20, 0, 0)
ego.set_target_velocity(ego_target_speed)
npc.set_target_velocity(npc_target_speed)
print('target velocity is set!')
else:
ego_move, ego_steer = ego_action
npc_move, npc_steer = npc_action
ego_steer = c_tau * ego_steer + (1 -
c_tau) * ego.get_control().steer
npc_steer = c_tau * npc_steer + (1 -
c_tau) * npc.get_control().steer
if ego_move >= 0:
ego_throttle = c_tau * ego_move + (
1 - c_tau) * ego.get_control().throttle
ego_control = carla.VehicleControl(throttle=ego_throttle,
steer=ego_steer,
brake=0)
elif ego_move < 0:
ego_brake = -c_tau * ego_move + (
1 - c_tau) * ego.get_control().brake
ego_control = carla.VehicleControl(throttle=0,
steer=ego_steer,
brake=ego_brake)
if npc_move >= 0:
npc_throttle = c_tau * npc_move + (
1 - c_tau) * npc.get_control().throttle
npc_control = carla.VehicleControl(throttle=npc_throttle,
steer=0,
brake=0)
elif npc_move < 0:
npc_brake = -c_tau * npc_move + (
1 - c_tau) * npc.get_control().brake
npc_control = carla.VehicleControl(throttle=0,
steer=0,
brake=npc_brake)
ego.apply_control(ego_control)
npc.apply_control(npc_control)
print('ego:%f,%f,%f,npc:%f,%f,%f' %
(ego.get_control().throttle, ego_steer,
ego.get_control().brake, npc.get_control().throttle,
npc_steer, npc.get_control().brake))
def get_actor(actor_id):
"""
Accessor for sumo actor.
"""
results = traci.vehicle.getSubscriptionResults(actor_id)
type_id = results[traci.constants.VAR_TYPE]
vclass = SumoActorClass(results[traci.constants.VAR_VEHICLECLASS])
color = results[traci.constants.VAR_COLOR]
length = results[traci.constants.VAR_LENGTH]
width = results[traci.constants.VAR_WIDTH]
height = results[traci.constants.VAR_HEIGHT]
location = list(results[traci.constants.VAR_POSITION3D])
rotation = [
results[traci.constants.VAR_SLOPE],
results[traci.constants.VAR_ANGLE], 0.0
]
transform = carla.Transform(
carla.Location(location[0], location[1], location[2]),
carla.Rotation(rotation[0], rotation[1], rotation[2]))
signals = results[traci.constants.VAR_SIGNALS]
extent = carla.Vector3D(length / 2.0, width / 2.0, height / 2.0)
return SumoActor(type_id, vclass, transform, signals, extent, color)
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 getWaypoint(self):
if len(self.waypoint_list) != 0:
location = carla.Location(self.waypoint_list[0][0],
self.waypoint_list[0][1],
self.waypoint_list[0][2])
rotation = self.map.get_waypoint(
location,
project_to_road=True,
lane_type=carla.LaneType.Driving).transform.rotation
box = carla.BoundingBox(location, carla.Vector3D(0, 6, 3))
if len(self.waypoint_list) == 1:
self.world.debug.draw_box(box,
rotation,
thickness=0.5,
color=carla.Color(255, 255, 0, 255),
life_time=0)
else:
self.world.debug.draw_box(box,
rotation,
thickness=0.5,
color=carla.Color(0, 255, 0, 255),
life_time=2)
return self.waypoint_list[0]
else:
return None
def draw_sensors(self):
for sensor in self.me_sensor_manager.sensors_list:
self.world.debug.draw_box(
carla.BoundingBox(sensor.get_transform().location,
carla.Vector3D(0.3, 0.1, 0.1)),
sensor.get_transform().rotation, 0.03,
carla.Color(255, 0, 0, 0), 0.01)
def _Radar_callback_plot(self, radar_data):
current_rot = radar_data.transform.rotation
velocity_range = 7.5 # m/s
world = self.world
debug = world.debug
def clamp(min_v, max_v, value):
return max(min_v, min(value, max_v))
for detect in radar_data:
azi = math.degrees(detect.azimuth)
alt = math.degrees(detect.altitude)
# The 0.25 adjusts a bit the distance so the dots can
# be properly seen
fw_vec = carla.Vector3D(x=detect.depth - 0.25)
carla.Transform(
carla.Location(),
carla.Rotation(pitch=current_rot.pitch + alt,
yaw=current_rot.yaw + azi,
roll=current_rot.roll)).transform(fw_vec)
norm_velocity = detect.velocity / velocity_range # range [-1, 1]
r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0)
g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0)
b = int(abs(clamp(-1.0, 0.0, -1.0 - norm_velocity)) * 255.0)
debug.draw_point(radar_data.transform.location + fw_vec,
size=0.075,
life_time=0.06,
persistent_lines=False,
color=carla.Color(r, g, b))
def __init__(self):
self.status = Status.ARRIVED
self.memory = {'location':carla.Vector3D(0.0,0.0,0.0)}
self.destination = self.get_location()
self.status_changed = lambda *_, **__: None
self.destination_changed = lambda *_, **__: None
self.data_changed = lambda *_, **__: None
def parking_decision(data):
camera_sensor.stop();
cameraControl = carla.VehicleControl(throttle=0);
vehicle.apply_control(cameraControl);
output=interface.decision(data,mlab);
overlapRatio = output['result'];
print('rate of Overlap:', overlapRatio);
if overlapRatio < 0.1:
print('parking place is between the next 2 vehicles');
location = vehicle.get_location();
location.x +=15;
location.y +=1.0;
location.z +=1.8;
text = 'Parking vacancy detected';
debug.draw_string(location, text, True, carla.Color(255,255,0), 4.0, True);
location.y +=1.5;
debug.draw_box(carla.BoundingBox(location,carla.Vector3D(0.2,2.0,0.8)),carla.Rotation(yaw=180), 0.1, carla.Color(255,0,0),4.0, True);
time.sleep(3.0);
mlab.end();
if hasattr(center_sensor, 'actorId'):
print('center_sensor info in detection time:', center_sensor.actorName, center_sensor.actorId);
if center_sensor.actorId != 0:
number=2;
else:
number=1;
print('matlab disconnected');
parking_preparation(number);
else:
print('no parking place has been detected right now');
cameraControl = carla.VehicleControl(throttle=0.2);
vehicle.apply_control(cameraControl);
print('car is moving');
camera_sensor.listen(lambda image: parking_decision(image));
def __init__(self):
self.speed = 0
self.angle = 0
self.bearing_deg = 0.0
self.vel = carla.Vector3D()
self.cruise_button = 0
self.is_engaged = False
def __init__(self,
vehicle_id,
type_id,
model_filename,
color,
location,
rotation,
velocity,
lights_state=VissimLightState.NONE):
# Static parameters.
self.id = vehicle_id
self.type = type_id
self.model_filename = model_filename
self.color = color
# Dynamic attributes.
loc = carla.Location(location[0], location[1], location[2])
rot = carla.Rotation(math.degrees(rotation[0]), math.degrees(rotation[1]),
math.degrees(rotation[2]))
self._transform = carla.Transform(loc, rot)
self._velocity = carla.Vector3D(
velocity * math.cos(math.radians(rot.yaw)) * math.cos(math.radians(rot.pitch)),
velocity * math.sin(math.radians(rot.yaw)) * math.cos(math.radians(rot.pitch)),
velocity * math.sin(math.radians(rot.pitch)))
self._lights_state = lights_state
def draw_box(self, transform, color, lift_time=0.01):
self.world.debug.draw_box(box=carla.BoundingBox(
transform.location + carla.Location(z=0.00),
carla.Vector3D(x=0.5 / 2., y=0.5 / 2., z=0.05)),
rotation=transform.rotation,
color=color,
life_time=lift_time)
def draw_box_with_arrow(self,
transform,
color,
text,
with_arrow=True,
with_text=False):
self.world.debug.draw_box(box=carla.BoundingBox(
transform.location + carla.Location(z=0.3),
carla.Vector3D(x=(4.925 + 0.1) / 2, y=(2.116 + 0.1) / 2, z=0.05)),
rotation=transform.rotation,
color=color,
life_time=0.01,
thickness=0.2)
if with_arrow:
yaw = math.radians(transform.rotation.yaw)
self.world.debug.draw_arrow(
begin=transform.location + carla.Location(z=0.9),
end=transform.location +
carla.Location(x=math.cos(yaw), y=math.sin(yaw), z=0.9),
thickness=0.2,
arrow_size=0.3,
color=color,
life_time=0.01)
if with_text:
self.world.debug.draw_string(location=transform.location +
carla.Location(z=1.0),
text=text,
color=carla.Color(r=255,
b=255,
g=255,
a=255),
life_time=0.01,
draw_shadow=True)
def get_traffic_light_area(self, tl):
base_transform = tl.get_transform()
base_rot = base_transform.rotation.yaw
area_loc = base_transform.transform(tl.trigger_volume.location)
wpx = self._map.get_waypoint(area_loc)
while not wpx.is_intersection:
next = wpx.next(1.0)[0]
if next:
wpx = next
else:
break
wpx_location = wpx.transform.location
area_ext = tl.trigger_volume.extent
area = []
# why the 0.9 you may ask?... because the triggerboxes are set manually and sometimes they
# cross to adjacent lanes by accident
x_values = np.arange(-area_ext.x * 0.9, area_ext.x * 0.9, 1.0)
for x in x_values:
pt = self.rotate_point(carla.Vector3D(x, 0, area_ext.z), base_rot)
area.append(wpx_location + carla.Location(x=pt.x, y=pt.y))
return area_loc, area
def get_ego_step(self, ego, action, sim_time, flag): # 0加速变道 1刹车变道
if flag == 1:
ego_target_speed = carla.Vector3D(18, 0, 0)
ego.set_target_velocity(ego_target_speed)
print('ego velocity is set!')
if action == 0:
if 1 < sim_time <= 1.55:
ego_control = carla.VehicleControl(throttle=1, steer=-0.1)
ego.apply_control(ego_control)
elif 1.55 < sim_time <= 2.1:
ego_control = carla.VehicleControl(throttle=1, steer=0.1)
ego.apply_control(ego_control)
else:
ego_control = carla.VehicleControl(throttle=1, brake=0)
ego.apply_control(ego_control)
if action == 1:
if 1.25 <= sim_time <= 2:
ego_control = carla.VehicleControl(throttle=0, brake=1)
ego.apply_control(ego_control)
elif 2 < sim_time <= 2.7:
ego_control = carla.VehicleControl(throttle=1, steer=-0.1)
ego.apply_control(ego_control)
elif 2.7 < sim_time <= 3.4:
ego_control = carla.VehicleControl(throttle=1, steer=0.1)
ego.apply_control(ego_control)
elif sim_time > 3.4:
ego_control = carla.VehicleControl(throttle=1, steer=0)
ego.apply_control(ego_control)
else:
ego_control = carla.VehicleControl(throttle=0, brake=0)
ego.apply_control(ego_control)
def draw_radar_measurement(debug_helper: carla.DebugHelper,
data: carla.RadarMeasurement,
velocity_range=7.5,
size=0.075,
life_time=0.06):
"""Code adapted from carla/PythonAPI/examples/manual_control.py:
- White: means static points.
- Red: indicates points moving towards the object.
- Blue: denoted points moving away.
"""
radar_rotation = data.transform.rotation
for detection in data:
azimuth = math.degrees(detection.azimuth) + radar_rotation.yaw
altitude = math.degrees(detection.altitude) + radar_rotation.pitch
# move to local coordinates:
forward_vec = carla.Vector3D(x=detection.depth - 0.25)
global_to_local(forward_vec,
reference=carla.Rotation(pitch=altitude,
yaw=azimuth,
roll=radar_rotation.roll))
# compute color:
norm_velocity = detection.velocity / velocity_range
r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0)
g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0)
b = int(abs(clamp(-1.0, 0.0, -1.0 - norm_velocity)) * 255.0)
# draw:
debug_helper.draw_point(data.transform.location + forward_vec,
size=size,
life_time=life_time,
persistent_lines=False,
color=carla.Color(r, g, b))
def _Radar_callback(weak_self, radar_data):
self = weak_self()
if not self:
return
# To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]:
# points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4'))
# points = np.reshape(points, (len(radar_data), 4))
current_rot = radar_data.transform.rotation
for detect in radar_data:
azi = math.degrees(detect.azimuth)
alt = math.degrees(detect.altitude)
# The 0.25 adjusts a bit the distance so the dots can
# be properly seen
fw_vec = carla.Vector3D(x=detect.depth - 0.25)
carla.Transform(
carla.Location(),
carla.Rotation(
pitch=current_rot.pitch + alt,
yaw=current_rot.yaw + azi,
roll=current_rot.roll)).transform(fw_vec)
def clamp(min_v, max_v, value):
return max(min_v, min(value, max_v))
norm_velocity = detect.velocity / self.velocity_range # range [-1, 1]
r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0)
g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0)
b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0)
self.debug.draw_point(
radar_data.transform.location + fw_vec,
size=0.075,
life_time=0.06,
persistent_lines=False,
color=carla.Color(r, g, b))
def control_pure_pursuit(vehicle_tr, waypoint_tr, max_steer, wheelbase, world):
wp_loc_rel = relative_location(
vehicle_tr, waypoint_tr.location) + carla.Vector3D(wheelbase, 0, 0)
# TODO: convert vehicle transform to rear axle transform
wp_ar = [wp_loc_rel.x, wp_loc_rel.y]
d2 = wp_ar[0]**2 + wp_ar[1]**2
steer_rad = math.atan(2 * wheelbase * wp_loc_rel.y / d2)
steer_deg = math.degrees(steer_rad)
steer_deg = np.clip(steer_deg, -max_steer, max_steer)
yaw = vehicle_tr.rotation.yaw
angle = yaw + steer_deg
angle_rad = math.radians(angle)
print("v loc: " + str(vehicle_tr.location))
print("yaw: " + str(yaw))
print("angle: " + str(angle))
#trying to draw on arrow
y = math.sin(angle_rad) * 10 + vehicle_tr.location.y
x = math.cos(angle_rad) * 10 + vehicle_tr.location.x
print("x, y " + str(x) + " " + str(y))
#world.debug.draw_point(carla.Location(x,y,0), life_time = 1.0)
world.debug.draw_arrow(vehicle_tr.location,
carla.Location(x, y, 0),
life_time=1)
return steer_deg / max_steer
def rotate_point(self, point, angle):
"""
rotate a given point by a given angle
"""
x_ = math.cos(math.radians(angle)) * point.x - math.sin(math.radians(angle)) * point.y
y_ = math.sin(math.radians(angle)) * point.x + math.cos(math.radians(angle)) * point.y
return carla.Vector3D(x_, y_, point.z)
def debug_square(client,
l,
r,
rotation=carla.Rotation(),
t=1.0,
c=carla.Color(r=255, g=0, b=0, a=100)):
"""Draw a square centered on a point.
Parameters
----------
client : carla.Client or carla.World
Client.
l : carla.Transform or carla.Location
Position in map to place the point.
r : float
Radius of the square from the center
t : float, optional
Life time of point.
c : carla.Color, optional
Color of the point.
"""
if isinstance(l, carla.Transform):
l = l.location
if isinstance(client, carla.Client):
world = client.get_world()
else:
world = client
box = carla.BoundingBox(l, carla.Vector3D(r, r, r))
world.debug.draw_box(box, rotation, thickness=0.5, color=c, life_time=t)
def _get_trafficlight_trigger_location(
self,
traffic_light,
) -> carla.Location: # pylint: disable=no-member
"""Calculates the yaw of the waypoint that represents the trigger volume of
the traffic light."""
def rotate_point(point, radians):
"""Rotates 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) # pylint: disable=no-member
base_transform = traffic_light.get_transform()
base_rot = base_transform.rotation.yaw
area_loc = base_transform.transform(traffic_light.trigger_volume.location)
area_ext = traffic_light.trigger_volume.extent
point = rotate_point(
carla.Vector3D(0, 0, area_ext.z), # pylint: disable=no-member
math.radians(base_rot),
)
point_location = area_loc + carla.Location(x=point.x, y=point.y) # pylint: disable=no-member
return carla.Location(point_location.x, point_location.y, point_location.z) # pylint: disable=no-member
def spawn_actor(world):
blueprint = world.get_blueprint_library().filter('vehicle.*model3*')[0]
waypoints = world.get_map().generate_waypoints(10.0)
targetLane = -3
waypoint = waypoints[500]
waypoint = change_lane(waypoint, targetLane - waypoint.lane_id)
location = waypoint.transform.location + carla.Vector3D(0, 0, 1.5)
rotation = waypoint.transform.rotation
vehicle = world.spawn_actor(blueprint, carla.Transform(location, rotation))
actor_list.append(vehicle)
vehicle.set_simulate_physics(True)
transform = carla.Transform(carla.Location(x=0.8, z=1.7))
#Add spectator camera
camera_bp = world.get_blueprint_library().find('sensor.camera.rgb')
camera_transform = carla.Transform(carla.Location(x=-10, z=10),
carla.Rotation(-30, 0, 0))
camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle)
actor_list.append(camera) #Add to actor_list at [1]
attach_lidar(world, vehicle, transform)
attach_collision_sensor(world, vehicle, transform)
return vehicle, camera
def get_trafficlight_trigger_location(traffic_light): # pylint: disable=invalid-name
"""
Calculates the yaw of the waypoint that represents the trigger volume of the traffic light
"""
def rotate_point(point, angle):
"""
rotate a given point by a given angle
"""
x_ = math.cos(math.radians(angle)) * point.x - math.sin(
math.radians(angle)) * point.y
y_ = math.sin(math.radians(angle)) * point.x - math.cos(
math.radians(angle)) * point.y
return carla.Vector3D(x_, y_, point.z)
base_transform = traffic_light.get_transform()
base_rot = base_transform.rotation.yaw
area_loc = base_transform.transform(
traffic_light.trigger_volume.location)
area_ext = traffic_light.trigger_volume.extent
point = rotate_point(carla.Vector3D(0, 0, area_ext.z), 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 run_step(self):
"""
Execute on tick of the controller's control loop
The control loop is very simplistic:
If the actor speed is below the _target_speed, set throttle to 1.0,
otherwise, set throttle to 0.0
Note, that this is a longitudinal controller only.
If _init_speed is True, the control command is post-processed to ensure that
the initial actor velocity is maintained independent of physics.
"""
control = self._actor.get_control()
velocity = self._actor.get_velocity()
current_speed = math.sqrt(velocity.x**2 + velocity.y**2)
if current_speed < self._target_speed:
control.throttle = 1.0
else:
control.throttle = 0.0
self._actor.apply_control(control)
if self._init_speed:
if abs(self._target_speed - current_speed) > 3:
yaw = self._actor.get_transform().rotation.yaw * (math.pi /
180)
vx = math.cos(yaw) * self._target_speed
vy = math.sin(yaw) * self._target_speed
self._actor.set_target_velocity(carla.Vector3D(vx, vy, 0))
