def TrajectoryFromTrack(track, start=0, end=None):
states = list(dict_utils.get_item_iterator(track.motion_states))
if end is None:
end = states[-1][0]
filtered_motion_states = list(
filter(lambda s: start <= s[0] <= end, states))
n = len(filtered_motion_states)
traj = np.zeros((n, int(StateDefinition.MIN_STATE_SIZE)))
for i, state in enumerate(filtered_motion_states):
# TODO: rename start to be the scenario start time!
traj[i, :] = BarkStateFromMotionState(state[1], start)
return traj
def trajectory_from_track(track, start=0, end=None):
states = list(dict_utils.get_item_iterator(track.motion_states))
if end is None:
end = states[-1][0]
filtered_motion_states = list(
filter(lambda s: start <= s[0] <= end, states))
n = len(filtered_motion_states)
traj = np.zeros((n, int(StateDefinition.MIN_STATE_SIZE)))
start_offset = filtered_motion_states[0][0]
for i, state in enumerate(filtered_motion_states):
traj[i, :] = bark_state_from_motion_state(state[1], start)
return traj
def GoalDefinitionFromTrack(track, end):
states = list(dict_utils.get_item_iterator(track.motion_states))
motion_state = states[-1][1]
bark_state = BarkStateFromMotionState(motion_state)
goal_polygon = Polygon2d(
np.array([0.0, 0.0, 0.0]),
[Point2d(-1.5, 0),
Point2d(-1.5, 8),
Point2d(1.5, 8),
Point2d(1.5, 0)])
goal_polygon = goal_polygon.Translate(
Point2d(bark_state[0, int(StateDefinition.X_POSITION)],
bark_state[0, int(StateDefinition.Y_POSITION)]))
goal_definition = GoalDefinitionPolygon(goal_polygon)
return goal_definition
def GoalDefinitionFromTrack(track, end, xy_offset):
goal_size = 12.0
states = list(dict_utils.get_item_iterator(track.motion_states))
# Goal position is spatial position of last state
motion_state = states[-1][1]
bark_state = BarkStateFromMotionState(motion_state, xy_offset=xy_offset)
goal_polygon = Polygon2d(np.array([0.5 * goal_size, 0.5 * goal_size, 0.0]),
[
Point2d(0.0, 0.0),
Point2d(goal_size, 0.0),
Point2d(goal_size, goal_size),
Point2d(0.0, goal_size),
Point2d(0.0, 0.0)
])
goal_point = Point2d(
bark_state[0, int(StateDefinition.X_POSITION)] - 0.5 * goal_size,
bark_state[0, int(StateDefinition.Y_POSITION)] - 0.5 * goal_size)
goal_polygon = goal_polygon.Translate(goal_point)
goal_definition = GoalDefinitionPolygon(goal_polygon)
return goal_definition
