Untitled
unknown
plain_text
a year ago
2.8 kB
5
Indexable
Never
class OpenScenarioDrivenDistance(Criterion): def __init__(self, actor, openscenario_distance_success, distance_acceptable=None, optional=False, name="CheckOpenScenarioDrivenDistance"): """ Setup actor """ super(OpenScenarioDrivenDistance, self).__init__(name, actor, openscenario_distance_success, distance_acceptable, optional) self._last_location = None self._map = CarlaDataProvider.get_map() self.first_waypoint = None def initialise(self): self._last_location = CarlaDataProvider.get_location(self.actor) self._vehicle_velocity = CarlaDataProvider.get_velocity(self.actor) self.first_waypoint = self._map.get_waypoint(self._last_location) super(OpenScenarioDrivenDistance, self).initialise() def update(self): """ Check distance """ new_status = py_trees.common.Status.RUNNING if self.actor is None: return new_status location = CarlaDataProvider.get_location(self.actor) cwaypoint = self._map.get_waypoint(location) if location is None: return new_status if self._last_location is None: self._last_location = location return new_status # self.actual_value += location.distance(self._last_location) self._last_location = location if self.first_waypoint and cwaypoint: waypoint_distance = self.first_waypoint.transform.location.distance(cwaypoint.transform.location) if waypoint_distance < 2.0: self.actual_value = waypoint_distance print("Distance between first waypoint and current waypoint is less than 2") 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 terminate(self, new_status): """ Set final status """ if self.test_status != "SUCCESS": self.test_status = "FAILURE" self.actual_value = round(self.actual_value, 2) super(OpenScenarioDrivenDistance, self).terminate(new_status)