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 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 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): """ Check if actor is in trigger distance """ new_status = py_trees.common.Status.RUNNING # calculate transform with method in openscenario_parser.py osc_transform = sr_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) if self._along_route: # Global planner needs a location at a driving lane actor_location = self._map.get_waypoint( actor_location).transform.location osc_location = self._map.get_waypoint( osc_location).transform.location distance = calculate_distance(actor_location, osc_location, self._grp) if self._comparison_operator(distance, self._distance): new_status = py_trees.common.Status.SUCCESS 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) 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): """ 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 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 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 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 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._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): """ Check if the ego vehicle is within trigger distance to other actor """ new_status = py_trees.common.Status.RUNNING location = CarlaDataProvider.get_location(self._actor) reference_location = CarlaDataProvider.get_location(self._reference_actor) if location is None or reference_location is None: return new_status if self._comparison_operator(calculate_distance(location, reference_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): """ 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 update(self): """ Check if the ego vehicle is within trigger distance to other actor """ new_status = py_trees.common.Status.RUNNING ego_location = CarlaDataProvider.get_location(self._actor) other_location = CarlaDataProvider.get_location(self._other_actor) 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): """ 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 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