Untitled

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)