diff --git a/.pylintrc b/.pylintrc index 91dbe28c3..b3b4fe6fe 100644 --- a/.pylintrc +++ b/.pylintrc @@ -1,6 +1,6 @@ [MESSAGES CONTROL] max-line-length=120 -disable=no-self-use,anomalous-backslash-in-string,too-many-arguments,too-few-public-methods,too-many-instance-attributes,redefined-variable-type,unused-argument,bad-continuation,too-many-lines,too-many-branches,locally-disabled,too-many-locals,too-many-statements,duplicate-code,too-many-nested-blocks,fixme +disable=no-self-use,anomalous-backslash-in-string,too-many-arguments,too-few-public-methods,too-many-instance-attributes,redefined-variable-type,unused-argument,bad-continuation,too-many-lines,too-many-branches,locally-disabled,too-many-locals,too-many-statements,duplicate-code,too-many-nested-blocks,fixme,useless-object-inheritance ignored-modules=carla,carla.command,agents.navigation.basic_agent,agents.navigation.roaming_agent,agents.tools.misc,agents.navigation.local_planner,agents.navigation.global_route_planner,agents.navigation.global_route_planner_dao,shutil,carla_msgs,nav_msgs,sensor_msgs,std_msgs,tf,cv_bridge,geometry_msgs,rosgraph_msgs,rospy variable-rgx=[a-z0-9_]{1,40}$ function-rgx=[a-z0-9_]{1,40}$ diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index e76d84a04..d54d7cf33 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -13,11 +13,18 @@ ### :rocket: New Features * Added a sensor barrier for the agents to ensure that the simulation waits for them to render their data. * Added an option to produce a machine-readable JSON version of the scenario report. -* Added a static obstacle evasion OpenSCENARIO scenario -* Added support for OSC Routing options -* Added support for OSC SynchronizeAction -* Added support to place OSC controller implementation alongside the OSC scenario * Updated *GameTime.restart()* at *srunner/scenariomanager/timer.py* to also reset the frame number +* OpenSCENARIO Support: + * Added a static obstacle evasion OpenSCENARIO scenario + * Added support for OSC Routing options + * Added support for OSC SynchronizeAction + * Added support to place OSC controller implementation alongside the OSC scenario + * Extended SimpleVehicleController + * Added controller using CARLA's autopilot (in replacement for ActivateControllerAction) + * Updated ActivateControllerAction to its specified behavior according to OSC 1.0 + * Added support for storyboards with multiple stories + * Added support for ObjectControllers. Note that the controller has to be implemented in Python, + or be one of the provided controllers. ### :bug: Bug Fixes * Fixed metrics-manager.py failing to run with port argument * Fixed exception when using OSC scenarios without EnvironmentAction inside Storyboard-Init diff --git a/Docs/openscenario_support.md b/Docs/openscenario_support.md index 14c6ff60a..a7e736e97 100755 --- a/Docs/openscenario_support.md +++ b/Docs/openscenario_support.md @@ -197,7 +197,7 @@ contains of submodules, which are not listed, the support status applies to all ActivateControllerAction ❌ ✅ -Can be used to activate/deactive the CARLA autopilot. + ControllerAction ✅ diff --git a/srunner/scenariomanager/actorcontrols/actor_control.py b/srunner/scenariomanager/actorcontrols/actor_control.py index 480e41347..0848da6cd 100644 --- a/srunner/scenariomanager/actorcontrols/actor_control.py +++ b/srunner/scenariomanager/actorcontrols/actor_control.py @@ -150,6 +150,24 @@ def set_init_speed(self): """ self.control_instance.set_init_speed() + def change_lon_control(self, enable): + """ + Enable/Disable longitudinal control component of actor controller + + Args: + enable (boolean): Enable/Disable signal + """ + self.control_instance.change_lon_control(enable) + + def change_lat_control(self, enable): + """ + Enable/Disable lateral control component of actor controller + + Args: + enable (boolean): Enable/Disable signal + """ + self.control_instance.change_lat_control(enable) + def run_step(self): """ Execute on tick of the controller's control loop diff --git a/srunner/scenariomanager/actorcontrols/basic_control.py b/srunner/scenariomanager/actorcontrols/basic_control.py index b2ecb9456..9c9972e14 100644 --- a/srunner/scenariomanager/actorcontrols/basic_control.py +++ b/srunner/scenariomanager/actorcontrols/basic_control.py @@ -37,6 +37,12 @@ class BasicControl(object): Defaults to False. _reached_goal (boolean): Defaults to False. + _use_lon_control (boolean): + Use longitudinal component of controller + Defaults to True + _use_lat_control (boolean): + Use lateral component of controller + Defaults to True """ _actor = None @@ -45,6 +51,8 @@ class BasicControl(object): _target_speed = 0 _reached_goal = False _init_speed = False + _use_lon_control = True + _use_lat_control = True def __init__(self, actor): """ @@ -78,6 +86,24 @@ def set_init_speed(self): """ self._init_speed = True + def change_lon_control(self, enable): + """ + Enable/Disable longitudinal control component + + Args: + enable (boolean): Enable/Disable signal + """ + self._use_lon_control = enable + + def change_lat_control(self, enable): + """ + Enable/Disable lateral control component + + Args: + enable (boolean): Enable/Disable signal + """ + self._use_lat_control = enable + def check_reached_waypoint_goal(self): """ Check if the actor reached the end of the waypoint list diff --git a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py index cb73d6f2e..427a2b03b 100644 --- a/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py @@ -55,6 +55,8 @@ class SimpleVehicleControl(BasicControl): (consider_trafficlights, true/false) - Enable consideration of traffic lights (max_deceleration, float) - Use a reasonable deceleration value for this vehicle + (max_acceleration, float) - Use a reasonable acceleration value for + this vehicle (attach_camera, true/false) - Attach OpenCV display to actor (useful for debugging) @@ -69,6 +71,12 @@ class SimpleVehicleControl(BasicControl): Defaults to False. _proximity_threshold (float): Distance in front of actor in which obstacles are considered Defaults to infinity. + _consider_trafficlights (boolean): Enable/Disable consideration of red traffic lights + Defaults to False. + _max_deceleration (float): Deceleration value of the vehicle when braking + Defaults to None (infinity). + _max_acceleration (float): Acceleration value of the vehicle when accelerating + Defaults to None (infinity). _cv_image (CV Image): Contains the OpenCV image, in case a debug camera is attached to the actor Defaults to None. _camera (sensor.camera.rgb): Debug camera attached to actor @@ -277,11 +285,10 @@ def _set_new_velocity(self, next_location): if self._consider_traffic_lights: if (self._actor.is_at_traffic_light() and - self._actor.get_traffic_light_state() == carla.TrafficLightState.Red): + self._actor.get_traffic_light_state() == carla.TrafficLightState.Red): target_speed = 0 if target_speed < current_speed and math.fabs(target_speed - current_speed) > 0.01: - print(target_speed, current_speed) self._actor.set_light_state(carla.VehicleLightState.Brake) if self._max_deceleration is not None: target_speed = max(target_speed, current_speed - (current_time - diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index 75f5b68b7..baabe6fd3 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -278,7 +278,7 @@ class ChangeActorControl(AtomicBehavior): Atomic to change the longitudinal/lateral control logic for an actor. The (actor, controller) pair is stored inside the Blackboard. - The behavior immediately terminates with SUCCESS after the controller. + The behavior immediately terminates with SUCCESS after the controller was changed. Args: actor (carla.Actor): Actor that should be controlled by the controller. @@ -329,6 +329,64 @@ def update(self): return py_trees.common.Status.SUCCESS +class DeActivateActorControlComponents(AtomicBehavior): + + """ + Atomic to enable/disable the longitudinal/lateral control component of an actor controller. + The (actor, controller) pair is retrieved from the Blackboard. + + The behavior immediately terminates with SUCCESS. + + Args: + actor (carla.Actor): Actor that should be controlled by the controller. + control_py_module (string): Name of the python module containing the implementation + of the controller. + args (dictionary): Additional arguments for the controller. + scenario_file_path (string): Additional path to controller implementation. + name (string): Name of the behavior. + Defaults to 'ChangeActorControl'. + + Attributes: + _actor_control (ActorControl): Instance of the actor control. + """ + + def __init__(self, actor, lon_control=None, lat_control=None, name="ChangeActorControl"): + """ + Setup actor controller. + """ + super(DeActivateActorControlComponents, self).__init__(name, actor) + + self._lon_control = lon_control + self._lat_control = lat_control + + def update(self): + """ + Write (actor, controler) pair to Blackboard, or update the controller + if actor already exists as a key. + + returns: + py_trees.common.Status.SUCCESS + """ + + actor_dict = {} + + try: + check_actors = operator.attrgetter("ActorsWithController") + actor_dict = check_actors(py_trees.blackboard.Blackboard()) + except AttributeError: + pass + + if self._actor.id in actor_dict: + if self._lon_control is not None: + actor_dict[self._actor.id].change_lon_control(self._lon_control) + if self._lat_control is not None: + actor_dict[self._actor.id].change_lat_control(self._lat_control) + else: + return py_trees.common.Status.FAILURE + + return py_trees.common.Status.SUCCESS + + class UpdateAllActorControls(AtomicBehavior): """ diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index bd5654bd7..bfa3d5875 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -258,12 +258,14 @@ def _create_behavior(self): Basic behavior do nothing, i.e. Idle """ - stories_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="OSCStories") + stories_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, + name="OSCStories") joint_actor_list = self.other_actors + self.ego_vehicles + [None] for story in self.config.stories: story_name = story.get("name") - story_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=story_name) + story_behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, + name=story_name) for act in story.iter("Act"): act_sequence = py_trees.composites.Sequence( @@ -289,7 +291,8 @@ def _create_behavior(self): for entity in actor.iter("EntityRef"): entity_name = entity.attrib.get('entityRef', None) for k, _ in enumerate(joint_actor_list): - if joint_actor_list[k] and entity_name == joint_actor_list[k].attributes['role_name']: + if (joint_actor_list[k] and + entity_name == joint_actor_list[k].attributes['role_name']): actor_ids.append(k) break @@ -298,10 +301,11 @@ def _create_behavior(self): sequence.attrib.get('name'))) actor_ids.append(len(joint_actor_list) - 1) - # Collect catalog reference maneuvers in order to process them at the same time as normal maneuvers + # Collect catalog reference maneuvers to process them at the same time as normal maneuvers catalog_maneuver_list = [] for catalog_reference in sequence.iter("CatalogReference"): - catalog_maneuver = OpenScenarioParser.get_catalog_entry(self.config.catalogs, catalog_reference) + catalog_maneuver = OpenScenarioParser.get_catalog_entry(self.config.catalogs, + catalog_reference) catalog_maneuver_list.append(catalog_maneuver) all_maneuvers = itertools.chain(iter(catalog_maneuver_list), sequence.iter("Maneuver")) single_sequence_iteration = py_trees.composites.Parallel( @@ -319,14 +323,15 @@ def _create_behavior(self): if child.tag == "Action": for actor_id in actor_ids: maneuver_behavior = OpenScenarioParser.convert_maneuver_to_atomic( - child, joint_actor_list[actor_id], joint_actor_list, self.config.catalogs) + child, joint_actor_list[actor_id], + joint_actor_list, self.config.catalogs) maneuver_behavior = StoryElementStatusToBlackboard( maneuver_behavior, "ACTION", child.attrib.get('name')) parallel_actions.add_child( - oneshot_with_check(variable_name=# See note in get_xml_path - get_xml_path(story, sequence) + '>' + \ - get_xml_path(maneuver, child), - behaviour=maneuver_behavior)) + oneshot_with_check(variable_name= # See note in get_xml_path + get_xml_path(story, sequence) + '>' + \ + get_xml_path(maneuver, child), + behaviour=maneuver_behavior)) if child.tag == "StartTrigger": # There is always one StartConditions block per Event @@ -340,14 +345,14 @@ def _create_behavior(self): event_sequence.add_child(parallel_actions) maneuver_parallel.add_child( oneshot_with_check(variable_name=get_xml_path(story, sequence) + '>' + - get_xml_path(maneuver, event), # See get_xml_path - behaviour=event_sequence)) + get_xml_path(maneuver, event), # See get_xml_path + behaviour=event_sequence)) maneuver_parallel = StoryElementStatusToBlackboard( maneuver_parallel, "MANEUVER", maneuver.attrib.get('name')) single_sequence_iteration.add_child( oneshot_with_check(variable_name=get_xml_path(story, sequence) + '>' + - maneuver.attrib.get('name'), # See get_xml_path - behaviour=maneuver_parallel)) + maneuver.attrib.get('name'), # See get_xml_path + behaviour=maneuver_parallel)) # OpenSCENARIO refers to Sequences as Scenes in this instance single_sequence_iteration = StoryElementStatusToBlackboard( @@ -360,7 +365,7 @@ def _create_behavior(self): if sequence_behavior.children: parallel_sequences.add_child( oneshot_with_check(variable_name=get_xml_path(story, sequence), - behaviour=sequence_behavior)) + behaviour=sequence_behavior)) if parallel_sequences.children: parallel_sequences = StoryElementStatusToBlackboard( @@ -370,7 +375,8 @@ def _create_behavior(self): start_triggers = act.find("StartTrigger") if list(start_triggers) is not None: for start_condition in start_triggers: - parallel_start_criteria = self._create_condition_container(start_condition, story, "StartConditions") + parallel_start_criteria = self._create_condition_container(start_condition, story, + "StartConditions") if parallel_start_criteria.children: start_conditions.add_child(parallel_start_criteria) end_triggers = act.find("StopTrigger") @@ -390,8 +396,8 @@ def _create_behavior(self): story_behavior.add_child(act_sequence) stories_behavior.add_child(oneshot_with_check(variable_name=get_xml_path(story, story) + '>' + - story_name, # See get_xml_path - behaviour=story_behavior)) + story_name, # See get_xml_path + behaviour=story_behavior)) # Build behavior tree behavior = py_trees.composites.Parallel( diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index 072385dd8..6efa41a7e 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -26,12 +26,12 @@ ActorTransformSetterToOSCPosition, RunScript, ChangeWeather, - ChangeAutoPilot, ChangeRoadFriction, ChangeActorTargetSpeed, ChangeActorControl, ChangeActorWaypoints, ChangeActorLateralMotion, + DeActivateActorControlComponents, SyncArrivalOSC, Idle) # pylint: disable=unused-import @@ -1111,13 +1111,19 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): raise AttributeError("Unknown speed action") elif private_action.find('ActivateControllerAction') is not None: private_action = private_action.find('ActivateControllerAction') - activate = strtobool(private_action.attrib.get('longitudinal')) - atomic = ChangeAutoPilot(actor, activate, name=maneuver_name) + lon_control = None + lat_control = None + if 'longitudinal' in private_action.attrib.keys(): + lon_control = strtobool(private_action.attrib.get('longitudinal')) + if 'lateral' in private_action.attrib.keys(): + lat_control = strtobool(private_action.attrib.get('lateral')) + atomic = DeActivateActorControlComponents(actor, lon_control, lat_control, name=maneuver_name) elif private_action.find('ControllerAction') is not None: controller_action = private_action.find('ControllerAction') module, args = OpenScenarioParser.get_controller(controller_action, catalogs) atomic = ChangeActorControl(actor, control_py_module=module, args=args, - scenario_file_path=OpenScenarioParser.osc_filepath) + scenario_file_path=OpenScenarioParser.osc_filepath, + name=maneuver_name) elif private_action.find('TeleportAction') is not None: teleport_action = private_action.find('TeleportAction') position = teleport_action.find('Position')