def apply_physics(self, phys_settings):
self.world.tick()
"""
Default wheel settings:
front_left_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=70, radius=36.7)
front_right_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=70, radius=36.7)
rear_left_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=0.0, radius=36.0)
rear_right_wheel = carla.WheelPhysicsControl(tire_friction=3.5, damping_rate=0.25, max_steer_angle=0.0, radius=36.0)
"""
front_left_wheel = carla.WheelPhysicsControl(tire_friction=phys_settings['flwf'], damping_rate=0.25, max_steer_angle=phys_settings['flwmsa'], radius=36.7)
front_right_wheel = carla.WheelPhysicsControl(tire_friction=phys_settings['frwf'], damping_rate=0.25, max_steer_angle=phys_settings['frwmsa'], radius=36.7)
rear_left_wheel = carla.WheelPhysicsControl(tire_friction=phys_settings['rlwf'], damping_rate=0.25, max_steer_angle=0.0, radius=36.0)
rear_right_wheel = carla.WheelPhysicsControl(tire_friction=phys_settings['rrwf'], damping_rate=0.25, max_steer_angle=0.0, radius=36.0)
wheels = [front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel]
# Change Vehicle Physics Control parameters of the vehicle
physics_control = self.player.get_physics_control()
#physics_control.max_rpm = phys_settings['max_rpm']
physics_control.mass = phys_settings['mass']
#physics_control.drag_coefficient = phys_settings['drag_coefficient']
physics_control.wheels = wheels
physics_control.steering_curve = [carla.Vector2D(0, 1), carla.Vector2D(20, phys_settings['steer1']), carla.Vector2D(60, phys_settings['steer2']), carla.Vector2D(120, phys_settings['steer3'])]
physics_control.torque_curve = [carla.Vector2D(0, 400), carla.Vector2D(890, phys_settings['torque1']), carla.Vector2D(5729.577, 400)]
self.speed = phys_settings['speed']
# Apply Vehicle Physics Control for the vehicle
self.player.apply_physics_control(physics_control)
def point_inside_boundingbox(point, bb_center, bb_extent):
"""
X
:param point:
:param bb_center:
:param bb_extent:
:return:
"""
# pylint: disable=invalid-name
A = carla.Vector2D(bb_center.x - bb_extent.x,
bb_center.y - bb_extent.y)
B = carla.Vector2D(bb_center.x + bb_extent.x,
bb_center.y - bb_extent.y)
D = carla.Vector2D(bb_center.x - bb_extent.x,
bb_center.y + bb_extent.y)
M = carla.Vector2D(point.x, point.y)
AB = B - A
AD = D - A
AM = M - A
am_ab = AM.x * AB.x + AM.y * AB.y
ab_ab = AB.x * AB.x + AB.y * AB.y
am_ad = AM.x * AD.x + AM.y * AD.y
ad_ad = AD.x * AD.x + AD.y * AD.y
return am_ab > 0 and am_ab < ab_ab and am_ad > 0 and am_ad < ad_ad
def render(self, display: pg.Surface, turn_angle: float):
"""
Render the indicator
@param: turn_angle wheels turn angle in radians
"""
# background w/ border
round_rect(self.surface, pg.Rect(0, 0, self.size[0],
self.size[1]), self.border_color,
self.radius, self.border, self.inner_color)
line_width = 1
axle_width = self.size[0] - (2 * self.h_offset)
frame_height = self.size[1] - (2 * self.v_offset)
front_axle_y = self.v_offset
rear_axle_y = self.v_offset + frame_height
wheel_half_size = 18
# front axle
pg.draw.line(self.surface, self.border_color,
(self.h_offset, front_axle_y),
(self.h_offset + axle_width, self.v_offset), line_width)
# rear axle
pg.draw.line(self.surface, self.border_color,
(self.h_offset, rear_axle_y),
(self.h_offset + axle_width, rear_axle_y), line_width)
# frame center
pg.draw.line(self.surface, self.border_color,
(self.h_offset + axle_width // 2, front_axle_y),
(self.h_offset + axle_width // 2, rear_axle_y),
line_width)
for x_offset in [self.h_offset, self.h_offset + axle_width]:
# front wheels
pivot_p = carla.Vector2D(x_offset, front_axle_y)
from_p = carla.Vector2D(x_offset, front_axle_y + wheel_half_size)
to_p = carla.Vector2D(x_offset, front_axle_y - wheel_half_size)
from_p = self._rotate_point(pivot_p, from_p, turn_angle)
to_p = self._rotate_point(pivot_p, to_p, turn_angle)
pg.draw.line(self.surface, self.border_color, (from_p.x, from_p.y),
(to_p.x, to_p.y), 10)
# bottom wheels
pg.draw.line(self.surface, self.border_color,
(x_offset, rear_axle_y + wheel_half_size),
(x_offset, rear_axle_y - wheel_half_size), 10)
# update display
display.blit(self.surface, self.pos)
def _point_inside_boundingbox(self, point, bb_center, bb_extent):
A = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y - bb_extent.y)
B = carla.Vector2D(bb_center.x + bb_extent.x, bb_center.y - bb_extent.y)
D = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y + bb_extent.y)
M = carla.Vector2D(point.x, point.y)
AB = B - A
AD = D - A
AM = M - A
am_ab = AM.x * AB.x + AM.y * AB.y
ab_ab = AB.x * AB.x + AB.y * AB.y
am_ad = AM.x * AD.x + AM.y * AD.y
ad_ad = AD.x * AD.x + AD.y * AD.y
return am_ab > 0 and am_ab < ab_ab and am_ad > 0 and am_ad < ad_ad
def cb_car_info(self, car_info):
self.car_info = car_info
if car_info.initial:
self.length = (
carla.Vector2D(car_info.front_axle_center.x,
car_info.front_axle_center.y) -
carla.Vector2D(car_info.rear_axle_center.x,
car_info.rear_axle_center.y)).length()
self.rear_length = (
carla.Vector2D(car_info.car_pos.x, car_info.car_pos.y) -
carla.Vector2D(car_info.rear_axle_center.x,
car_info.rear_axle_center.y)).length()
self.max_steer_angle = self.car_info.max_steer_angle
print('car length {}, rear length {}'.format(
self.length, self.rear_length))
def publish_plan(self):
current_time = rospy.Time.now()
gui_path = NavPath()
gui_path.header.frame_id = 'map'
gui_path.header.stamp = current_time
values = [(carla.Vector2D(self.actor.get_location().x,
self.actor.get_location().y),
self.actor.get_transform().rotation.yaw)]
# Exclude last point because no yaw information.
values += [(self.path.get_position(i), self.path.get_yaw(i))
for i in range(len(self.path.route_points) - 1)]
for (position, yaw) in values:
pose = PoseStamped()
pose.header.frame_id = 'map'
pose.header.stamp = current_time
pose.pose.position.x = position.x
pose.pose.position.y = position.y
pose.pose.position.z = 0
quaternion = tf.transformations.quaternion_from_euler(
0, 0, np.deg2rad(yaw))
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
gui_path.poses.append(pose)
self.plan_pub.publish(gui_path)
def _rotate_point(self, center: carla.Vector2D, point: carla.Vector2D,
angle: float):
return carla.Vector2D(
math.cos(angle) * (point.x - center.x) - math.sin(angle) *
(point.y - center.y) + center.x,
math.sin(angle) * (point.x - center.x) + math.cos(angle) *
(point.y - center.y) + center.y)
def parse_vector_list(info):
"""Parses a list of string into a list of Vector2D"""
vector_list = []
for i in range(0, len(info), 2):
vector = carla.Vector2D(
x=float(info[i][1:-1]),
y=float(info[i + 1][:-1]),
)
vector_list.append(vector)
return vector_list
def update_crowd_range(self):
# Cap frequency so that GAMMA loop doesn't die.
if self.last_crowd_range_update is None or time.time(
) - self.last_crowd_range_update > 1.0:
pos = self.actor.get_location()
bounds_min = carla.Vector2D(pos.x - self.crowd_range,
pos.y - self.crowd_range)
bounds_max = carla.Vector2D(pos.x + self.crowd_range,
pos.y + self.crowd_range)
exclude_bounds_min = carla.Vector2D(
pos.x - self.exclude_crowd_range,
pos.y - self.exclude_crowd_range)
exclude_bounds_max = carla.Vector2D(
pos.x + self.exclude_crowd_range,
pos.y + self.exclude_crowd_range)
self.crowd_service.simulation_bounds = (bounds_min, bounds_max)
self.crowd_service.forbidden_bounds = (exclude_bounds_min,
exclude_bounds_max)
self.last_crowd_range_update = time.time()
def __init__(self):
address = rospy.get_param('address', '127.0.0.1')
port = rospy.get_param('port', 2000)
self.map_location = rospy.get_param('map_location', 'meskel_square')
self.random_seed = rospy.get_param('random_seed', 1)
self.rng = random.Random(rospy.get_param('random_seed', 0))
sys.stdout.flush()
with (DATA_PATH /
'{}.sim_bounds'.format(self.map_location)).open('r') as f:
bounds_min = carla.Vector2D(
*[float(v) for v in f.readline().split(',')])
bounds_max = carla.Vector2D(
*[float(v) for v in f.readline().split(',')])
self.bounds_occupancy = carla.OccupancyMap(bounds_min, bounds_max)
sys.stdout.flush()
self.sumo_network = carla.SumoNetwork.load(
str(DATA_PATH / '{}.net.xml'.format(self.map_location)))
self.sumo_network_segments = self.sumo_network.create_segment_map()
self.sumo_network_spawn_segments = self.sumo_network_segments.intersection(
carla.OccupancyMap(bounds_min, bounds_max))
self.sumo_network_spawn_segments.seed_rand(self.rng.getrandbits(32))
self.sumo_network_occupancy = carla.OccupancyMap.load(
str(DATA_PATH / '{}.network.wkt'.format(self.map_location)))
sys.stdout.flush()
self.sidewalk = self.sumo_network_occupancy.create_sidewalk(1.5)
self.sidewalk_segments = self.sidewalk.create_segment_map()
self.sidewalk_spawn_segments = self.sidewalk_segments.intersection(
carla.OccupancyMap(bounds_min, bounds_max))
self.sidewalk_spawn_segments.seed_rand(self.rng.getrandbits(32))
self.sidewalk_occupancy = carla.OccupancyMap.load(
str(DATA_PATH / '{}.sidewalk.wkt'.format(self.map_location)))
sys.stdout.flush()
self.client = carla.Client(address, port)
self.client.set_timeout(10.0)
self.world = self.client.get_world()
sys.stdout.flush()
def main():
# Connect to client
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(2.0)
# Get World and Actors
world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()
# Get a random vehicle from world (there should be one at least)
vehicle = random.choice([actor for actor in actors if 'vehicle' in actor.type_id])
# Create Wheels Physics Control
front_left_wheel = carla.WheelPhysicsControl(tire_friction=4.5, damping_rate=1.0, max_steer_angle=70.0, radius=30.0)
front_right_wheel = carla.WheelPhysicsControl(tire_friction=2.5, damping_rate=1.5, max_steer_angle=70.0, radius=25.0)
rear_left_wheel = carla.WheelPhysicsControl(tire_friction=1.0, damping_rate=0.2, max_steer_angle=0.0, radius=15.0)
rear_right_wheel = carla.WheelPhysicsControl(tire_friction=1.5, damping_rate=1.3, max_steer_angle=0.0, radius=20.0)
wheels = [front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel]
# Change Vehicle Physics Control parameters of the vehicle
physics_control = vehicle.get_physics_control()
physics_control.torque_curve = [carla.Vector2D(x=0, y=400), carla.Vector2D(x=1300, y=600)]
physics_control.max_rpm = 100000
physics_control.moi = 1.0
physics_control.damping_rate_full_throttle = 0.0
physics_control.use_gear_autobox = True
physics_control.gear_switch_time = 0.5
physics_control.clutch_strength = 10
physics_control.mass = 10000
physics_control.drag_coefficient = 0.25
physics_control.steering_curve = [carla.Vector2D(x=0, y=1), carla.Vector2D(x=100, y=1), carla.Vector2D(x=300, y=1)]
physics_control.wheels = wheels
physics_control.use_sweep_wheel_collision = True
# Apply Vehicle Physics Control for the vehicle
vehicle.apply_physics_control(physics_control)
def publish_il_car_info(self, step=None):
car_info_msg = CarInfo()
pos = self.actor.get_location()
pos2D = carla.Vector2D(pos.x, pos.y)
vel = self.actor.get_velocity()
yaw = np.deg2rad(self.actor.get_transform().rotation.yaw)
v_2d = np.array([vel.x, vel.y, 0])
forward = np.array([math.cos(yaw), math.sin(yaw), 0])
speed = np.vdot(forward, v_2d)
self.speed = speed
car_info_msg.id = self.actor.id
car_info_msg.car_pos.x = pos.x
car_info_msg.car_pos.y = pos.y
car_info_msg.car_pos.z = pos.z
car_info_msg.car_yaw = yaw
car_info_msg.car_speed = speed
car_info_msg.car_steer = self.actor.get_control().steer
car_info_msg.car_vel.x = vel.x
car_info_msg.car_vel.y = vel.y
car_info_msg.car_vel.z = vel.z
car_info_msg.car_bbox = Polygon()
corners = get_bounding_box_corners(self.actor)
for corner in corners:
car_info_msg.car_bbox.points.append(
Point32(x=corner.x, y=corner.y, z=0.0))
car_info_msg.initial = False
if step is None:
wheels = self.actor.get_physics_control().wheels
# TODO I think that CARLA might have forgotten to divide by 100 here.
wheel_positions = [w.position / 100 for w in wheels]
front_axle_center = (wheel_positions[0] + wheel_positions[1]) / 2
rear_axle_center = (wheel_positions[2] + wheel_positions[3]) / 2
car_info_msg.front_axle_center.x = front_axle_center.x
car_info_msg.front_axle_center.y = front_axle_center.y
car_info_msg.front_axle_center.z = front_axle_center.z
car_info_msg.rear_axle_center.x = rear_axle_center.x
car_info_msg.rear_axle_center.y = rear_axle_center.y
car_info_msg.rear_axle_center.z = rear_axle_center.z
car_info_msg.max_steer_angle = wheels[0].max_steer_angle
car_info_msg.initial = True
try:
self.car_info_pub.publish(car_info_msg)
except Exception as e:
print(e)
def update_path(self, lane_decision):
if lane_decision == REMAIN:
return
pos = self.actor.get_location()
ego_veh_pos = carla.Vector2D(pos.x, pos.y)
yaw = np.deg2rad(self.actor.get_transform().rotation.yaw)
forward_vec = carla.Vector2D(math.cos(yaw), math.sin(yaw))
sidewalk_vec = forward_vec.rotate(
np.deg2rad(90)) # rotate clockwise by 90 degree
ego_veh_pos_in_new_lane = None
if lane_decision == CHANGE_LEFT:
ego_veh_pos_in_new_lane = ego_veh_pos - 4.0 * sidewalk_vec
else:
ego_veh_pos_in_new_lane = ego_veh_pos + 4.0 * sidewalk_vec
cur_route_point = self.sumo_network.get_nearest_route_point(
self.path.get_position(0))
new_route_point = self.sumo_network.get_nearest_route_point(
ego_veh_pos_in_new_lane)
lane_change_probability = 1.0
if new_route_point.edge == cur_route_point.edge and new_route_point.lane != cur_route_point.lane:
if self.rng.uniform(0.0, 1.0) <= lane_change_probability:
new_path_candidates = self.sumo_network.get_next_route_paths(
new_route_point, self.path.min_points - 1,
self.path.interval)
new_path = NetworkAgentPath(self.sumo_network,
self.path.min_points,
self.path.interval)
new_path.route_points = self.rng.choice(
new_path_candidates)[0:self.path.min_points]
print('NEW PATH!')
sys.stdout.flush()
self.path = new_path
def get_bounding_box_corners(actor):
bbox = actor.bounding_box
loc = carla.Vector2D(bbox.location.x,
bbox.location.y) + get_position(actor)
forward_vec = get_forward_direction(actor).make_unit_vector()
sideward_vec = forward_vec.rotate(np.deg2rad(90))
half_y_len = bbox.extent.y
half_x_len = bbox.extent.x
corners = [
loc - half_x_len * forward_vec + half_y_len * sideward_vec,
loc + half_x_len * forward_vec + half_y_len * sideward_vec,
loc + half_x_len * forward_vec - half_y_len * sideward_vec,
loc - half_x_len * forward_vec - half_y_len * sideward_vec
]
return corners
def get_pedestrian_bounding_box_corners(actor):
bbox = actor.bounding_box
loc = carla.Vector2D(bbox.location.x,
bbox.location.y) + get_position(actor)
forward_vec = get_forward_direction(actor).make_unit_vector()
sideward_vec = forward_vec.rotate(np.deg2rad(90))
# Hardcoded values for pedestrians.
half_y_len = 0.25
half_x_len = 0.25
corners = [
loc - half_x_len * forward_vec + half_y_len * sideward_vec,
loc + half_x_len * forward_vec + half_y_len * sideward_vec,
loc + half_x_len * forward_vec - half_y_len * sideward_vec,
loc - half_x_len * forward_vec - half_y_len * sideward_vec
]
return corners
def parse_vector_list(info):
"""
Parses a list of string into a list of Vector2D
Args:
info (list): list corresponding to a row of the recorder
"""
vector_list = []
for i in range(0, len(info), 2):
vector = carla.Vector2D(
float(info[i][1:-1]),
float(info[i + 1][:-1]),
)
vector_list.append(vector)
return vector_list
REMAIN = 0
CHANGE_RIGHT = 1
VEHICLE_STEER_KP = 1.5 # 2.0
PID_TUNING_ON = False
Pyro4.config.SERIALIZERS_ACCEPTED.add('serpent')
Pyro4.config.SERIALIZER = 'serpent'
Pyro4.util.SerializerBase.register_class_to_dict(
carla.Vector2D, lambda o: {
'__class__': 'carla.Vector2D',
'x': o.x,
'y': o.y
})
Pyro4.util.SerializerBase.register_dict_to_class(
'carla.Vector2D', lambda c, o: carla.Vector2D(o['x'], o['y']))
''' ========== UTILITY FUNCTIONS AND CLASSES ========== '''
class NetworkAgentPath:
def __init__(self, sumo_network, min_points, interval):
self.sumo_network = sumo_network
self.min_points = min_points
self.interval = interval
self.route_points = []
@staticmethod
def rand_path(sumo_network,
min_points,
interval,
segment_map,
def project(a): # LatLon -> CARLA coordinates.
return carla.Vector2D(lat2y(a[0]), lon2x(a[1]))
def main():
actor_list = []
try:
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
new_map = 'Town03'
print("Changing map to {}".format(new_map))
world = client.load_world(new_map)
world.set_weather(carla.WeatherParameters(cloudyness=20.0, sun_azimuth_angle=90, sun_altitude_angle=90))
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.1
world.apply_settings(settings)
spectator = world.get_spectator() # the spectator is the view of the simulator window
blueprint_library = world.get_blueprint_library()
bp = blueprint_library.find('vehicle.audi.tt')
color = '156,52,8'
bp.set_attribute('color', color)
# get the map of the world and the waypoint for the starting location
world_map = world.get_map()
starting_loc = ROUTE[0]
starting_wpt = world_map.get_waypoint(carla.Location(x=starting_loc[0], y=starting_loc[1], z=starting_loc[2]))
# spawn the vehicle at the chosen waypoint and add it to the actor list
vehicle = world.spawn_actor(bp, starting_wpt.transform)
actor_list.append(vehicle)
print('created %s' % vehicle.type_id)
vd = VehicleData()
lidar = add_lidar_sensor(world, vehicle, vd)
actor_list.append(lidar)
# vehicle.set_autopilot(True)
# move simulation view to center on vehicle, up high
world.tick()
world_snapshot = world.get_snapshot()
actor_snapshot = world_snapshot.find(vehicle.id)
spectator_transform = actor_snapshot.get_transform()
spectator_transform.location += carla.Location(x=0, y=0, z=100.0)
spectator_transform.rotation.pitch = -89
spectator_transform.rotation.yaw = -179
spectator_transform.rotation.roll = -1
spectator.set_transform(spectator_transform)
pc = vehicle.get_physics_control();
# get rid of the steering curve
pc.steering_curve = [carla.Vector2D(0.0, 1.0), carla.Vector2D(120.0, 1.0)];
vehicle.apply_physics_control(pc)
# wheel positions are in global coordinates and in centimeters, convert to wheelbase
wheel_xy = np.array([(pc.wheels[0].position.x - pc.wheels[2].position.x), (pc.wheels[0].position.y - pc.wheels[2].position.y)])
wheelbase = 1e-2*np.linalg.norm(wheel_xy)
# keep track of Vehicle State information
max_steer = pc.wheels[0].max_steer_angle
vd.config(wheelbase, max_steer)
# create a basic agent of the vehicle
agent = BasicAgent(vehicle, target_speed=40)
agent.set_destination_list(ROUTE)
# drive to waypoints until they are all gone
while len(agent._local_planner._waypoints_queue) > 0:
# for i in range(150):
# print(i)
# world.wait_for_tick(10.00)
world.tick()
snapshot = world.get_snapshot()
time = snapshot.elapsed_seconds
control = agent.run_step()
control.manual_gear_shift = False
vehicle.apply_control(control)
vd.appendTime(time)
vd.appendVelocityData(vehicle.get_velocity())
vd.appendControlData(control)
vd.appendTransformTruth(vehicle.get_transform())
control.brake = 1.0
control.throttle = 0.0
vehicle.apply_control(control)
world.tick()
if args.plot:
vd.runMotionModelFull()
vd.plot()
if args.vehicle_data_file is not None:
vd.saveToFile(args.vehicle_data_file)
print("Saved to {}".format(args.vehicle_data_file))
print('Finished following waypoints')
# END - Take the simulation out of synchronous/fixed time mode
settings = world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
if args.interact:
import code
code.interact(local=locals())
finally:
print('destroying actors')
for actor in actor_list:
if actor.is_alive:
actor.destroy()
print('done.')
def get_heading(actor):
heading = actor.get_transform().get_forward_vector()
return carla.Vector2D(heading.x, heading.y)
def main():
actor_list = []
try:
#connect to server
client = carla.Client('localhost', 2000)
client.set_timeout(2.0)
#load sumo network
sumo_network = carla.SumoNetwork.load(str(DATA_PATH/'{}.net.xml'.format("meskel_square")))
world = client.get_world()
# Get arbitrary position.
position = carla.Vector2D(350, 350)
# Get nearest route point on SUMO network.
route_point = sumo_network.get_nearest_route_point(position)
# Get route point position.
route_point_position = sumo_network.get_route_point_position(route_point)
print("route_point_position: ",route_point_position)
# Get route point transform
route_point_transform = carla.Transform(carla.Location(x=route_point_position.x,y=route_point_position.y,z=0.5))
print("route_point_transform: ",route_point_transform)
blueprint_library = world.get_blueprint_library()
#choose the vehicle you want
bp = blueprint_library.filter("model3")[0]
print(bp)
print("spawn_point: ",route_point_transform)
vehicle = world.spawn_actor(bp,route_point_transform)
#let vechicle move
#vehicle.apply_control(carla.VehicleControl(throttle=0.5,steer=0.0))
actor_list.append(vehicle)
#camera
camera_bp = blueprint_library.find('sensor.camera.rgb')
camera_bp.set_attribute("image_size_x",str(IM_WIDTH))
camera_bp.set_attribute("image_size_y",str(IM_HEIGHT))
camera_bp.set_attribute("fov","120")
#relative to center of vehicle (left hand coordination)
spawn_point = carla.Transform(carla.Location(x=-5,y=3,z=5),carla.Rotation(pitch=-40,yaw=-50))
#attach sensor to vehicle
camera = world.spawn_actor(camera_bp, spawn_point, attach_to=vehicle)
actor_list.append(camera)
camera.listen(lambda data : process_img(data))
time.sleep(20)
finally:
print('destroying actors')
for actor in actor_list:
#print(actor)
actor.destroy()
print('done.')
def test_named_args(self):
torque_curve = [[0, 400], [24, 56], [24, 56], [1315.47, 654.445],
[5729, 400]]
steering_curve = [
carla.Vector2D(x=0, y=1),
carla.Vector2D(x=20.0, y=0.9),
carla.Vector2D(x=63.0868, y=0.703473),
carla.Vector2D(x=119.12, y=0.573047)
]
wheels = [
carla.WheelPhysicsControl(tire_friction=2,
damping_rate=0,
steer_angle=30,
disable_steering=1),
carla.WheelPhysicsControl(tire_friction=2,
damping_rate=0,
steer_angle=30,
disable_steering=1),
carla.WheelPhysicsControl(tire_friction=2,
damping_rate=0,
steer_angle=30,
disable_steering=1),
carla.WheelPhysicsControl(tire_friction=2,
damping_rate=0,
steer_angle=30,
disable_steering=1)
]
pc = carla.VehiclePhysicsControl(
torque_curve=torque_curve,
max_rpm=5729,
moi=1,
damping_rate_full_throttle=0.15,
damping_rate_zero_throttle_clutch_engaged=2,
damping_rate_zero_throttle_clutch_disengaged=0.35,
use_gear_autobox=1,
gear_switch_time=0.5,
clutch_strength=10,
mass=5500,
drag_coefficient=0.3,
center_of_mass=carla.Vector3D(x=0.5, y=1, z=1),
steering_curve=steering_curve,
wheels=wheels)
error = .001
for i in range(0, len(torque_curve)):
self.assertTrue(
abs(pc.torque_curve[i].x - torque_curve[i][0]) <= error)
self.assertTrue(
abs(pc.torque_curve[i].y - torque_curve[i][1]) <= error)
self.assertTrue(abs(pc.max_rpm - 5729) <= error)
self.assertTrue(abs(pc.moi - 1) <= error)
self.assertTrue(abs(pc.damping_rate_full_throttle - 0.15) <= error)
self.assertTrue(
abs(pc.damping_rate_zero_throttle_clutch_engaged - 2) <= error)
self.assertTrue(
abs(pc.damping_rate_zero_throttle_clutch_disengaged -
0.35) <= error)
self.assertTrue(abs(pc.use_gear_autobox - 1) <= error)
self.assertTrue(abs(pc.gear_switch_time - 0.5) <= error)
self.assertTrue(abs(pc.clutch_strength - 10) <= error)
self.assertTrue(abs(pc.mass - 5500) <= error)
self.assertTrue(abs(pc.drag_coefficient - 0.3) <= error)
self.assertTrue(abs(pc.center_of_mass.x - 0.5) <= error)
self.assertTrue(abs(pc.center_of_mass.y - 1) <= error)
self.assertTrue(abs(pc.center_of_mass.z - 1) <= error)
for i in range(0, len(steering_curve)):
self.assertTrue(
abs(pc.steering_curve[i].x - steering_curve[i].x) <= error)
self.assertTrue(
abs(pc.steering_curve[i].y - steering_curve[i].y) <= error)
for i in range(0, len(wheels)):
self.assertTrue(
abs(pc.wheels[i].tire_friction -
wheels[i].tire_friction) <= error)
self.assertTrue(
abs(pc.wheels[i].damping_rate -
wheels[i].damping_rate) <= error)
self.assertTrue(
abs(pc.wheels[i].steer_angle - wheels[i].steer_angle) <= error)
self.assertEqual(pc.wheels[i].disable_steering,
wheels[i].disable_steering)
def get_velocity(actor):
v = actor.get_velocity()
return carla.Vector2D(v.x, v.y)
def get_position(actor):
pos3d = actor.get_location()
return carla.Vector2D(pos3d.x, pos3d.y)
def main(args):
# Load context data.
with (DATA_PATH / '{}.sim_bounds'.format(DATASET)).open('r') as f:
bounds_min = carla.Vector2D(
*[float(v) for v in f.readline().split(',')])
bounds_max = carla.Vector2D(
*[float(v) for v in f.readline().split(',')])
bounds_occupancy = carla.OccupancyMap(bounds_min, bounds_max)
sumo_network = carla.SumoNetwork.load(
str(DATA_PATH / '{}.net.xml'.format(DATASET)))
sumo_network_segments = sumo_network.create_segment_map()
sumo_network_spawn_segments = sumo_network_segments.intersection(
carla.OccupancyMap(bounds_min, bounds_max))
sumo_network_occupancy = carla.OccupancyMap.load(
str(DATA_PATH / '{}.network.wkt'.format(DATASET)))
sidewalk = sumo_network_occupancy.create_sidewalk(1.5)
# Connect to simulator.
client = carla.Client(args.host, args.port)
client.set_timeout(10.0)
world = client.get_world()
# Generate path.
spawn_point = sumo_network.get_nearest_route_point(SPAWN_POSITION)
ego_path = SumoNetworkAgentPath([spawn_point], 200, 1.0)
ego_path.resize(sumo_network)
# Calculate transform for spawning.
spawn_position = ego_path.get_position(sumo_network, 0)
spawn_heading_yaw = ego_path.get_yaw(sumo_network, 0)
spawn_transform = carla.Transform()
spawn_transform.location.x = spawn_position.x
spawn_transform.location.y = spawn_position.y
spawn_transform.location.z = 1.0
spawn_transform.rotation.yaw = spawn_heading_yaw
# Spawn ego actor and spectator camera.
ego_actor = world.spawn_actor(
world.get_blueprint_library().filter('vehicle.audi.etron')[0],
spawn_transform)
# Main control loop.
while True:
# Prepare GAMMA instance.
gamma = carla.RVOSimulator()
# Add exo-agents to GAMMA.
gamma_id = 0
for actor in world.get_actors():
if actor.id != ego_actor.id:
if isinstance(actor, carla.Vehicle):
if is_bike(actor):
type_tag = 'Bicycle'
else:
type_tag = 'Car'
bounding_box_corners = get_vehicle_bounding_box_corners(
actor)
elif isinstance(actor, carla.Walker):
type_tag = 'People'
bounding_box_corners = get_pedestrian_bounding_box_corners(
actor)
else:
continue
gamma.add_agent(carla.AgentParams.get_default(type_tag),
gamma_id)
gamma.set_agent_position(gamma_id, get_position(actor))
gamma.set_agent_velocity(gamma_id, get_velocity(actor))
gamma.set_agent_heading(gamma_id, get_forward_direction(actor))
gamma.set_agent_bounding_box_corners(gamma_id,
bounding_box_corners)
gamma.set_agent_pref_velocity(gamma_id, get_velocity(actor))
gamma_id += 1
# Extend ego path.
ego_path.cut(sumo_network, get_position(ego_actor))
ego_path.resize(sumo_network)
# Add ego-agent to GAMMA.
ego_id = gamma_id
gamma.add_agent(carla.AgentParams.get_default('Car'), ego_id)
gamma.set_agent_position(ego_id, get_position(ego_actor))
gamma.set_agent_velocity(ego_id, get_velocity(ego_actor))
gamma.set_agent_heading(ego_id, get_forward_direction(ego_actor))
gamma.set_agent_bounding_box_corners(
ego_id, get_vehicle_bounding_box_corners(ego_actor))
target_position = ego_path.get_position(sumo_network, 5)
pref_vel = MAX_SPEED * (target_position -
get_position(ego_actor)).make_unit_vector()
path_forward = (
ego_path.get_position(sumo_network, 1) -
ego_path.get_position(sumo_network, 0)).make_unit_vector()
gamma.set_agent_pref_velocity(ego_id, pref_vel)
gamma.set_agent_path_forward(ego_id, path_forward)
left_line_end = get_position(ego_actor) + (1.5 + 2.0 + 0.8) * (
(get_forward_direction(ego_actor).rotate(
np.deg2rad(-90))).make_unit_vector())
right_line_end = get_position(ego_actor) + (1.5 + 2.0 + 0.8) * (
(get_forward_direction(ego_actor).rotate(
np.deg2rad(90))).make_unit_vector())
left_lane_constrained_by_sidewalk = sidewalk.intersects(
carla.Segment2D(get_position(ego_actor), left_line_end))
right_lane_constrained_by_sidewalk = sidewalk.intersects(
carla.Segment2D(get_position(ego_actor), right_line_end))
# Flip left-right -> right-left since GAMMA uses a different handed coordinate system.
gamma.set_agent_lane_constraints(ego_id,
right_lane_constrained_by_sidewalk,
left_lane_constrained_by_sidewalk)
# Step GAMMA, and retrieve ego-agent's target velocity.
gamma.do_step()
target_vel = gamma.get_agent_velocity(ego_id)
# Calculate ego-vehicle control.
control = carla.VehicleControl()
control.throttle = KP_THROTTLE * (target_vel.length() -
get_velocity(ego_actor).length())
control.steer = KP_STEERING * get_signed_angle_diff(
target_vel, get_velocity(ego_actor))
control.manual_gear_shift = True # Reduces transmission lag.
control.gear = 1 # Reduces transmission lag.
ego_actor.apply_control(control)
time.sleep(0.1)
def array_to_vector2D(array):
vector = carla.Vector2D(array[0], array[1])
return vector
def update_gamma_lane_decision(self):
if not self.actor:
return
pos = self.actor.get_location()
pos2D = carla.Vector2D(pos.x, pos.y)
vel = self.actor.get_velocity()
yaw = np.deg2rad(self.actor.get_transform().rotation.yaw)
forward_vec = carla.Vector2D(math.cos(yaw), math.sin(yaw))
sidewalk_vec = forward_vec.rotate(
np.deg2rad(90)) # rotate clockwise by 90 degree
ego_veh_pos = pos2D
left_ego_veh_pos = ego_veh_pos - 4.0 * sidewalk_vec
right_ego_veh_pos = ego_veh_pos + 4.0 * sidewalk_vec
cur_route_point = self.sumo_network.get_nearest_route_point(
ego_veh_pos)
left_route_point = self.sumo_network.get_nearest_route_point(
left_ego_veh_pos)
right_route_point = self.sumo_network.get_nearest_route_point(
right_ego_veh_pos)
left_lane_exist = False
right_lane_exist = False
if left_route_point.edge == cur_route_point.edge and left_route_point.lane != cur_route_point.lane:
left_lane_exist = True
else:
left_lane_exist = False
if right_route_point.edge == cur_route_point.edge and right_route_point.lane != cur_route_point.lane:
right_lane_exist = True
else:
right_lane_exist = False
min_dist_to_front_veh = self.dist_to_nearest_agt_in_region(
ego_veh_pos,
forward_vec,
sidewalk_vec,
lookahead_x=30,
lookahead_y=4,
ref_point=None)
min_dist_to_left_front_veh = -1.0
min_dist_to_right_front_veh = -1.0
if left_lane_exist:
# if want to change lane, also need to consider vehicles behind
min_dist_to_left_front_veh = self.dist_to_nearest_agt_in_region(
left_ego_veh_pos,
forward_vec,
sidewalk_vec,
lookahead_x=35,
lookahead_y=4,
ref_point=left_ego_veh_pos - 12.0 * forward_vec,
consider_ped=True)
if right_lane_exist:
# if want to change lane, also need to consider vehicles behind
min_dist_to_right_front_veh = self.dist_to_nearest_agt_in_region(
right_ego_veh_pos,
forward_vec,
sidewalk_vec,
lookahead_x=35,
lookahead_y=4,
ref_point=right_ego_veh_pos - 12.0 * forward_vec,
consider_ped=True)
lane_decision = None
if min_dist_to_front_veh >= 15:
lane_decision = REMAIN
else:
if min_dist_to_left_front_veh > min_dist_to_right_front_veh:
if min_dist_to_left_front_veh > min_dist_to_front_veh + 3.0:
lane_decision = CHANGE_LEFT
else:
lane_decision = REMAIN
else: # min_dist_to_left_front_veh <= min_dist_to_right_front_veh:
if min_dist_to_right_front_veh > min_dist_to_front_veh + 3.0:
lane_decision = CHANGE_RIGHT
else:
lane_decision = REMAIN
if lane_decision != self.last_decision and lane_decision * self.last_decision != -1:
self.update_path(lane_decision)
self.last_decision = lane_decision
def get_forward_direction(actor):
forward = actor.get_transform().get_forward_vector()
return carla.Vector2D(forward.x, forward.y)
def update(self):
end_time = rospy.Time.now()
elapsed = (end_time - init_time).to_sec()
if not self.ego_car_info:
return
if len(self.world.get_actors()) > self.total_num_agents / 1.2 or time.time() -start_time > 15.0:
# print("[crowd_processor.py] {} crowd agents ready".format(
# len(self.world.get_actors())))
self.agents_ready_pub.publish(True)
else:
pass
# print("[crowd_processor.py] {} percent of agents ready".format(
# len(self.world.get_actors()) / float(self.total_num_agents)))
ego_car_position = self.ego_car_info.car_pos
ego_car_position = carla.Vector2D(
ego_car_position.x,
ego_car_position.y)
agents_msg = msg_builder.msg.TrafficAgentArray()
agents_path_msg = msg_builder.msg.AgentPathArray()
current_time = rospy.Time.now()
self.crowd_service.acquire_local_intentions()
local_intentions = self.crowd_service.local_intentions
self.crowd_service.release_local_intentions()
local_intentions_lookup = {}
for x in local_intentions:
local_intentions_lookup[x[0]] = x[1:]
for actor in self.world.get_actors():
if not actor.id in local_intentions_lookup:
continue
if actor is None:
continue
local_intention = local_intentions_lookup[actor.id]
actor_pos = carla.Vector2D(actor.get_location().x, actor.get_location().y)
if (actor_pos - ego_car_position).length() > 50: # TODO Add as ROS parameter.
continue
agent_tmp = msg_builder.msg.TrafficAgent()
agent_tmp.last_update = current_time
agent_tmp.id = actor.id
agent_tmp.type = {'Car': 'car', 'Bicycle': 'bike', 'People': 'ped'}[local_intention[0]]
agent_tmp.pose.position.x = actor.get_location().x
agent_tmp.pose.position.y = actor.get_location().y
agent_tmp.pose.position.z = actor.get_location().z
agent_tmp.vel.x = actor.get_velocity().x
agent_tmp.vel.y = actor.get_velocity().y
quat_tf = tf.transformations.quaternion_from_euler(
0, 0,
np.deg2rad(actor.get_transform().rotation.yaw))
agent_tmp.pose.orientation = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3])
agent_tmp.bbox = Polygon()
corners = get_bounding_box_corners(actor, expand=0.3)
for corner in corners:
agent_tmp.bbox.points.append(Point32(
x=corner.x, y=corner.y, z=0.0))
agents_msg.agents.append(agent_tmp)
agent_paths_tmp = msg_builder.msg.AgentPaths()
agent_paths_tmp.id = actor.id
agent_paths_tmp.type = {'Car': 'car', 'Bicycle': 'bike', 'People': 'ped'}[local_intention[0]]
if local_intention[0] in ['Car', 'Bicycle']:
initial_route_point = carla.SumoNetworkRoutePoint()
initial_route_point.edge = local_intention[1].edge
initial_route_point.lane = local_intention[1].lane
initial_route_point.segment = local_intention[1].segment
initial_route_point.offset = local_intention[1].offset
paths = [[initial_route_point]]
topological_hash = ''
# TODO Add 20, 1.0 as ROS paramters.
for _ in range(20):
next_paths = []
for path in paths:
next_route_points = self.sumo_network.get_next_route_points(path[-1], 1.0)
next_paths.extend(path + [route_point] for route_point in next_route_points)
if len(next_route_points) > 1:
topological_hash += '({},{},{},{}={})'.format(
path[-1].edge, path[-1].lane, path[-1].segment, path[-1].offset,
len(next_route_points))
paths = next_paths
agent_paths_tmp.reset_intention = self.topological_hash_map[actor.id] is None or \
self.topological_hash_map[actor.id] != topological_hash
for path in paths:
path_msg = Path()
path_msg.header.frame_id = 'map'
path_msg.header.stamp = current_time
for path_point in [actor_pos] + \
[self.sumo_network.get_route_point_position(route_point) for route_point in path]:
pose_msg = PoseStamped()
pose_msg.header.frame_id = 'map'
pose_msg.header.stamp = current_time
pose_msg.pose.position.x = path_point.x
pose_msg.pose.position.y = path_point.y
path_msg.poses.append(pose_msg)
# self.draw_path(path_msg)
agent_paths_tmp.path_candidates.append(path_msg)
agent_paths_tmp.cross_dirs = []
self.topological_hash_map[actor.id] = topological_hash
elif local_intention[0] == 'People':
sidewalk_route_point = carla.SidewalkRoutePoint()
sidewalk_route_point.polygon_id = local_intention[1].polygon_id
sidewalk_route_point.segment_id = local_intention[1].segment_id
sidewalk_route_point.offset = local_intention[1].offset
sidewalk_route_orientation = local_intention[2]
path = [sidewalk_route_point]
for _ in range(20):
if sidewalk_route_orientation:
path.append(self.sidewalk.get_next_route_point(path[-1], 1.0)) # TODO Add as ROS parameter.
else:
path.append(self.sidewalk.get_previous_route_point(path[-1], 1.0)) # TODO Add as ROS parameter.
topological_hash = '{},{}'.format(sidewalk_route_point.polygon_id, sidewalk_route_orientation)
agent_paths_tmp.reset_intention = self.topological_hash_map[actor.id] is None or \
self.topological_hash_map[actor.id] != topological_hash
path_msg = Path()
path_msg.header.frame_id = 'map'
path_msg.header.stamp = current_time
for path_point in [actor_pos] + [self.sidewalk.get_route_point_position(route_point) for route_point in
path]:
pose_msg = PoseStamped()
pose_msg.header.frame_id = 'map'
pose_msg.header.stamp = current_time
pose_msg.pose.position.x = path_point.x
pose_msg.pose.position.y = path_point.y
path_msg.poses.append(pose_msg)
# self.draw_path(path_msg)
agent_paths_tmp.path_candidates = [path_msg]
agent_paths_tmp.cross_dirs = [sidewalk_route_orientation]
self.topological_hash_map[actor.id] = topological_hash
agents_path_msg.agents.append(agent_paths_tmp)
try:
agents_msg.header.frame_id = 'map'
agents_msg.header.stamp = current_time
self.agents_pub.publish(agents_msg)
agents_path_msg.header.frame_id = 'map'
agents_path_msg.header.stamp = current_time
self.agents_path_pub.publish(agents_path_msg)
except Exception as e:
print(e)
# self.do_update = False
end_time = rospy.Time.now()
elapsed = (end_time - init_time).to_sec()
def get_position(self):
location = self.actor.get_location()
if location.z < -0.3:
print("Car dropped under ground")
self.ego_dead_pub.publish(True)
return carla.Vector2D(location.x, location.y)
