From 087c630cb29f078d4b971f44166a956f332517c7 Mon Sep 17 00:00:00 2001 From: Adam Eriksson Date: Wed, 28 Jun 2023 08:49:22 +0200 Subject: [PATCH 1/3] Swapped controller to get a smoother trajectory and added support for negative speeds (reversing). --- .../actorcontrols/actor_control.py | 4 ++-- .../actorcontrols/simple_vehicle_control.py | 22 ++++++++++++++++--- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/srunner/scenariomanager/actorcontrols/actor_control.py b/srunner/scenariomanager/actorcontrols/actor_control.py index 333618d6c..020dc443d 100644 --- a/srunner/scenariomanager/actorcontrols/actor_control.py +++ b/srunner/scenariomanager/actorcontrols/actor_control.py @@ -21,7 +21,7 @@ import carla from srunner.scenariomanager.actorcontrols.external_control import ExternalControl -from srunner.scenariomanager.actorcontrols.npc_vehicle_control import NpcVehicleControl +from srunner.scenariomanager.actorcontrols.simple_vehicle_control import SimpleVehicleControl from srunner.scenariomanager.actorcontrols.pedestrian_control import PedestrianControl @@ -69,7 +69,7 @@ def __init__(self, actor, control_py_module, args, scenario_file_path): if isinstance(actor, carla.Walker): self.control_instance = PedestrianControl(actor) elif isinstance(actor, carla.Vehicle): - self.control_instance = NpcVehicleControl(actor) + self.control_instance = SimpleVehicleControl(actor) else: # use ExternalControl for all misc objects to handle all actors the same way self.control_instance = ExternalControl(actor) diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index 5f1d58daa..1da3d1e4b 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -93,7 +93,7 @@ def __init__(self, actor, args=None): self._consider_traffic_lights = False self._consider_obstacles = False self._proximity_threshold = float('inf') - self._waypoint_reached_threshold = 4.0 + self._waypoint_reached_threshold = 0.2 # don't know if this is a reasonable value but it's better than 4m (previous). self._max_deceleration = None self._max_acceleration = None @@ -321,31 +321,47 @@ def _set_new_velocity(self, next_location): velocity.x = direction.x / direction_norm * target_speed velocity.y = direction.y / direction_norm * target_speed + if target_speed < 0: + velocity.x = -velocity.x + velocity.y = -velocity.y + self._actor.set_target_velocity(velocity) # set new angular velocity current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw + # print(f"Current yaw: {current_yaw}") # When we have a waypoint list, use the direction between the waypoints to calculate the heading (change) # otherwise use the waypoint heading directly if self._waypoints: - delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw + if target_speed < 0: + new_yaw = math.degrees(math.atan2(direction.y, direction.x)) - 180 + # print(f"New yaw: {new_yaw}") + delta_yaw = new_yaw - current_yaw + else: + delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw else: new_yaw = CarlaDataProvider.get_map().get_waypoint(next_location).transform.rotation.yaw delta_yaw = new_yaw - current_yaw + # print(f"Delta yaw: {delta_yaw}") if math.fabs(delta_yaw) > 360: delta_yaw = delta_yaw % 360 - + # print(f"Delta yaw after modulo function: {delta_yaw}") + if delta_yaw > 180: delta_yaw = delta_yaw - 360 elif delta_yaw < -180: delta_yaw = delta_yaw + 360 + # print(f"Delta yaw after half circle function: {delta_yaw}") angular_velocity = carla.Vector3D(0, 0, 0) if target_speed == 0: angular_velocity.z = 0 + elif target_speed < 0: + angular_velocity.z = delta_yaw / (direction_norm / -target_speed) else: angular_velocity.z = delta_yaw / (direction_norm / target_speed) + # print(f"Angular velocity: {angular_velocity.z}") self._actor.set_target_angular_velocity(angular_velocity) self._last_update = current_time From c36e28bedc7038505068b60415c194f31d766341 Mon Sep 17 00:00:00 2001 From: Adam Eriksson Date: Wed, 28 Jun 2023 12:47:35 +0200 Subject: [PATCH 2/3] Seif made local changes to fix scenariorunner ending the openscenario early and destroying all actors. --- scenario_runner.py | 12 ++---------- srunner/scenariomanager/scenario_manager.py | 4 ++-- 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/scenario_runner.py b/scenario_runner.py index 5d2dd6015..c2abdb410 100755 --- a/scenario_runner.py +++ b/scenario_runner.py @@ -132,7 +132,7 @@ def _signal_handler(self, signum, frame): """ self._shutdown_requested = True if self.manager: - self.manager.stop_scenario() + self.manager._running = False def _get_scenario_class_or_fail(self, scenario): """ @@ -187,14 +187,6 @@ def _cleanup(self): CarlaDataProvider.cleanup() - for i, _ in enumerate(self.ego_vehicles): - if self.ego_vehicles[i]: - if not self._args.waitForEgo and self.ego_vehicles[i] is not None and self.ego_vehicles[i].is_alive: - print("Destroying ego vehicle {}".format(self.ego_vehicles[i].id)) - self.ego_vehicles[i].destroy() - self.ego_vehicles[i] = None - self.ego_vehicles = [] - if self.agent_instance: self.agent_instance.destroy() self.agent_instance = None @@ -415,7 +407,7 @@ def _load_and_run_scenario(self, config): self._analyze_scenario(config) # Remove all actors, stop the recorder and save all criterias (if needed) - scenario.remove_all_actors() + # scenario.remove_all_actors() if self._args.record: self.client.stop_recorder() self._record_criteria(self.manager.scenario.get_criteria(), recorder_name) diff --git a/srunner/scenariomanager/scenario_manager.py b/srunner/scenariomanager/scenario_manager.py index 62cffabf1..cbe96cbfd 100644 --- a/srunner/scenariomanager/scenario_manager.py +++ b/srunner/scenariomanager/scenario_manager.py @@ -182,7 +182,7 @@ def _tick_scenario(self, timestamp): sys.stdout.flush() if self.scenario_tree.status != py_trees.common.Status.RUNNING: - self._running = False + self._running = True if self._sync_mode and self._running and self._watchdog.get_status(): CarlaDataProvider.get_world().tick() @@ -198,7 +198,7 @@ def stop_scenario(self): """ This function is used by the overall signal handler to terminate the scenario execution """ - self._running = False + pass def analyze_scenario(self, stdout, filename, junit, json): """ From 92b7a861cb2f6c77846dc97569b10a692620b530 Mon Sep 17 00:00:00 2001 From: Adam Eriksson Date: Wed, 28 Jun 2023 13:34:13 +0200 Subject: [PATCH 3/3] removed commented code that was left from troubleshooting. --- .../actorcontrols/simple_vehicle_control.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index 1da3d1e4b..2d0514ee5 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -93,7 +93,7 @@ def __init__(self, actor, args=None): self._consider_traffic_lights = False self._consider_obstacles = False self._proximity_threshold = float('inf') - self._waypoint_reached_threshold = 0.2 # don't know if this is a reasonable value but it's better than 4m (previous). + self._waypoint_reached_threshold = 0.2 # don't know if this is a reasonable value but it's better than 4m. self._max_deceleration = None self._max_acceleration = None @@ -329,13 +329,11 @@ def _set_new_velocity(self, next_location): # set new angular velocity current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw - # print(f"Current yaw: {current_yaw}") # When we have a waypoint list, use the direction between the waypoints to calculate the heading (change) # otherwise use the waypoint heading directly if self._waypoints: if target_speed < 0: new_yaw = math.degrees(math.atan2(direction.y, direction.x)) - 180 - # print(f"New yaw: {new_yaw}") delta_yaw = new_yaw - current_yaw else: delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw @@ -343,16 +341,13 @@ def _set_new_velocity(self, next_location): new_yaw = CarlaDataProvider.get_map().get_waypoint(next_location).transform.rotation.yaw delta_yaw = new_yaw - current_yaw - # print(f"Delta yaw: {delta_yaw}") if math.fabs(delta_yaw) > 360: delta_yaw = delta_yaw % 360 - # print(f"Delta yaw after modulo function: {delta_yaw}") if delta_yaw > 180: delta_yaw = delta_yaw - 360 elif delta_yaw < -180: delta_yaw = delta_yaw + 360 - # print(f"Delta yaw after half circle function: {delta_yaw}") angular_velocity = carla.Vector3D(0, 0, 0) if target_speed == 0: @@ -361,7 +356,6 @@ def _set_new_velocity(self, next_location): angular_velocity.z = delta_yaw / (direction_norm / -target_speed) else: angular_velocity.z = delta_yaw / (direction_norm / target_speed) - # print(f"Angular velocity: {angular_velocity.z}") self._actor.set_target_angular_velocity(angular_velocity) self._last_update = current_time