def get_distance_between_actors(current,
target,
distance_type="euclidianDistance",
freespace=False,
global_planner=None):
"""
This function finds the distance between actors for different use cases described by distance_type and freespace
attributes
"""
target_transform = CarlaDataProvider.get_transform(target)
current_transform = CarlaDataProvider.get_transform(current)
target_wp = CarlaDataProvider.get_map().get_waypoint(
target_transform.location)
current_wp = CarlaDataProvider.get_map().get_waypoint(
current_transform.location)
extent_sum_x, extent_sum_y = 0, 0
if freespace:
if isinstance(target, (carla.Vehicle, carla.Walker)):
extent_sum_x = target.bounding_box.extent.x + current.bounding_box.extent.x
extent_sum_y = target.bounding_box.extent.y + current.bounding_box.extent.y
if distance_type == "longitudinal":
if not current_wp.road_id == target_wp.road_id:
distance = 0
# Get the route
route = global_planner.trace_route(current_transform.location,
target_transform.location)
# Get the distance of the route
for i in range(1, len(route)):
curr_loc = route[i][0].transform.location
prev_loc = route[i - 1][0].transform.location
distance += curr_loc.distance(prev_loc)
else:
distance = abs(current_wp.s - target_wp.s)
if freespace:
distance = distance - extent_sum_x
elif distance_type == "lateral":
target_t = get_troad_from_transform(target_transform)
current_t = get_troad_from_transform(current_transform)
distance = abs(target_t - current_t)
if freespace:
distance = distance - extent_sum_y
elif distance_type in ["cartesianDistance", "euclidianDistance"]:
distance = target_transform.location.distance(
current_transform.location)
if freespace:
distance = distance - extent_sum_x
else:
raise TypeError("unknown distance_type: {}".format(distance_type))
# distance will be negative for feeespace when there is overlap condition
# truncate to 0.0 when this happens
distance = 0.0 if distance < 0.0 else distance
return distance
def initialise(self):
# In case of walkers, we have to extract the current heading
if self._type == 'walker':
self._control.speed = self._target_velocity
self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector()
super(AccelerateToVelocity, self).initialise()
def initialise(self):
self._location = CarlaDataProvider.get_location(self._actor)
self._start_time = GameTime.get_time()
# In case of walkers, we have to extract the current heading
if self._type == 'walker':
self._control.speed = self._target_velocity
self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector()
super(KeepVelocity, self).initialise()
def _set_new_velocity(self, next_location):
"""
Calculate and set the new actor veloctiy given the current actor
location and the _next_location_
Args:
next_location (carla.Location): Next target location of the actor
returns:
direction (carla.Vector3D): Normalized direction vector of the actor
"""
# set new linear velocity
velocity = carla.Vector3D(0, 0, 0)
direction = next_location - CarlaDataProvider.get_location(self._actor)
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
velocity.x = direction.x / direction_norm * self._target_speed
velocity.y = direction.y / direction_norm * self._target_speed
self._actor.set_velocity(velocity)
# set new angular velocity
current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw
new_yaw = CarlaDataProvider.get_map().get_waypoint(
next_location).transform.rotation.yaw
delta_yaw = new_yaw - current_yaw
if math.fabs(delta_yaw) > 360:
delta_yaw = delta_yaw % 360
if delta_yaw > 180:
delta_yaw = delta_yaw - 360
elif delta_yaw < -180:
delta_yaw = delta_yaw + 360
angular_velocity = carla.Vector3D(0, 0, 0)
angular_velocity.z = delta_yaw / (direction_norm / self._target_speed)
self._actor.set_angular_velocity(angular_velocity)
return direction_norm
def _set_new_velocity(self, next_location):
"""
Calculate and set the new actor veloctiy given the current actor
location and the _next_location_
If _consider_obstacles is true, the speed is adapted according to the closest
obstacle in front of the actor, if it is within the _proximity_threshold distance.
If _consider_trafficlights is true, the vehicle will enforce a stop at a red
traffic light.
If _max_deceleration is set, the vehicle will reduce its speed according to the
given deceleration value.
If the vehicle reduces its speed, braking lights will be activated.
Args:
next_location (carla.Location): Next target location of the actor
returns:
direction (carla.Vector3D): Length of direction vector of the actor
"""
current_time = GameTime.get_time()
target_speed = self._target_speed
if not self._last_update:
self._last_update = current_time
current_speed = math.sqrt(self._actor.get_velocity().x**2 +
self._actor.get_velocity().y**2)
if self._consider_obstacles:
# If distance is less than the proximity threshold, adapt velocity
if self._obstacle_distance < self._proximity_threshold:
distance = max(self._obstacle_distance, 0)
if distance > 0:
current_speed_other = math.sqrt(
self._obstacle_actor.get_velocity().x**2 +
self._obstacle_actor.get_velocity().y**2)
if current_speed_other < current_speed:
acceleration = -0.5 * (
current_speed - current_speed_other)**2 / distance
target_speed = max(
acceleration * (current_time - self._last_update) +
current_speed, 0)
else:
target_speed = 0
if self._consider_traffic_lights:
if (self._actor.is_at_traffic_light()
and self._actor.get_traffic_light_state()
== carla.TrafficLightState.Red):
target_speed = 0
if target_speed < current_speed:
self._actor.set_light_state(carla.VehicleLightState.Brake)
if self._max_deceleration is not None:
target_speed = max(
target_speed,
current_speed - (current_time - self._last_update) *
self._max_deceleration)
else:
self._actor.set_light_state(carla.VehicleLightState.NONE)
if self._max_acceleration is not None:
target_speed = min(
target_speed,
current_speed + (current_time - self._last_update) *
self._max_acceleration)
# set new linear velocity
velocity = carla.Vector3D(0, 0, 0)
direction = next_location - CarlaDataProvider.get_location(self._actor)
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
velocity.x = direction.x / direction_norm * target_speed
velocity.y = direction.y / direction_norm * target_speed
self._actor.set_target_velocity(velocity)
# set new angular velocity
current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw
# When we have a waypoint list, use the direction between the waypoints to calculate the heading (change)
# otherwise use the waypoint heading directly
if self._waypoints:
delta_yaw = math.degrees(math.atan2(direction.y,
direction.x)) - current_yaw
else:
new_yaw = CarlaDataProvider.get_map().get_waypoint(
next_location).transform.rotation.yaw
delta_yaw = new_yaw - current_yaw
if math.fabs(delta_yaw) > 360:
delta_yaw = delta_yaw % 360
if delta_yaw > 180:
delta_yaw = delta_yaw - 360
elif delta_yaw < -180:
delta_yaw = delta_yaw + 360
angular_velocity = carla.Vector3D(0, 0, 0)
if target_speed == 0:
angular_velocity.z = 0
else:
angular_velocity.z = delta_yaw / (direction_norm / target_speed)
self._actor.set_target_angular_velocity(angular_velocity)
self._last_update = current_time
return direction_norm
def _set_new_velocity(self, next_location):
"""
Calculate and set the new actor veloctiy given the current actor
location and the _next_location_
If _consider_obstacles is true, the speed is adapted according to the closest
obstacle in front of the actor, if it is within the _proximity_threshold distance.
Args:
next_location (carla.Location): Next target location of the actor
returns:
direction (carla.Vector3D): Length of direction vector of the actor
"""
current_time = GameTime.get_time()
target_speed = self._target_speed
if not self._last_update:
self._last_update = current_time
if self._consider_obstacles:
# If distance is less than the proximity threshold, adapt velocity
if self._obstacle_distance < self._proximity_threshold:
distance = max(self._obstacle_distance, 0)
if distance > 0:
current_speed = math.sqrt(self._actor.get_velocity().x**2 +
self._actor.get_velocity().y**2)
current_speed_other = math.sqrt(
self._obstacle_actor.get_velocity().x**2 +
self._obstacle_actor.get_velocity().y**2)
if current_speed_other < current_speed:
acceleration = -0.5 * (
current_speed - current_speed_other)**2 / distance
target_speed = max(
acceleration * (current_time - self._last_update) +
current_speed, 0)
else:
target_speed = 0
# set new linear velocity
velocity = carla.Vector3D(0, 0, 0)
direction = next_location - CarlaDataProvider.get_location(self._actor)
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
velocity.x = direction.x / direction_norm * target_speed
velocity.y = direction.y / direction_norm * target_speed
self._actor.set_velocity(velocity)
# set new angular velocity
current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw
new_yaw = CarlaDataProvider.get_map().get_waypoint(
next_location).transform.rotation.yaw
delta_yaw = new_yaw - current_yaw
if math.fabs(delta_yaw) > 360:
delta_yaw = delta_yaw % 360
if delta_yaw > 180:
delta_yaw = delta_yaw - 360
elif delta_yaw < -180:
delta_yaw = delta_yaw + 360
angular_velocity = carla.Vector3D(0, 0, 0)
if target_speed == 0:
angular_velocity.z = 0
else:
angular_velocity.z = delta_yaw / (direction_norm / target_speed)
self._actor.set_angular_velocity(angular_velocity)
self._last_update = current_time
return direction_norm
def gather_info(self, ego_control_and_speed_info=None):
# if self.step % 1 == 0:
# self.record_other_actor_info_for_causal_analysis(ego_control_and_speed_info)
ego_bbox = get_bbox(self._vehicle)
ego_front_bbox = ego_bbox[:2]
actors = self._world.get_actors()
vehicle_list = actors.filter('*vehicle*')
pedestrian_list = actors.filter('*walker*')
min_d = 10000
d_angle_norm = 1
for i, vehicle in enumerate(vehicle_list):
if vehicle.id == self._vehicle.id:
continue
d_angle_norm_i = angle_from_center_view_fov(vehicle,
self._vehicle,
fov=90)
d_angle_norm = np.min([d_angle_norm, d_angle_norm_i])
if d_angle_norm_i == 0:
other_bbox = get_bbox(vehicle)
for other_b in other_bbox:
for ego_b in ego_bbox:
d = norm_2d(other_b, ego_b)
# print('vehicle', i, 'd', d)
min_d = np.min([min_d, d])
for i, pedestrian in enumerate(pedestrian_list):
d_angle_norm_i = angle_from_center_view_fov(pedestrian,
self._vehicle,
fov=90)
d_angle_norm = np.min([d_angle_norm, d_angle_norm_i])
if d_angle_norm_i == 0:
pedestrian_location = pedestrian.get_transform().location
for ego_b in ego_front_bbox:
d = norm_2d(pedestrian_location, ego_b)
# print('pedestrian', i, 'd', d)
min_d = np.min([min_d, d])
if min_d < self.min_d:
self.min_d = min_d
with open(self.deviations_path, 'a') as f_out:
f_out.write('min_d,' + str(self.min_d) + '\n')
if d_angle_norm < self.d_angle_norm:
self.d_angle_norm = d_angle_norm
with open(self.deviations_path, 'a') as f_out:
f_out.write('d_angle_norm,' + str(self.d_angle_norm) + '\n')
angle_th = 120
current_location = CarlaDataProvider.get_location(self._vehicle)
current_transform = CarlaDataProvider.get_transform(self._vehicle)
current_waypoint = self._map.get_waypoint(current_location,
project_to_road=False,
lane_type=carla.LaneType.Any)
ego_forward = current_transform.get_forward_vector()
ego_forward = np.array([ego_forward.x, ego_forward.y])
ego_forward /= np.linalg.norm(ego_forward)
ego_right = current_transform.get_right_vector()
ego_right = np.array([ego_right.x, ego_right.y])
ego_right /= np.linalg.norm(ego_right)
lane_center_waypoint = self._map.get_waypoint(
current_location, lane_type=carla.LaneType.Any)
lane_center_transform = lane_center_waypoint.transform
lane_center_location = lane_center_transform.location
lane_forward = lane_center_transform.get_forward_vector()
lane_forward = np.array([lane_forward.x, lane_forward.y])
lane_forward /= np.linalg.norm(lane_forward)
lane_right = current_transform.get_right_vector()
lane_right = np.array([lane_right.x, lane_right.y])
lane_right /= np.linalg.norm(lane_right)
dev_dist = current_location.distance(lane_center_location)
# normalized to [0, 1]. 0 - same direction, 1 - opposite direction
# print('ego_forward, lane_forward, np.dot(ego_forward, lane_forward)', ego_forward, lane_forward, np.dot(ego_forward, lane_forward))
dev_angle = math.acos(np.clip(np.dot(ego_forward, lane_forward), -1,
1)) / np.pi
# smoothing and integrate
dev_dist *= (dev_angle + 0.5)
if dev_dist > self.dev_dist:
self.dev_dist = dev_dist
with open(self.deviations_path, 'a') as f_out:
f_out.write('dev_dist,' + str(self.dev_dist) + '\n')
# print(current_location, current_waypoint.lane_type, current_waypoint.is_junction)
# print(lane_center_location, lane_center_waypoint.lane_type, lane_center_waypoint.is_junction)
def get_d(coeff, dir, dir_label):
n = 1
while n * coeff < 7:
new_loc = carla.Location(
current_location.x + n * coeff * dir[0],
current_location.y + n * coeff * dir[1], 0)
# print(coeff, dir, dir_label)
# print(dir_label, 'current_location, dir, new_loc', current_location, dir, new_loc)
new_wp = self._map.get_waypoint(new_loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
if not (new_wp and new_wp.lane_type in [
carla.LaneType.Driving, carla.LaneType.Parking,
carla.LaneType.Bidirectional
] and np.abs(new_wp.transform.rotation.yaw % 360 -
lane_center_waypoint.transform.rotation.yaw % 360)
< angle_th):
# if new_wp and new_wp.lane_type in [carla.LaneType.Driving, carla.LaneType.Parking, carla.LaneType.Bidirectional]:
# print('new_wp.transform.rotation.yaw, lane_center_waypoint.transform.rotation.yaw', new_wp.transform.rotation.yaw, lane_center_waypoint.transform.rotation.yaw)
break
else:
n += 1
# if new_wp:
# print(n, new_wp.transform.rotation.yaw)
d = new_loc.distance(current_location)
# print(d, new_loc, current_location)
with open(self.deviations_path, 'a') as f_out:
if new_wp and new_wp.lane_type in [
carla.LaneType.Driving, carla.LaneType.Parking,
carla.LaneType.Bidirectional
]:
# print(dir_label, 'wronglane_d', d)
if d < self.wronglane_d:
self.wronglane_d = d
f_out.write('wronglane_d,' + str(self.wronglane_d) +
'\n')
# print(dir_label, 'current_location, dir, new_loc', current_location, dir, new_loc, 'wronglane_d,'+str(self.wronglane_d)+'\n')
else:
# if not new_wp:
# s = 'None wp'
# else:
# s = new_wp.lane_type
# print(dir_label, 'offroad_d', d, s, coeff)
# if new_wp:
# print(dir_label, 'lanetype', new_wp.lane_type)
if d < self.offroad_d:
self.offroad_d = d
f_out.write('offroad_d,' + str(self.offroad_d) + '\n')
# print(dir_label, 'current_location, dir, new_loc', current_location, dir, new_loc, 'offroad_d,'+str(self.offroad_d)+'\n')
if current_waypoint and not current_waypoint.is_junction:
get_d(-0.1, lane_right, 'left')
get_d(0.1, lane_right, 'right')
get_d(-0.1, ego_right, 'ego_left')
get_d(0.1, ego_right, 'ego_right')
get_d(0.1, ego_forward, 'ego_forward')
def gather_info(self):
def norm_2d(loc_1, loc_2):
return np.sqrt((loc_1.x - loc_2.x)**2 + (loc_1.y - loc_2.y)**2)
def get_bbox(vehicle):
current_tra = vehicle.get_transform()
current_loc = current_tra.location
heading_vec = current_tra.get_forward_vector()
heading_vec.z = 0
heading_vec = heading_vec / math.sqrt(
math.pow(heading_vec.x, 2) + math.pow(heading_vec.y, 2))
perpendicular_vec = carla.Vector3D(-heading_vec.y, heading_vec.x,
0)
extent = vehicle.bounding_box.extent
x_boundary_vector = heading_vec * extent.x
y_boundary_vector = perpendicular_vec * extent.y
bbox = [
current_loc +
carla.Location(x_boundary_vector - y_boundary_vector),
current_loc +
carla.Location(x_boundary_vector + y_boundary_vector),
current_loc +
carla.Location(-1 * x_boundary_vector - y_boundary_vector),
current_loc +
carla.Location(-1 * x_boundary_vector + y_boundary_vector)
]
return bbox
ego_bbox = get_bbox(self._vehicle)
ego_front_bbox = ego_bbox[:2]
actors = self._world.get_actors()
vehicle_list = actors.filter('*vehicle*')
pedestrian_list = actors.filter('*walker*')
min_d = 10000
for i, vehicle in enumerate(vehicle_list):
if vehicle.id == self._vehicle.id:
continue
other_bbox = get_bbox(vehicle)
for other_b in other_bbox:
for ego_b in ego_bbox:
d = norm_2d(other_b, ego_b)
# print('vehicle', i, 'd', d)
min_d = np.min([min_d, d])
for i, pedestrian in enumerate(pedestrian_list):
pedestrian_location = pedestrian.get_transform().location
for ego_b in ego_front_bbox:
d = norm_2d(pedestrian_location, ego_b)
# print('pedestrian', i, 'd', d)
min_d = np.min([min_d, d])
if min_d < self.min_d:
self.min_d = min_d
with open(self.deviations_path, 'a') as f_out:
f_out.write('min_d,' + str(self.min_d) + '\n')
angle_th = 120
current_location = CarlaDataProvider.get_location(self._vehicle)
current_transform = CarlaDataProvider.get_transform(self._vehicle)
current_waypoint = self._map.get_waypoint(current_location,
project_to_road=False,
lane_type=carla.LaneType.Any)
lane_center_waypoint = self._map.get_waypoint(
current_location, lane_type=carla.LaneType.Any)
lane_center_transform = lane_center_waypoint.transform
lane_center_location = lane_center_transform.location
dev_dist = current_location.distance(lane_center_location)
if dev_dist > self.dev_dist:
self.dev_dist = dev_dist
with open(self.deviations_path, 'a') as f_out:
f_out.write('dev_dist,' + str(self.dev_dist) + '\n')
# print(current_location, current_waypoint.lane_type, current_waypoint.is_junction)
# print(lane_center_location, lane_center_waypoint.lane_type, lane_center_waypoint.is_junction)
if current_waypoint and not current_waypoint.is_junction:
ego_forward = current_transform.get_forward_vector()
lane_forward = lane_center_transform.get_forward_vector()
dev_angle = 2 * get_angle(lane_forward.x, lane_forward.y,
ego_forward.x, ego_forward.y) / np.pi
# print(lane_forward, ego_forward, dev_angle)
if dev_angle > 1:
dev_angle = 2 - dev_angle
elif dev_angle < -1:
dev_angle = (-1) * (2 + dev_angle)
# carla map has opposite y axis
dev_angle *= -1
def get_d(coeff, dev_angle):
if coeff < 0:
dev_angle = 1 - dev_angle
elif coeff > 0:
dev_angle = dev_angle + 1
# print(dev_angle, coeff)
n = 1
rv = lane_center_waypoint.transform.get_right_vector()
new_loc = carla.Location(
lane_center_location.x + n * coeff * rv.x,
lane_center_location.y + n * coeff * rv.y, 0)
new_wp = self._map.get_waypoint(new_loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
while new_wp and new_wp.lane_type in [
carla.LaneType.Driving, carla.LaneType.Parking,
carla.LaneType.Bidirectional
] and np.abs(new_wp.transform.rotation.yaw -
lane_center_waypoint.transform.rotation.yaw
) < angle_th:
prev_loc = new_loc
n += 1
new_loc = carla.Location(
lane_center_location.x + n * coeff * rv.x,
lane_center_location.y + n * coeff * rv.y, 0)
new_wp = self._map.get_waypoint(
new_loc,
project_to_road=False,
lane_type=carla.LaneType.Any)
# if new_wp:
# print(n, new_wp.transform.rotation.yaw)
d = new_loc.distance(current_location)
d *= dev_angle
# print(d, new_loc, current_location)
with open(self.deviations_path, 'a') as f_out:
if (not new_wp) or (new_wp.lane_type not in [
carla.LaneType.Driving, carla.LaneType.Parking,
carla.LaneType.Bidirectional
]):
if not new_wp:
s = 'None wp'
else:
s = new_wp.lane_type
# print('offroad_d', d, s, coeff)
# if new_wp:
# print('lanetype', new_wp.lane_type)
if d < self.offroad_d:
self.offroad_d = d
with open(self.deviations_path, 'a') as f_out:
f_out.write('offroad_d,' +
str(self.offroad_d) + '\n')
else:
with open(self.deviations_path, 'a') as f_out:
# print('wronglane_d', d, coeff)
if d < self.wronglane_d:
self.wronglane_d = d
f_out.write('wronglane_d,' +
str(self.wronglane_d) + '\n')
get_d(-0.2, dev_angle)
get_d(0.2, dev_angle)
def update(self):
"""
Compute next control step for the given waypoint plan, obtain and apply control to actor
"""
new_status = py_trees.common.Status.RUNNING
check_term = operator.attrgetter("terminate_WF_actor_{}".format(self._actor.id))
terminate_wf = check_term(py_trees.blackboard.Blackboard())
check_run = operator.attrgetter("running_WF_actor_{}".format(self._actor.id))
active_wf = check_run(py_trees.blackboard.Blackboard())
# Termination of WF if the WFs unique_id is listed in terminate_wf
# only one WF should be active, therefore all previous WF have to be terminated
if self._unique_id in terminate_wf:
terminate_wf.remove(self._unique_id)
if self._unique_id in active_wf:
active_wf.remove(self._unique_id)
py_trees.blackboard.Blackboard().set(
"terminate_WF_actor_{}".format(self._actor.id), terminate_wf, overwrite=True)
py_trees.blackboard.Blackboard().set(
"running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True)
new_status = py_trees.common.Status.SUCCESS
return new_status
if self._blackboard_queue_name is not None:
while not self._queue.empty():
actor = self._queue.get()
if actor is not None and actor not in self._actor_dict:
self._apply_local_planner(actor)
success = True
for actor in self._local_planner_dict:
local_planner = self._local_planner_dict[actor] if actor else None
if actor is not None and actor.is_alive and local_planner is not None:
# Check if the actor is a vehicle/bike
if not isinstance(actor, carla.Walker):
control = local_planner.run_step(debug=False)
if self._avoid_collision and detect_lane_obstacle(actor):
control.throttle = 0.0
control.brake = 1.0
actor.apply_control(control)
# Check if the actor reached the end of the plan
# @TODO replace access to private _waypoints_queue with public getter
if local_planner._waypoints_queue: # pylint: disable=protected-access
success = False
# If the actor is a pedestrian, we have to use the WalkerAIController
# The walker is sent to the next waypoint in its plan
else:
actor_location = CarlaDataProvider.get_location(actor)
success = False
if self._actor_dict[actor]:
location = self._actor_dict[actor][0]
direction = location - actor_location
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
control = actor.get_control()
control.speed = self._target_speed
control.direction = direction / direction_norm
actor.apply_control(control)
if direction_norm < 1.0:
self._actor_dict[actor] = self._actor_dict[actor][1:]
if self._actor_dict[actor] is None:
success = True
else:
control = actor.get_control()
control.speed = self._target_speed
control.direction = CarlaDataProvider.get_transform(actor).rotation.get_forward_vector()
actor.apply_control(control)
if success:
new_status = py_trees.common.Status.SUCCESS
return new_status
