# Copyright (C) 2020. Huawei Technologies Co., Ltd. All rights reserved.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
import enum
from functools import partial
from typing import Dict, Literal, Sequence, Tuple, Union
import numpy as np
from smarts.core.controllers.actuator_dynamic_controller import (
ActuatorDynamicController,
ActuatorDynamicControllerState,
)
from smarts.core.controllers.direct_controller import DirectController
from smarts.core.controllers.lane_following_controller import (
LaneFollowingController,
LaneFollowingControllerState,
)
from smarts.core.controllers.motion_planner_controller import (
MotionPlannerController,
MotionPlannerControllerState,
)
from smarts.core.controllers.trajectory_interpolation_controller import (
TrajectoryInterpolationController,
)
from smarts.core.controllers.trajectory_tracking_controller import (
TrajectoryTrackingController,
TrajectoryTrackingControllerState,
)
from .action_space_type import ActionSpaceType
METER_PER_SECOND_TO_KM_PER_HR = 3.6
[docs]class LaneAction(enum.Enum):
"""The action for lane space actions."""
keep_lane: str = "keep_lane"
slow_down: str = "slow_down"
change_lane_left: str = "change_lane_left"
change_lane_right: str = "change_lane_right"
[docs]class Controllers:
"""Handles vehicle controller selection."""
[docs] @staticmethod
def get_action_shape(action_space: ActionSpaceType):
"""Describes the shape of actions that are used for standard controllers.
Args:
action_space (ActionSpaceType): The action space to describe.
Raises:
NotImplementedError: The action space requested does is not yet implemented.
Returns:
Tuple[Any, str]: The action space and the descriptive attribute.
"""
# TODO MTA: test the action shapes against dummy agents.
if action_space == ActionSpaceType.Empty:
return Union[type(None), Literal[False], Tuple], "null"
if action_space == ActionSpaceType.Lane:
return (
Literal[
LaneAction.keep_lane,
LaneAction.slow_down,
LaneAction.change_lane_left,
LaneAction.change_lane_right,
],
"lane_action",
)
if action_space in (
ActionSpaceType.ActuatorDynamic,
ActionSpaceType.Continuous,
):
return Tuple[float, float, float], ("throttle", "break", "steering")
if action_space == ActionSpaceType.LaneWithContinuousSpeed:
return Tuple[float, int], ("lane_speed", "lane_change_delta")
if action_space in (ActionSpaceType.MPC, ActionSpaceType.Trajectory):
return Tuple[
Sequence[float], Sequence[float], Sequence[float], Sequence[float]
], ("x_coords", "y_coords", "headings", "speeds")
if action_space == ActionSpaceType.Direct:
return Union[float, Tuple[float, float]], [
"speed",
("linear_acceleration", "angular_velocity"),
]
if action_space == ActionSpaceType.TrajectoryWithTime:
return Tuple[
Sequence[float],
Sequence[float],
Sequence[float],
Sequence[float],
Sequence[float],
], ("times", "x_coords", "y_coords", "headings", "speeds")
TargetPoseSpace = Tuple[float, float, float, float]
TargetPoseAttributes = ("x_coord", "y_coord", "heading", "time_delta")
if action_space == ActionSpaceType.TargetPose:
return TargetPoseSpace, TargetPoseAttributes
if action_space == ActionSpaceType.MultiTargetPose:
return Dict[str, TargetPoseSpace], {"agent_id": TargetPoseAttributes}
if action_space == ActionSpaceType.RelativeTargetPose:
return Tuple[float, float, float], ("delta_x", "delta_y", "delta_heading")
raise NotImplementedError(f"Type {action_space} is not implemented")
[docs]class ControllerOutOfLaneException(Exception):
"""Represents an error due to a vehicle straying too far from any available lane."""
pass
[docs]class ControllerState:
"""Controller state"""
[docs] @staticmethod
def from_action_space(action_space, vehicle_pose, sim):
"""Generate the appropriate controller state given an action space."""
if action_space in (
ActionSpaceType.Lane,
ActionSpaceType.LaneWithContinuousSpeed,
):
target_lane = sim.road_map.nearest_lane(vehicle_pose.point)
if not target_lane:
# This likely means this is a traffic history vehicle that is out-of-lane.
# If not, maybe increase radius in nearest_lane call?
raise ControllerOutOfLaneException(
"Controller has failed because actor is too far from lane for lane-following."
)
return LaneFollowingControllerState(target_lane.lane_id)
if action_space == ActionSpaceType.ActuatorDynamic:
return ActuatorDynamicControllerState()
if action_space == ActionSpaceType.Trajectory:
return TrajectoryTrackingControllerState()
if action_space == ActionSpaceType.MPC:
return TrajectoryTrackingControllerState()
if action_space in (
ActionSpaceType.TargetPose,
ActionSpaceType.MultiTargetPose,
ActionSpaceType.RelativeTargetPose,
):
return MotionPlannerControllerState()
# Other action spaces do not need a controller state object
return None