def update(self):
"""
Check if the ego vehicle can arrive at other vehicle within time
"""
new_status = py_trees.common.Status.RUNNING
current_location = CarlaDataProvider.get_location(self._ego_vehicle)
target_location = CarlaDataProvider.get_location(self._other_vehicle)
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._ego_vehicle)
other_velocity = CarlaDataProvider.get_velocity(self._other_vehicle)
# 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 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 _tick_scenario(self, timestamp):
"""
Run next tick of scenario
This function is a callback for world.on_tick()
Important:
- It hast to be ensured that the scenario has not yet completed/failed
and that the time moved forward.
- A thread lock should be used to avoid that the scenario tick is performed
multiple times in parallel.
"""
with self._my_lock:
if self._running and self._timestamp_last_run < timestamp.elapsed_seconds:
self._timestamp_last_run = timestamp.elapsed_seconds
if self._debug_mode:
print("\n--------- Tick ---------\n")
# Update game time and vehicle information
GameTime.on_carla_tick(timestamp)
CarlaDataProvider.on_carla_tick()
# Tick scenario
self.scenario_tree.tick_once()
if self._debug_mode:
print("\n")
py_trees.display.print_ascii_tree(self.scenario_tree,
show_status=True)
sys.stdout.flush()
if self.scenario_tree.status != py_trees.common.Status.RUNNING:
self._running = False
def stop_scenario(self):
"""
This function triggers a proper termination of a scenario
"""
if self.scenario is not None:
self.scenario.terminate()
CarlaDataProvider.cleanup()
def load_scenario(self, scenario):
"""
Load a new scenario
"""
self.restart()
self.scenario = scenario.scenario
self.scenario_tree = self.scenario.scenario_tree
self.ego_vehicle = scenario.ego_vehicle
self.other_vehicles = scenario.other_vehicles
CarlaDataProvider.register_vehicle(self.ego_vehicle)
CarlaDataProvider.register_vehicles(self.other_vehicles)
def update(self):
"""
Check if the ego vehicle is within trigger distance to other vehicle
"""
new_status = py_trees.common.Status.RUNNING
ego_location = CarlaDataProvider.get_location(self._ego_vehicle)
other_location = CarlaDataProvider.get_location(self._other_vehicle)
if ego_location is None or other_location is None:
return new_status
if calculate_distance(ego_location, other_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 update(self):
"""
Dynamic control update for vehicle velocity
"""
new_status = py_trees.common.Status.RUNNING
distance_reference = calculate_distance(
CarlaDataProvider.get_location(self._vehicle_reference),
self._target_location)
distance = calculate_distance(
CarlaDataProvider.get_location(self._vehicle),
self._target_location)
velocity_reference = CarlaDataProvider.get_velocity(
self._vehicle_reference)
time_reference = float('inf')
if velocity_reference > 0:
time_reference = distance_reference / velocity_reference
velocity_current = CarlaDataProvider.get_velocity(self._vehicle)
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._vehicle.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 vehicle has the trigger velocity
"""
new_status = py_trees.common.Status.RUNNING
delta_velocity = CarlaDataProvider.get_velocity(
self._vehicle) - self._target_velocity
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 update(self):
"""
Check driven distance
"""
new_status = py_trees.common.Status.RUNNING
new_location = CarlaDataProvider.get_location(self._vehicle)
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
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._vehicle) < self._target_velocity:
self._control.throttle = 1.0
else:
self._control.throttle = 0.0
self._vehicle.apply_control(self._control)
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._vehicle) > 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._vehicle.apply_control(self._control)
return new_status
def update(self):
"""
Check if the _vehicle location is within trigger region
"""
new_status = py_trees.common.Status.RUNNING
location = CarlaDataProvider.get_location(self._vehicle)
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 initialise(self):
self._location = CarlaDataProvider.get_location(self._vehicle)
super(DriveDistance, self).initialise()