def poll_planners(crazyflies, timeHelper, planners, duration):
start_time = timeHelper.time()
while not timeHelper.isShutdown():
t = timeHelper.time() - start_time
if t > duration:
break
for cf, planner in zip(crazyflies, planners):
ev = firm.plan_current_goal(planner, t)
cf.cmdFullState(ev.pos, ev.vel, ev.acc, ev.yaw, ev.omega)
timeHelper.sleepForRate(POLL_RATE)
def poll_planners(crazyflies, timeHelper, planners, duration):
start_time = rospy.Time.now()
rate = rospy.Rate(100) # hz
while not rospy.is_shutdown():
t = (rospy.Time.now() - start_time).to_sec()
if t > duration:
break
for cf, planner in zip(crazyflies, planners):
ev = firm.plan_current_goal(planner, t)
cf.cmdFullState(firm2arr(ev.pos), firm2arr(ev.vel),
firm2arr(ev.acc), ev.yaw, firm2arr(ev.omega))
rate.sleep()