def from_action_space(action_space, vehicle_pose, sim):
if action_space == ActionSpaceType.Lane:
# TAI: we should probably be fetching these waypoint through the mission planner
target_lane_id = sim.waypoints.closest_waypoint(
vehicle_pose, filter_from_count=4
).lane_id
return LaneFollowingControllerState(target_lane_id)
if action_space == ActionSpaceType.LaneWithContinuousSpeed:
# TAI: we should probably be fetching these waypoint through the mission planner
target_lane_id = sim.waypoints.closest_waypoint(
vehicle_pose, filter_from_count=4
).lane_id
return LaneFollowingControllerState(target_lane_id)
if action_space == ActionSpaceType.ActuatorDynamic:
return ActuatorDynamicControllerState()
if action_space == ActionSpaceType.Trajectory:
return TrajectoryTrackingControllerState()
if action_space == ActionSpaceType.MPC:
return TrajectoryTrackingControllerState()
# Other action spaces do not need a controller state object
return None
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()
# Other action spaces do not need a controller state object
return None