def process_img(im_rgb, im_ss, text, actors_present, frame, SAVE_FLAG,
DISP_FLAG):
if SAVE_FLAG == 1:
#for bounding boxes
debug = world.debug
for actor in actors_present:
debug.draw_box(box=carla.BoundingBox(
actor.get_transform().location, actor.bounding_box.extent),
rotation=actor.get_transform().rotation,
thickness=0.1,
color=carla.Color(255, 0, 0, 0),
life_time=0.1)
im_rgb.save_to_disk("data\{}_rgb.jpg".format(frame))
im_ss.save_to_disk("data\{}_semseg.jpg".format(frame))
if DISP_FLAG == 1:
i = np.frombuffer(im_rgb.raw_data, dtype=np.dtype("uint8"))
i2 = im_rgb.reshape((IM_WIDTH, IM_HEIGHT, 4))
cv2.putText(i2, text, bottomLeftCornerOfText, font, fontScale,
(255, 255, 255), lineType, cv2.LINE_AA)
#for bounding boxes
debug = world.debug
for actor in actors_present:
debug.draw_box(box=carla.BoundingBox(
actor.get_transform().location, actor.bounding_box.extent),
rotation=actor.get_transform().rotation,
thickness=0.1,
color=carla.Color(255, 0, 0, 0),
life_time=0.1)
cv2.imshow("", i2)
cv2.waitKey(1)
return
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 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 draw_box(self, marker, lifetime, color):
"""
draw box from ros 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)
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 initialize(world,
blueprint,
position=carla.Vector3D(0.0, 0.0, 0.0),
rotation=carla.Rotation(0.0, 0.0, 0.0),
transform=None,
verbose=False):
'''
Initializes a Carla actor with the provided data and returns the created
actor.
'''
if not transform:
transform = carla.Transform(position, rotation)
actor = world.try_spawn_actor(blueprint, transform)
if not actor:
print("Problem creating actor. Returning None")
elif verbose:
time.sleep(0.1)
print("Creating actor:")
print_info(actor)
world.debug.draw_box(
carla.BoundingBox(
actor.get_location() + actor.bounding_box.location,
actor.bounding_box.extent),
actor.get_transform().rotation)
return actor
def config_friction(friction_bp,
location,
extent,
scale=0.001,
color=(255, 0, 0)):
""" Config and spawn a friction event """
# friction from blueprint
# Defind bounding box and location
friction_extent = extent
friction_bp.set_attribute('friction', str(scale))
friction_bp.set_attribute('extent_x', str(100 * friction_extent.x))
friction_bp.set_attribute('extent_y', str(100 * friction_extent.y))
friction_bp.set_attribute('extent_z', str(100 * friction_extent.z))
friction_transform = carla.Transform()
friction_transform.location = location
box = carla.BoundingBox(friction_transform.location, friction_extent)
rotation = friction_transform.rotation
life_time = 20
thickness = 0.2
color = carla.Color(r=color[0], g=color[1], b=color[2])
config = {'box': box, \
'rotation': rotation, \
'life_time': life_time, \
'thickness': thickness,
'color': color
}
return friction_bp, friction_transform, config
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 parse_bounding_box(info):
"""
Parses a list into a carla.BoundingBox.
Some actors like sensors might have 'nan' location and 'inf' extent, so filter those.
"""
if 'nan' in info[3]:
location = carla.Location()
else:
location = carla.Location(
float(info[3][1:-1]) / 100,
float(info[4][:-1]) / 100,
float(info[5][:-1]) / 100,
)
if 'inf' in info[7]:
extent = carla.Vector3D()
else:
extent = carla.Vector3D(
float(info[7][1:-1]) / 100,
float(info[8][:-1]) / 100,
float(info[9][:-1]) / 100,
)
bbox = carla.BoundingBox(location, extent)
return bbox
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 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 debug_draw_green_player_bbox(self):
self._world.debug.draw_box(carla.BoundingBox(
self._player.get_transform().location,
self._player.bounding_box.extent),
self._player.get_transform().rotation,
thickness=0.5,
color=carla.Color(r=0, g=255, b=0, a=255),
life_time=3.0)
def as_simulator_bounding_box(self):
"""Retrieves the bounding box as instance of a simulator bounding box.
Returns:
carla.BoundingBox: Instance that represents the bounding box.
"""
import carla
bb_loc = self.transform.location.as_simulator_location()
bb_extent = self.extent.as_simulator_vector()
return carla.BoundingBox(bb_loc, bb_extent)
def _draw_bbox(self, obj):
bbox_location = obj.get_transform(
).location + obj.bounding_box.location
self._debug_helper.draw_box(
carla.BoundingBox(bbox_location, obj.bounding_box.extent),
obj.get_transform().rotation,
0.05, # thickness
carla.Color(255, 0, 0, 0),
5 # lifetime [sec]
)
def draw_box_with_arrow(word, transform, color, text):
word.debug.draw_box(box=carla.BoundingBox(transform.location, carla.Vector3D(x=(4.925 + 0.6)/2., y=(2.116 + 0.6)/2., z=0.8)),
rotation=transform.rotation,
color=color)
yaw = math.radians(transform.rotation.yaw)
word.debug.draw_arrow(begin=transform.location, end=transform.location + carla.Location(x=math.cos(yaw), y=math.sin(yaw), z=0),
thickness=0.1, arrow_size=0.2,
color=color)
word.debug.draw_string(location=transform.location, text=text, color=carla.Color(r=255, b=255, g=255, a=255),
life_time=100.0, draw_shadow=True)
def config_waypoint_box(target_waypoint, color=(255, 0, 0)):
extent_wp = carla.Location(0.1, 0.1, 0.1)
if target_waypoint is None:
box = carla.BoundingBox(carla.Location(), extent_wp)
rotation = carla.Rotation()
else:
box = carla.BoundingBox(target_waypoint.transform.location, extent_wp)
rotation = target_waypoint.transform.rotation
life_time = 0.3 * 10
thickness = 0.01 * 1
color = carla.Color(r=color[0], g=color[1], b=color[2])
config = {'box': box, \
'rotation': rotation, \
'life_time': life_time, \
'thickness': thickness,
'color': color
}
return config
def _draw_waypoint(self,
wp: msg.WaypointWithSpeedLimit,
color: carla.Color,
lifetime: float = 0):
loc = carla.Location(wp.x, wp.y, wp.z + 0.5)
rotation = carla.Rotation(0, 0, 0)
inner_size = 0.04
outer_size = 0.068
self._debug_helper.draw_box(carla.BoundingBox(
loc, carla.Vector3D(inner_size, inner_size, inner_size)),
rotation,
thickness=0.06,
color=color,
life_time=lifetime)
self._debug_helper.draw_box(carla.BoundingBox(
loc, carla.Vector3D(outer_size, outer_size, outer_size)),
rotation,
thickness=0.01,
color=carla.Color(0, 0, 0, 0),
life_time=lifetime)
def debug_draw_red_player_bbox(self):
self.__world.debug.draw_box(
carla.BoundingBox(
self.__ego_vehicle.get_transform().location,
self.__ego_vehicle.bounding_box.extent,
),
self.__ego_vehicle.get_transform().rotation,
thickness=0.5,
color=carla.Color(r=255, g=0, b=0, a=255),
life_time=3.0,
)
def draw_trigger_volume(world, actor):
"""Draws the trigger volume of an actor.
Args:
world (carla.World): A handle to the world running inside the
simulation.
actor (carla.Actor): A CARLA actor.
"""
transform = actor.get_transform()
tv = transform.transform(actor.trigger_volume.location)
bbox = carla.BoundingBox(tv, actor.trigger_volume.extent)
world.debug.draw_box(bbox, transform.rotation, life_time=1000)
def as_carla_bounding_box(self):
""" Retrieves the current BoundingBox3D as an instance of
carla.BoundingBox
Returns:
A carla.BoundingBox instance that represents the current bounding
box.
"""
import carla
bb_loc = self.transform.location.as_carla_location()
bb_extent = self.extent.as_carla_vector()
return carla.BoundingBox(bb_loc, bb_extent)
def draw_bounding_box(world):
"""Shows how to draw traffic light actor bounding boxes from a worlds snapshot."""
debug = world.debug
world_snaphost = world.get_snapshot()
for actor_snapshot in world_snaphost:
actor = world.get_actor(actor_snapshot.id)
if actor.type_id == 'traffic.traffic_light':
actor_transform = actor_snapshot.get_transform()
bounding_box = carla.BoundingBox(actor_transform.location, carla.Vector3D(0.5, 0.5, 2))
debug.draw_box(bounding_box, actor_transform.rotation, 0.05, Colors.red, 0)
def draw_boundingbox(actor,
life_time=-1.0,
color=carla.Color(255, 255, 255),
thickness=0.1,
offset=carla.Location(0.0, 0.0, 0.0)):
world = actor.get_world()
world.debug.draw_box(carla.BoundingBox(
actor.get_location() + actor.bounding_box.location + offset,
actor.bounding_box.extent),
actor.get_transform().rotation,
color=color,
thickness=thickness,
life_time=life_time)
def convert_traffic_stop_actors(traffic_stop_actors):
""" Converts a Carla traffic stop actor into a Pylot stop sign object."""
stop_signs = []
for ts_actor in traffic_stop_actors:
transform = to_pylot_transform(ts_actor.get_transform())
world_trigger_volume = ts_actor.get_transform().transform(
ts_actor.trigger_volume.location)
bbox = pylot.simulation.utils.BoundingBox(
carla.BoundingBox(world_trigger_volume,
ts_actor.trigger_volume.extent))
stop_sign = pylot.simulation.utils.StopSign(transform, bbox)
stop_signs.append(stop_sign)
return stop_signs
def draw_box_with_arrow2(word, transform, color, text):
word.debug.draw_box(box=carla.BoundingBox(
transform.location + carla.Location(z=0.05),
carla.Vector3D(x=(4.925) / 2., y=(2.116) / 2., z=0.05)),
rotation=transform.rotation,
color=color)
yaw = math.radians(transform.rotation.yaw)
word.debug.draw_arrow(
begin=transform.location + carla.Location(z=0.2),
end=transform.location +
carla.Location(x=math.cos(yaw), y=math.sin(yaw), z=0.2),
thickness=0.1,
arrow_size=0.2,
color=color)
def draw_lane(waypoints):
global world
# draw lane
for waypoint in waypoints[0:min(len(waypoints)-1, 20)]:
box_point = carla.Location(waypoint.transform.location.x,
waypoint.transform.location.y,
waypoint.transform.location.z-0.4)
box = carla.BoundingBox(box_point, carla.Vector3D(x=2,y=0.1,z=0.4))
rotation = carla.Rotation(pitch=waypoint.transform.rotation.pitch,
yaw=waypoint.transform.rotation.yaw,
roll=waypoint.transform.rotation.roll)
world.debug.draw_box(box=box, rotation=rotation, thickness=1.2, life_time=0)
# wait for draw
sleep(0.3)
def config_measurements_box(measurements, color=(255, 0, 0)):
extent_m = carla.Location(0.1, 0.1, 0.1)
box = carla.BoundingBox(measurements.t.location, extent_m)
rotation = measurements.t.rotation
life_time = .1 * 1
thickness = 0.01 * 1
color = carla.Color(r=color[0], g=color[1], b=color[2])
config = {'box': box, \
'rotation': rotation, \
'life_time': life_time, \
'thickness': thickness,
'color': color
}
return config
def _draw_trigger_volume(self, world, tl_actor):
transform = tl_actor.get_transform()
tv = transform.transform(tl_actor.trigger_volume.location)
bbox = carla.BoundingBox(tv, tl_actor.trigger_volume.extent)
tl_state = tl_actor.get_state()
if tl_state in TL_STATE_TO_PIXEL_COLOR:
r, g, b = TL_STATE_TO_PIXEL_COLOR[tl_state]
bbox_color = carla.Color(r, g, b)
else:
bbox_color = carla.Color(0, 0, 0)
bbox_life_time = 1 / self._flags.carla_step_frequency + TL_BBOX_LIFETIME_BUFFER
world.debug.draw_box(bbox,
transform.rotation,
thickness=0.5,
color=bbox_color,
life_time=bbox_life_time)
def spawn_friction(self, location, set_friction):
self.friction_bp = self.world.get_blueprint_library().find(
'static.trigger.friction')
extent = carla.Location(400, 100, 0.001)
self.friction_bp.set_attribute('friction', str(set_friction))
self.friction_bp.set_attribute('extent_x', str(extent.x))
self.friction_bp.set_attribute('extent_y', str(extent.y))
self.friction_bp.set_attribute('extent_z', str(extent.z))
# Spawn Trigger Friction
transform = carla.Transform()
transform.location = carla.Location(x=392.1, y=location, z=0.0)
self.friction = self.world.spawn_actor(self.friction_bp, transform)
# Optional for visualizing trigger
self.world.debug.draw_box(box=carla.BoundingBox(transform.location, extent * 1e-2), rotation=transform.rotation, \
life_time=1000, thickness=0.05, color=carla.Color(r=255,g=0,b=0))
self.actor.append(self.friction)
def draw_box_with_arrow(word, transform, color, text, with_arrow=False):
word.debug.draw_box(box=carla.BoundingBox(
transform.location + carla.Location(z=0.05),
carla.Vector3D(x=(4.925) / 2., y=(2.116) / 2., z=0.05)),
rotation=transform.rotation,
color=color)
yaw = math.radians(transform.rotation.yaw)
x, y, yaw = center2rear([transform.location.x, transform.location.y, yaw])
location = carla.Location(x=x, y=y, z=transform.location.z)
if with_arrow:
word.debug.draw_arrow(
begin=location + carla.Location(z=0.2),
end=location +
carla.Location(x=math.cos(yaw), y=math.sin(yaw), z=0.2),
thickness=0.1,
arrow_size=0.2,
color=color)
def render(self, display):
self.camera_manager.render(display)
self.hud.render(display)
debug = self.world.debug
#world_snapshot = self.world.get_snapshot()
#print(world_snapshot)
#Find traffic signal poll code - left it for future project
if 0:
if self.gametick % 20 == 0 and self.drawboxdone == False and 0:
for vehicle in self.world.get_actors().filter('traffic.*'):
if vehicle.type_id == 'traffic.speed_limit.30' or vehicle.type_id == 'traffic.speed_limit.60' or vehicle.type_id == 'traffic.speed_limit.90' or vehicle.type_id == 'traffic.stop':
#print(carla.BoundingBox(actor_snapshot.get_transform().location,carla.Vector3D(0.5,0.5,2)))
#print(actor_snapshot.get_transform().rotation)
debug.draw_box(
carla.BoundingBox(vehicle.get_transform().location,
carla.Vector3D(0.5, 0.5, 3)),
vehicle.get_transform().rotation, 0.05,
carla.Color(255, 0, 0, 0), 0)
self.drawboxdone = True
