trajectory_supervisor.helper_funcs.src.sync_data module

trajectory_supervisor.helper_funcs.src.sync_data.clip_trajectory(traj_in: numpy.ndarray, temp_offset: float, traj_type: str, mp_queue: multiprocessing.context.BaseContext.Queue) → None[source]

Clips the start of a given trajectory based on the temporal offset (‘temp_offset’) provided.

Parameters

  • traj_in – trajectory to be handled with following columns [s, x, y, heading, curv, vel, acc]

  • temp_offset – temporal offset to be clipped at beginning of trajectory in seconds

  • traj_type – string describing the trajectory type (‘perf’ or ‘emerg’)

  • mp_queue – queue that will receive all results

Authors
Created on

30.03.2020

trajectory_supervisor.helper_funcs.src.sync_data.sync_data(traj_perf: dict, traj_em: dict, objects: dict, allowed_t_offset: float = 0.5, use_mp: bool = False) → tuple[source]

This function syncs a given pair of trajectories with the object-list. Therefore, it extrapolates the trajectory or objects to the newest time-stamp available (among them) in order to be able to set them in relation to each other. If the trajectory has a more recent timestamp, the (older) objects are predicted forward accordingly with a constant velocity model. If the objects have a more recent time stamp, the (older) trajectories are integrated forward until the time difference is equalized.

In general, this function does not have to be used if it can be ensured that the objects used to plan the trajectories can be transmitted synchronously with each other. If this is not the case and/or one of the two data is significantly more frequent and a comparison is desirable, this function can be used.

Note: for the synchronization to work, a time stamp in seconds must be stored in the dict of the trajectories as well as in the dict of the objects. It is essential that both use the same time source. In addition, the time stamp of the trajectory must be the same as the time stamp of the objects that serve as the basis for planning.

Parameters

  • traj_perf – performance trajectory in form of a dict with the following entries: - ‘traj’: trajectory data as numpy array with columns: s, x, y, head, curv, vel, acc - ‘id’: unique id of the trajectory (int) - ‘time’: time stamp of the trajectory in seconds

  • traj_em – emergency trajectory in form of a dict (same entries / format as traj_perf)

  • objects –

    object-list dict with each key being a dedicated object id and each object hosting a dict with (at least) the following information:

    • ’id’: unique identifier

    • ’X’: x position of cg

    • ’Y’: y position of cg

    • ’theta’: heading (north = 0, +pi -pi)

    • ’v_x’: x velocity

    • ’type’: “car”, …

    • ’form’: “rectangle” / “circle”

    • ’width’: width (if rectangle)

    • ’length’: length (if rectangle)

    • ’diameter’: diameter (if circle)

    • ’time’: time-stamp

  • allowed_t_offset – (optional) maximum allowed temporal offset between trajectories and object-list [in s] -> if exceeded, a warning is raised and the sync is not performed

  • use_mp – (optional) if set to true and executed on a Linux machine, multiprocessing is used

Returns

  • traj_perf_sync - synced performance trajectory dict

  • traj_em_sync - synced emergency trajectory dict

  • obj_sync - synced object list