def run(args):
rospy.init_node('formation')
# Update control every 100 ms.
rate_limiter = rospy.Rate(100)
leader = Leader("tb3_0", rate_limiter)
follower_1 = Follower("tb3_1",
desired_rel_pos=np.array([-0.5, 0.35, 0.0]),
rate_limiter=rate_limiter,
des_d=0.4,
des_psi=2 * np.pi / 3,
leader=leader,
laser_range=[np.pi, np.pi])
follower_2 = Follower("tb3_2",
desired_rel_pos=np.array([-0.5, -0.35, 0.0]),
rate_limiter=rate_limiter,
des_d=0.4,
des_psi=4 * np.pi / 3,
leader=leader,
laser_range=[np.pi, np.pi])
while not rospy.is_shutdown():
if follower_1.ready and follower_2.ready:
leader.update_velocities()
leader.publish_leg()
leader.slam.update()
follower_1.formation_velocity()
follower_2.formation_velocity()
rate_limiter.sleep()
