Skip to content

Commit

Permalink
Rework ActivateControllerAction OSC action
Browse files Browse the repository at this point in the history
  • Loading branch information
fabianoboril committed Dec 22, 2020
1 parent 3f5a4a2 commit b6ef71d
Show file tree
Hide file tree
Showing 9 changed files with 159 additions and 31 deletions.
2 changes: 1 addition & 1 deletion .pylintrc
Original file line number Diff line number Diff line change
@@ -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}$
Expand Down
15 changes: 11 additions & 4 deletions Docs/CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion Docs/openscenario_support.md
Original file line number Diff line number Diff line change
Expand Up @@ -197,7 +197,7 @@ contains of submodules, which are not listed, the support status applies to all
<td><code>ActivateControllerAction</code></td>
<td>&#10060;</td>
<td>&#9989;</td>
<td>Can be used to activate/deactive the CARLA autopilot.</td>
<td></td>
<tr>
<td><code>ControllerAction</code></td>
<td>&#9989;</td>
Expand Down
18 changes: 18 additions & 0 deletions srunner/scenariomanager/actorcontrols/actor_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
26 changes: 26 additions & 0 deletions srunner/scenariomanager/actorcontrols/basic_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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):
"""
Expand Down Expand Up @@ -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
Expand Down
11 changes: 9 additions & 2 deletions srunner/scenariomanager/actorcontrols/simple_vehicle_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down Expand Up @@ -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 -
Expand Down
60 changes: 59 additions & 1 deletion srunner/scenariomanager/scenarioatomics/atomic_behaviors.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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):

"""
Expand Down
42 changes: 24 additions & 18 deletions srunner/scenarios/open_scenario.py
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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

Expand All @@ -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(
Expand All @@ -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
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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")
Expand All @@ -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(
Expand Down
Loading

0 comments on commit b6ef71d

Please sign in to comment.