def update(self):
"""
Check if the ego vehicle can arrive at other actor within time
"""
new_status = py_trees.common.Status.RUNNING
current_location = CarlaDataProvider.get_location(self._actor)
other_location = CarlaDataProvider.get_location(self._other_actor)
if current_location is None or other_location is None:
return new_status
current_velocity = CarlaDataProvider.get_velocity(self._actor)
other_velocity = CarlaDataProvider.get_velocity(self._other_actor)
if self._along_route:
# Global planner needs a location at a driving lane
current_location = self._map.get_waypoint(current_location).transform.location
other_location = self._map.get_waypoint(other_location).transform.location
distance = calculate_distance(current_location, other_location, self._grp)
# if velocity is too small, simply use a large time to arrival
time_to_arrival = self._max_time_to_arrival
if current_velocity > other_velocity:
time_to_arrival = 2 * distance / (current_velocity - other_velocity)
if self._comparison_operator(time_to_arrival, self._time):
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):
# get actor speed
actor_speed = CarlaDataProvider.get_velocity(self._actor)
target_speed = CarlaDataProvider.get_velocity(
self._other_actor) + self._delta_velocity
# distance between actors
distance = CarlaDataProvider.get_location(self._actor).distance(
CarlaDataProvider.get_location(self._other_actor))
# driven distance of actor
driven_distance = CarlaDataProvider.get_location(self._actor).distance(
self._initial_actor_pos)
if actor_speed < target_speed:
# set throttle to throttle_value to accelerate
self._control.throttle = self._throttle_value
if actor_speed >= target_speed:
# keep velocity until the actors are in trigger distance
self._control.throttle = 0
self._actor.apply_control(self._control)
# new status:
if distance <= self._trigger_distance:
new_status = py_trees.common.Status.SUCCESS
elif driven_distance > self._max_distance:
new_status = py_trees.common.Status.FAILURE
else:
new_status = py_trees.common.Status.RUNNING
return new_status
def update(self):
"""
Check if the ego vehicle can arrive at other actor within time
"""
new_status = py_trees.common.Status.RUNNING
current_location = CarlaDataProvider.get_location(self._actor)
target_location = CarlaDataProvider.get_location(self._other_actor)
if current_location is None or target_location is None:
return new_status
distance = calculate_distance(current_location, target_location)
current_velocity = CarlaDataProvider.get_velocity(self._actor)
other_velocity = CarlaDataProvider.get_velocity(self._other_actor)
# if velocity is too small, simply use a large time to arrival
time_to_arrival = self._max_time_to_arrival
if current_velocity > other_velocity:
time_to_arrival = 2 * distance / (current_velocity -
other_velocity)
if self._comparison_operator(time_to_arrival, self._time):
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):
"""
Dynamic control update for actor velocity
"""
new_status = py_trees.common.Status.RUNNING
distance_reference = calculate_distance(CarlaDataProvider.get_location(self._actor_reference),
self._target_location)
distance = calculate_distance(CarlaDataProvider.get_location(self._actor),
self._target_location)
velocity_reference = CarlaDataProvider.get_velocity(self._actor_reference)
time_reference = float('inf')
if velocity_reference > 0:
time_reference = distance_reference / velocity_reference
velocity_current = CarlaDataProvider.get_velocity(self._actor)
time_current = float('inf')
if velocity_current > 0:
time_current = distance / velocity_current
control_value = (self._gain) * (time_current - time_reference)
if control_value > 0:
self._control.throttle = min([control_value, 1])
self._control.brake = 0
else:
self._control.throttle = 0
self._control.brake = min([abs(control_value), 1])
self._actor.apply_control(self._control)
self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
return new_status
def update(self):
"""
Check if the ego vehicle can arrive at other actor within time
"""
new_status = py_trees.common.Status.RUNNING
current_location = CarlaDataProvider.get_location(self._actor)
other_location = CarlaDataProvider.get_location(self._other_actor)
# Get the bounding boxes
if self._condition_freespace:
if isinstance(self._actor, (carla.Vehicle, carla.Walker)):
actor_extent = self._actor.bounding_box.extent.x
else:
# Patch, as currently static objects have no bounding boxes
actor_extent = 0
if isinstance(self._other_actor, (carla.Vehicle, carla.Walker)):
other_extent = self._other_actor.bounding_box.extent.x
else:
# Patch, as currently static objects have no bounding boxes
other_extent = 0
if current_location is None or other_location is None:
return new_status
current_velocity = CarlaDataProvider.get_velocity(self._actor)
other_velocity = CarlaDataProvider.get_velocity(self._other_actor)
if self._along_route:
# Global planner needs a location at a driving lane
current_location = self._map.get_waypoint(
current_location).transform.location
other_location = self._map.get_waypoint(
other_location).transform.location
distance = calculate_distance(current_location, other_location,
self._grp)
# if velocity is too small, simply use a large time to arrival
time_to_arrival = self._max_time_to_arrival
if current_velocity > other_velocity:
if self._condition_freespace:
time_to_arrival = (distance - actor_extent - other_extent) / (
current_velocity - other_velocity)
else:
time_to_arrival = distance / (current_velocity -
other_velocity)
if self._comparison_operator(time_to_arrival, self._time):
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 velocity
"""
new_status = py_trees.common.Status.RUNNING
if self.actor is None:
return new_status
velocity = CarlaDataProvider.get_velocity(self.actor)
self.actual_value = max(velocity, self.actual_value)
if velocity > self.expected_value_success:
self.test_status = "FAILURE"
else:
self.test_status = "SUCCESS"
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 update(self):
"""
Check if the actor can arrive at target_location within time
"""
new_status = py_trees.common.Status.RUNNING
current_location = CarlaDataProvider.get_location(self._actor)
if current_location is None:
return new_status
distance = calculate_distance(current_location, self._target_location)
velocity = CarlaDataProvider.get_velocity(self._actor)
# if velocity is too small, simply use a large time to arrival
time_to_arrival = self._max_time_to_arrival
if velocity > EPSILON:
time_to_arrival = distance / velocity
if time_to_arrival < self._time:
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 speed is above the speed_threshold
"""
new_status = py_trees.common.Status.RUNNING
linear_speed = CarlaDataProvider.get_velocity(self._actor)
if linear_speed is not None:
if linear_speed < self._speed_threshold and self._time_last_valid_state:
if (GameTime.get_time() - self._time_last_valid_state
) > self._below_threshold_max_time:
# Game over. The actor has been "blocked" for too long
self.test_status = "FAILURE"
# record event
vehicle_location = CarlaDataProvider.get_location(
self._actor)
blocked_event = TrafficEvent(
event_type=TrafficEventType.VEHICLE_BLOCKED)
ActorSpeedAboveThresholdTest._set_event_message(
blocked_event, vehicle_location)
ActorSpeedAboveThresholdTest._set_event_dict(
blocked_event, vehicle_location)
self.list_traffic_events.append(blocked_event)
else:
self._time_last_valid_state = GameTime.get_time()
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 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 = srunner.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
distance = calculate_distance(actor_location, target_location)
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):
"""
As long as the stop condition (duration or distance) is not violated, set a new vehicle control
For vehicles: set throttle to throttle_value, as long as velocity is < target_velocity
For walkers: simply apply the given self._control
"""
new_status = py_trees.common.Status.RUNNING
if self._type == 'vehicle':
if CarlaDataProvider.get_velocity(
self._actor) < self._target_velocity:
self._control.throttle = 1.0
else:
self._control.throttle = 0.0
self._actor.apply_control(self._control)
new_location = CarlaDataProvider.get_location(self._actor)
self._distance += calculate_distance(self._location, new_location)
self._location = new_location
if self._distance > self._target_distance:
new_status = py_trees.common.Status.SUCCESS
if GameTime.get_time() - self._start_time > self._duration:
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 running a red light
"""
new_status = py_trees.common.Status.RUNNING
location = self._actor.get_location()
if location is None:
return new_status
if not self._target_stop_sign:
# scan for stop signs
self._target_stop_sign = self._scan_for_stop_sign()
else:
# we were in the middle of dealing with a stop sign
if not self.is_actor_affected_by_stop(self._actor,
self._target_stop_sign):
# is the vehicle out of the influence of this stop sign now?
if not self._stop_completed:
# did we stop?
self.test_status = "FAILURE"
stop_location = self._target_stop_sign.get_transform(
).location
running_stop_event = TrafficEvent(
event_type=TrafficEventType.STOP_INFRACTION)
running_stop_event.set_message(
"Agent ran a stop {} at (x={}, y={}, x={})".format(
self._target_stop_sign.id, stop_location.x,
stop_location.y, stop_location.z))
running_stop_event.set_dict({
'id': self._target_stop_sign.id,
'x': stop_location.x,
'y': stop_location.y,
'z': stop_location.z
})
self.list_traffic_events.append(running_stop_event)
# reset state
self._target_stop_sign = None
self._stop_completed = False
if self._target_stop_sign:
# we are already dealing with a target stop sign
#
# did the ego-vehicle stop?
current_speed = CarlaDataProvider.get_velocity(self._actor)
if current_speed < self.SPEED_THRESHOLD:
self._stop_completed = True
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 update(self):
"""
Gets the speed of the two actors and compares them according to the comparison operator
returns:
py_trees.common.Status.RUNNING when the comparison fails and
py_trees.common.Status.SUCCESS when it succeeds
"""
new_status = py_trees.common.Status.RUNNING
curr_speed = CarlaDataProvider.get_velocity(self._actor)
other_speed = CarlaDataProvider.get_velocity(self._other_actor)
relative_speed = curr_speed - other_speed
if self._comparison_operator(relative_speed, self._relative_speed):
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 has the trigger velocity
"""
new_status = py_trees.common.Status.RUNNING
delta_velocity = self._target_velocity - CarlaDataProvider.get_velocity(self._actor)
if delta_velocity < EPSILON:
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 _apply_local_planner(self, actor):
if self._target_speed is None:
self._target_speed = CarlaDataProvider.get_velocity(actor) * 3.6
else:
self._target_speed = self._target_speed * 3.6
local_planner = LocalPlanner( # pylint: disable=undefined-variable
actor, opt_dict={
'target_speed': self._target_speed,
'lateral_control_dict': self._args_lateral_dict})
if self._plan is not None:
local_planner.set_global_plan(self._plan)
self._local_planner_list.append(local_planner)
def update(self):
"""
Check if the _actor stands still (v=0)
"""
new_status = py_trees.common.Status.RUNNING
velocity = CarlaDataProvider.get_velocity(self._actor)
if velocity < EPSILON:
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):
"""
Set throttle to throttle_value, as long as velocity is < target_velocity
"""
new_status = py_trees.common.Status.RUNNING
if CarlaDataProvider.get_velocity(self._actor) < self._target_velocity:
self._control.throttle = 1.0
else:
self._control.throttle = 0.0
self._actor.apply_control(self._control)
self.logger.debug("%s.update()[%s->%s]" %
(self.__class__.__name__, self.status, new_status))
return new_status
def update(self):
"""
Check if the _actor stands still (v=0)
"""
new_status = py_trees.common.Status.RUNNING
velocity = CarlaDataProvider.get_velocity(self._actor)
if velocity > self._epsilon:
self._start_time = GameTime.get_time()
if GameTime.get_time() - self._start_time > self._duration:
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):
"""
Set brake to brake_value until reaching full stop
"""
new_status = py_trees.common.Status.RUNNING
if CarlaDataProvider.get_velocity(self._actor) > EPSILON:
self._control.brake = self._brake_value
else:
new_status = py_trees.common.Status.SUCCESS
self._control.brake = 0
self.logger.debug("%s.update()[%s->%s]" %
(self.__class__.__name__, self.status, new_status))
self._actor.apply_control(self._control)
return new_status
def update(self):
"""
Gets the speed of the actor and compares it with the reference one
returns:
py_trees.common.Status.RUNNING when the comparison fails and
py_trees.common.Status.SUCCESS when it succeeds
"""
new_status = py_trees.common.Status.RUNNING
actor_speed = CarlaDataProvider.get_velocity(self._actor)
if self._comparison_operator(actor_speed, self._target_velocity):
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 _apply_local_planner(self, actor):
"""
Convert the plan into locations for walkers (pedestrians), or to a waypoint list for other actors.
For non-walkers, activate the carla.agent.navigation.LocalPlanner module.
"""
if self._target_speed is None:
self._target_speed = CarlaDataProvider.get_velocity(actor)
else:
self._target_speed = self._target_speed
if isinstance(actor, carla.Walker):
self._local_planner_dict[actor] = "Walker"
if self._plan is not None:
if isinstance(self._plan[0], carla.Location):
self._actor_dict[actor] = self._plan
else:
self._actor_dict[actor] = [element[0].transform.location for element in self._plan]
else:
local_planner = LocalPlanner( # pylint: disable=undefined-variable
actor, opt_dict={
'target_speed': self._target_speed * 3.6,
'lateral_control_dict': self._args_lateral_dict})
if self._plan is not None:
if isinstance(self._plan[0], carla.Location):
plan = []
for location in self._plan:
waypoint = CarlaDataProvider.get_map().get_waypoint(location,
project_to_road=True,
lane_type=carla.LaneType.Any)
plan.append((waypoint, RoadOption.LANEFOLLOW))
local_planner.set_global_plan(plan)
else:
local_planner.set_global_plan(self._plan)
self._local_planner_dict[actor] = local_planner
self._actor_dict[actor] = self._plan