Skip to content

Commit

Permalink
Rework ActivateControllerAction OSC action
Browse files Browse the repository at this point in the history
Change-Id: I91034ba6728af16582bbfc4c992f2186b3ea1908
  • Loading branch information
fabianoboril committed Apr 16, 2021
1 parent 7c64341 commit 208882d
Show file tree
Hide file tree
Showing 5 changed files with 119 additions and 12 deletions.
18 changes: 18 additions & 0 deletions srunner/scenariomanager/actorcontrols/actor_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -173,6 +173,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 @@ -47,6 +53,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 @@ -90,6 +98,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
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,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 @@ -283,7 +289,6 @@ def _set_new_velocity(self, next_location):
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
20 changes: 10 additions & 10 deletions srunner/tools/openscenario_parser.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,12 +26,12 @@
ActorTransformSetterToOSCPosition,
RunScript,
ChangeWeather,
ChangeAutoPilot,
ChangeRoadFriction,
ChangeActorTargetSpeed,
ChangeActorControl,
ChangeActorWaypoints,
ChangeActorLateralMotion,
DeActivateActorControlComponents,
ChangeActorLaneOffset,
SyncArrivalOSC,
Idle)
Expand Down Expand Up @@ -1081,31 +1081,25 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs):
lat_maneuver = private_action.find('LaneOffsetAction')
continuous = strtobool(lat_maneuver.attrib.get('continuous', True))
# Parsing of the different Dynamic shapes is missing

lane_target_offset = lat_maneuver.find('LaneOffsetTarget')
if lane_target_offset.find('AbsoluteTargetLaneOffset') is not None:
absolute_offset = float(
lane_target_offset.find('AbsoluteTargetLaneOffset').attrib.get('value', 0))
atomic = ChangeActorLaneOffset(
actor, absolute_offset, continuous=continuous, name=maneuver_name)

elif lane_target_offset.find('RelativeTargetLaneOffset') is not None:
relative_target_offset = lane_target_offset.find('RelativeTargetLaneOffset')
relative_offset = float(relative_target_offset.attrib.get('value', 0))

relative_actor = None
for _actor in actor_list:
if relative_target_offset.attrib.get('entityRef', None) == _actor.attributes['role_name']:
relative_actor = _actor
break

if relative_actor is None:
raise AttributeError("Cannot find actor '{}' for condition".format(
relative_target_offset.attrib.get('entityRef', None)))

atomic = ChangeActorLaneOffset(actor, relative_offset, relative_actor,
continuous=continuous, name=maneuver_name)

else:
raise AttributeError("Unknown target offset")
else:
Expand Down Expand Up @@ -1147,13 +1141,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')
Expand Down

0 comments on commit 208882d

Please sign in to comment.