def publishTrajectory(traj, T, problem):
    if (len(traj) == 0):
        print('Trajectory has zero elements')
        raise
    signal.signal(signal.SIGINT, sigIntHandler)
    print((('Playing back trajectory ' + str(T)) + 's'))
    dt = (float(T) / float(len(traj)))
    t = 0
    while True:
        try:
            publishPose(traj[t], problem, (float(t) * dt))
            sleep(dt)
            t = ((t + 1) % len(traj))
        except KeyboardInterrupt:
            return False
    return True
def publishTrajectory(traj, T, problem):
    if (len(traj) == 0):
        print('Trajectory has zero elements')
        raise
    signal.signal(signal.SIGINT, sigIntHandler)
    print((('Playing back trajectory ' + str(T)) + 's'))
    dt = (float(T) / float(len(traj)))
    t = 0
    while True:
        try:
            publishPose(traj[t], problem, (float(t) * dt))
            sleep(dt)
            t = ((t + 1) % len(traj))
        except KeyboardInterrupt:
            return False
    return True