def update(self):
"""
Check if the actor location is within trigger region
"""
new_status = py_trees.common.Status.RUNNING
location = CarlaDataProvider.get_location(self._actor)
if location is None:
return new_status
if self._terminate_on_failure and (self.test_status == "FAILURE"):
new_status = py_trees.common.Status.FAILURE
elif self.test_status == "RUNNING" or self.test_status == "INIT":
# are we too far away from the route waypoints (i.e., off route)?
off_route = True
for waypoint in self._waypoints:
distance = math.sqrt(((location.x - waypoint.x)**2) +
((location.y - waypoint.y)**2))
if distance < self._radius:
off_route = False
break
if off_route:
self._counter_off_route += 1
if self._counter_off_route > self._offroad_max:
route_deviation_event = TrafficEvent(
type=TrafficEventType.ROUTE_DEVIATION)
route_deviation_event.set_message(
"Agent deviated from the route at (x={}, y={}, z={})".
format(location.x, location.y, location.z))
route_deviation_event.set_dict({
'x': location.x,
'y': location.y,
'z': location.z
})
self.list_traffic_events.append(route_deviation_event)
self.test_status = "FAILURE"
new_status = py_trees.common.Status.FAILURE
self.logger.debug("%s.update()[%s->%s]" %
(self.__class__.__name__, self.status, new_status))
return new_status
def update(self):
new_status = py_trees.common.Status.RUNNING
current_location = CarlaDataProvider.get_location(self._actor)
if current_location is None:
return new_status
if current_location.distance(self._location) < self._distance + 20:
actor_distance, _ = get_distance_along_route(self._route, current_location)
if (self._location_distance < actor_distance + self._distance and
actor_distance < self._location_distance) or \
self._location_distance < 1.0:
new_status = py_trees.common.Status.SUCCESS
return new_status
def update(self):
new_status = py_trees.common.Status.RUNNING
location = CarlaDataProvider.get_location(self.actor)
waypoint = CarlaDataProvider.get_map().get_waypoint(location)
# Wait for the actor to enter a junction
if not self.inside_junction and waypoint.is_junction:
self.inside_junction = True
# And to leave it
if self.inside_junction and not waypoint.is_junction:
if self.debug:
print("--- Leaving the junction")
new_status = py_trees.common.Status.SUCCESS
return new_status
def update(self):
"""
Check if the actor is within trigger distance to the intersection
"""
new_status = py_trees.common.Status.RUNNING
current_waypoint = self._map.get_waypoint(
CarlaDataProvider.get_location(self._actor))
distance = calculate_distance(current_waypoint.transform.location,
self._final_location)
if distance < self._distance:
new_status = py_trees.common.Status.SUCCESS
self.logger.debug("%s.update()[%s->%s]" %
(self.__class__.__name__, self.status, new_status))
return new_status
def update(self):
"""
Check if the actor is within trigger distance to the target location
"""
new_status = py_trees.common.Status.RUNNING
location = CarlaDataProvider.get_location(self._actor)
if location is None:
return new_status
if calculate_distance(
location, self._target_location) < self._distance:
new_status = py_trees.common.Status.SUCCESS
self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
return new_status
def run_step(self):
"""
Execute on tick of the controller's control loop
If _waypoints are provided, the vehicle moves towards the next waypoint
with the given _target_speed, until reaching the final waypoint. Upon reaching
the final waypoint, _reached_goal is set to True.
If _waypoints is empty, the vehicle moves in its current direction with
the given _target_speed.
"""
self._reached_goal = False
if not self._waypoints:
# get next waypoint from map, to avoid leaving the road
# then navigate to this waypoint
self._reached_goal = False
map_wp = None
if not self._generated_waypoint_list:
map_wp = CarlaDataProvider.get_map().get_waypoint(
CarlaDataProvider.get_location(self._actor))
else:
map_wp = CarlaDataProvider.get_map().get_waypoint(
self._generated_waypoint_list[-1].location)
while len(self._generated_waypoint_list) < 50:
map_wp = map_wp.next(2.0)[0]
self._generated_waypoint_list.append(map_wp.transform)
direction_norm = self._set_new_velocity(
self._generated_waypoint_list[0].location)
if direction_norm < 1.0:
self._generated_waypoint_list = self._generated_waypoint_list[
1:]
else:
# calculate required heading to reach next waypoint
self._reached_goal = False
direction_norm = self._set_new_velocity(
self._waypoints[0].location)
if direction_norm < 1.0:
self._waypoints = self._waypoints[1:]
if not self._waypoints:
self._reached_goal = True
def update(self):
"""
Check if actor is in trigger distance
"""
new_status = py_trees.common.Status.RUNNING
# calculate transform with method in openscenario_parser.py
osc_transform = srunner.tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform(
self._osc_position)
if osc_transform is not None:
osc_location = osc_transform.location
actor_location = CarlaDataProvider.get_location(self._actor)
distance = calculate_distance(osc_location, actor_location)
if self._comparison_operator(distance, self._distance):
new_status = py_trees.common.Status.SUCCESS
return new_status
def update(self):
"""
Check if the _actor location is within trigger region
"""
new_status = py_trees.common.Status.RUNNING
location = CarlaDataProvider.get_location(self._actor)
if location is None:
return new_status
not_in_region = (location.x < self._min_x or location.x > self._max_x) or (
location.y < self._min_y or location.y > self._max_y)
if not not_in_region:
new_status = py_trees.common.Status.SUCCESS
self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
return new_status
def update(self):
"""
Check if the actor location is within trigger region
"""
new_status = py_trees.common.Status.RUNNING
location = CarlaDataProvider.get_location(self._actor)
if location is None:
return new_status
if self._terminate_on_failure and (self.test_status == "FAILURE"):
new_status = py_trees.common.Status.FAILURE
elif self.test_status == "RUNNING" or self.test_status == "INIT":
# are we too far away from the route waypoints (i.e., off route)?
off_route = True
shortest_distance = float('inf')
for index in range(max(0, self._current_index - self._wsize),
min(self._current_index + self._wsize + 1, self._route_length)):
# look for the distance to the current waipoint + windows_size
ref_waypoint = self._waypoints[index]
distance = math.sqrt(((location.x - ref_waypoint.x) ** 2) + ((location.y - ref_waypoint.y) ** 2))
if distance < self.DISTANCE_THRESHOLD \
and distance <= shortest_distance \
and index >= self._current_index:
shortest_distance = distance
self._current_index = index
off_route = False
if off_route:
route_deviation_event = TrafficEvent(event_type=TrafficEventType.ROUTE_DEVIATION)
route_deviation_event.set_message("Agent deviated from the route at (x={}, y={}, z={})".format(
location.x, location.y, location.z))
route_deviation_event.set_dict({'x': location.x, 'y': location.y, 'z': location.z})
self.list_traffic_events.append(route_deviation_event)
self.test_status = "FAILURE"
new_status = py_trees.common.Status.FAILURE
self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
return new_status
def update(self):
"""
Check if the actor location is within trigger region
"""
new_status = py_trees.common.Status.RUNNING
location = CarlaDataProvider.get_location(self._actor)
if location is None:
return new_status
if self._terminate_on_failure and (self.test_status == "FAILURE"):
new_status = py_trees.common.Status.FAILURE
elif self.test_status == "RUNNING" or self.test_status == "INIT":
for index in range(self._current_index, min(self._current_index + self._wsize + 1, self._route_length)):
# look for the distance to the current waipoint + windows_size
ref_waypoint = self._waypoints[index]
distance = math.sqrt(((location.x - ref_waypoint.x) ** 2) + ((location.y - ref_waypoint.y) ** 2))
if distance < self.DISTANCE_THRESHOLD:
# good! segment completed!
self._current_index = index
self._percentage_route_completed = 100.0 * float(self._accum_meters[self._current_index]) \
/ float(self._accum_meters[-1])
self._traffic_event.set_dict({'route_completed': self._percentage_route_completed})
self._traffic_event.set_message(
"Agent has completed > {:.2f}% of the route".format(self._percentage_route_completed))
if self._percentage_route_completed > 99.0 and location.distance(self.target) < self.DISTANCE_THRESHOLD:
route_completion_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETED)
route_completion_event.set_message("Destination was successfully reached")
self.list_traffic_events.append(route_completion_event)
self.test_status = "SUCCESS"
elif self.test_status == "SUCCESS":
new_status = py_trees.common.Status.SUCCESS
self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
self.actual_value = self._percentage_route_completed
return new_status
def update(self):
"""
Check actor waypoints
"""
new_status = py_trees.common.Status.RUNNING
location = CarlaDataProvider.get_location(self._actor)
waypoint = self._map.get_waypoint(location)
if waypoint is None:
return new_status
right_waypoint = waypoint.get_right_lane()
if right_waypoint is None:
return new_status
lane_type = right_waypoint.lane_type
if lane_type != carla.LaneType.Driving:
new_status = py_trees.common.Status.SUCCESS
self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
return new_status
def update(self):
"""
Check if actor can arrive within trigger time
"""
new_status = py_trees.common.Status.RUNNING
# calculate transform with method in openscenario_parser.py
try:
osc_transform = sr_tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform(
self._osc_position)
except AttributeError:
return py_trees.common.Status.FAILURE
target_location = osc_transform.location
actor_location = CarlaDataProvider.get_location(self._actor)
if target_location is None or actor_location is None:
return new_status
if self._along_route:
# Global planner needs a location at a driving lane
actor_location = self._map.get_waypoint(
actor_location).transform.location
target_location = self._map.get_waypoint(
target_location).transform.location
distance = calculate_distance(actor_location, target_location,
self._grp)
actor_velocity = CarlaDataProvider.get_velocity(self._actor)
# time to arrival
if actor_velocity > 0:
time_to_arrival = distance / actor_velocity
elif distance == 0:
time_to_arrival = 0
else:
time_to_arrival = float('inf')
if self._comparison_operator(time_to_arrival, self._time):
new_status = py_trees.common.Status.SUCCESS
return new_status
def update(self):
"""
Check velocity
"""
new_status = py_trees.common.Status.RUNNING
if self.actor is None:
return new_status
location = CarlaDataProvider.get_location(self.actor)
if location is None:
return new_status
if self._last_location is None:
self._last_location = location
return new_status
self._distance += location.distance(self._last_location)
self._last_location = location
elapsed_time = GameTime.get_time()
if elapsed_time > 0.0:
self.actual_value = self._distance / elapsed_time
if self.actual_value > self.expected_value_success:
self.test_status = "SUCCESS"
elif (self.expected_value_acceptable is not None
and self.actual_value > self.expected_value_acceptable):
self.test_status = "ACCEPTABLE"
else:
self.test_status = "RUNNING"
if self._terminate_on_failure and (self.test_status == "FAILURE"):
new_status = py_trees.common.Status.FAILURE
self.logger.debug("%s.update()[%s->%s]" %
(self.__class__.__name__, self.status, new_status))
return new_status
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 update(self):
"""
Check driven distance
"""
new_status = py_trees.common.Status.RUNNING
new_location = CarlaDataProvider.get_location(self._actor)
self._distance += calculate_distance(self._location, new_location)
self._location = new_location
if self.tmp_travel_dist_file:
with open(self.tmp_travel_dist_file, 'a') as f_out:
f_out.write(
','.join([str(self._actor.id),
str(self._distance)]) + '\n')
if self._distance > self._target_distance:
new_status = py_trees.common.Status.SUCCESS
self.logger.debug("%s.update()[%s->%s]" %
(self.__class__.__name__, self.status, new_status))
return new_status
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
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 initialise(self):
self._location = CarlaDataProvider.get_location(self._actor)
super(DriveDistance, self).initialise()
def _count_collisions(weak_self, event): # pylint: disable=too-many-return-statements
"""
Callback to update collision count
"""
self = weak_self()
if not self:
return
actor_location = CarlaDataProvider.get_location(self.actor)
# Ignore the current one if it is the same id as before
if self.last_id == event.other_actor.id:
return
# Filter to only a specific actor
if self.other_actor and self.other_actor.id != event.other_actor.id:
return
# Filter to only a specific type
if self.other_actor_type:
if self.other_actor_type == "miscellaneous":
if "traffic" not in event.other_actor.type_id \
and "static" not in event.other_actor.type_id:
return
else:
if self.other_actor_type not in event.other_actor.type_id:
return
# Ignore it if its too close to a previous collision (avoid micro collisions)
for collision_location in self.registered_collisions:
distance_vector = actor_location - collision_location
distance = math.sqrt(
math.pow(distance_vector.x, 2) +
math.pow(distance_vector.y, 2))
if distance <= self.MIN_AREA_OF_COLLISION:
return
if ('static' in event.other_actor.type_id or 'traffic' in event.other_actor.type_id) \
and 'sidewalk' not in event.other_actor.type_id:
actor_type = TrafficEventType.COLLISION_STATIC
elif 'vehicle' in event.other_actor.type_id:
actor_type = TrafficEventType.COLLISION_VEHICLE
elif 'walker' in event.other_actor.type_id:
actor_type = TrafficEventType.COLLISION_PEDESTRIAN
else:
return
collision_event = TrafficEvent(event_type=actor_type)
collision_event.set_dict({
'type': event.other_actor.type_id,
'id': event.other_actor.id,
'x': actor_location.x,
'y': actor_location.y,
'z': actor_location.z
})
collision_event.set_message(
"Agent collided against object with type={} and id={} at (x={}, y={}, z={})"
.format(event.other_actor.type_id, event.other_actor.id,
round(actor_location.x, 3), round(actor_location.y, 3),
round(actor_location.z, 3)))
self.test_status = "FAILURE"
self.actual_value += 1
self.collision_time = GameTime.get_time()
self.registered_collisions.append(actor_location)
self.list_traffic_events.append(collision_event)
# Number 0: static objects -> ignore it
if event.other_actor.id != 0:
self.last_id = event.other_actor.id
def initialise(self):
# get initial actor position
self._initial_actor_pos = CarlaDataProvider.get_location(self._actor)
def run_step(self):
"""
Execute on tick of the controller's control loop
If _waypoints are provided, the vehicle moves towards the next waypoint
with the given _target_speed, until reaching the final waypoint. Upon reaching
the final waypoint, _reached_goal is set to True.
If _waypoints is empty, the vehicle moves in its current direction with
the given _target_speed.
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 self._cv_image is not None:
cv2.imshow("", self._cv_image)
cv2.waitKey(1)
if self._reached_goal:
# Reached the goal, so stop
velocity = carla.Vector3D(0, 0, 0)
self._actor.set_velocity(velocity)
return
self._reached_goal = False
if not self._waypoints:
# No waypoints are provided, so we have to create a list of waypoints internally
# get next waypoints from map, to avoid leaving the road
self._reached_goal = False
map_wp = None
if not self._generated_waypoint_list:
map_wp = CarlaDataProvider.get_map().get_waypoint(CarlaDataProvider.get_location(self._actor))
else:
map_wp = CarlaDataProvider.get_map().get_waypoint(self._generated_waypoint_list[-1].location)
while len(self._generated_waypoint_list) < 50:
map_wps = map_wp.next(3.0)
if map_wps:
self._generated_waypoint_list.append(map_wps[0].transform)
map_wp = map_wps[0]
else:
break
direction_norm = self._set_new_velocity(self._generated_waypoint_list[0].location)
if direction_norm < 2.0:
self._generated_waypoint_list = self._generated_waypoint_list[1:]
else:
# When changing from "free" driving without pre-defined waypoints to a defined route with waypoints
# it may happen that the first few waypoints are too close to the ego vehicle for obtaining a
# reasonable control command. Therefore, we drop these waypoints first.
while self._waypoints and self._waypoints[0].location.distance(self._actor.get_location()) < 0.5:
self._waypoints = self._waypoints[1:]
self._reached_goal = False
direction_norm = self._set_new_velocity(self._waypoints[0].location)
if direction_norm < 4.0:
self._waypoints = self._waypoints[1:]
if not self._waypoints:
self._reached_goal = True
def initialise(self):
self._last_location = CarlaDataProvider.get_location(self.actor)
super(AverageVelocityTest, self).initialise()
def initialise(self):
self._last_location = CarlaDataProvider.get_location(self.actor)
super(DrivenDistanceTest, self).initialise()
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 run_step(self):
"""
Execute on tick of the controller's control loop
If _waypoints are provided, the vehicle moves towards the next waypoint
with the given _target_speed, until reaching the final waypoint. Upon reaching
the final waypoint, _reached_goal is set to True.
If _waypoints is empty, the vehicle moves in its current direction with
the given _target_speed.
For further details see :func:`_set_new_velocity`
"""
if self._reached_goal:
# Reached the goal, so stop
velocity = carla.Vector3D(0, 0, 0)
self._actor.set_target_velocity(velocity)
return
if self._visualizer:
self._visualizer.render()
self._reached_goal = False
if not self._waypoints:
# No waypoints are provided, so we have to create a list of waypoints internally
# get next waypoints from map, to avoid leaving the road
self._reached_goal = False
map_wp = None
if not self._generated_waypoint_list:
map_wp = CarlaDataProvider.get_map().get_waypoint(
CarlaDataProvider.get_location(self._actor))
else:
map_wp = CarlaDataProvider.get_map().get_waypoint(
self._generated_waypoint_list[-1].location)
while len(self._generated_waypoint_list) < 50:
map_wps = map_wp.next(2.0)
if map_wps:
self._generated_waypoint_list.append(map_wps[0].transform)
map_wp = map_wps[0]
else:
break
# Remove all waypoints that are too close to the vehicle
while (self._generated_waypoint_list
and self._generated_waypoint_list[0].location.distance(
self._actor.get_location()) < 0.5):
self._generated_waypoint_list = self._generated_waypoint_list[
1:]
direction_norm = self._set_new_velocity(
self._offset_waypoint(self._generated_waypoint_list[0]))
if direction_norm < 2.0:
self._generated_waypoint_list = self._generated_waypoint_list[
1:]
else:
# When changing from "free" driving without pre-defined waypoints to a defined route with waypoints
# it may happen that the first few waypoints are too close to the ego vehicle for obtaining a
# reasonable control command. Therefore, we drop these waypoints first.
while self._waypoints and self._waypoints[0].location.distance(
self._actor.get_location()) < 0.5:
self._waypoints = self._waypoints[1:]
self._reached_goal = False
if not self._waypoints:
self._reached_goal = True
else:
direction_norm = self._set_new_velocity(
self._offset_waypoint(self._waypoints[0]))
if direction_norm < 4.0:
self._waypoints = self._waypoints[1:]
if not self._waypoints:
self._reached_goal = True
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
