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