def gettraj(pts):
beziers = fitCurve(pts, 1)
pathi = path()
single = game()
bz = []
for b in beziers:
for t in range(0, 51):
bz.append(bezier.q(b, t / 50.0).tolist())
bz = np.array(bz)
pli = []
for i in range(len(bz)):
single = game()
single.kx = bz[i][0]
single.ky = bz[i][1]
pli.append(single)
pathi.points = pli
xdot = np.gradient(bz.T[0])
ydot = np.gradient(bz.T[1])
Xdot = [xdot, ydot]
Xdot = np.array(Xdot)
Xdot = Xdot.T
pti = []
for i in range(len(Xdot)):
single = game()
single.kx = Xdot[i][0]
single.ky = Xdot[i][1]
pti.append(single)
return pathi, Xdot, pti
ppathr1n0 = rospy.Publisher('robot1n0/path', path, queue_size=20)
#traj_pub1n1 = rospy.Publisher('robot1n1/traj_vect',game, queue_size = 20)
traj_publ1n1 = rospy.Publisher('robot1n1/traj_vect_list', path, queue_size=20)
ppathr1n1 = rospy.Publisher('robot1n1/path', path, queue_size=20)
#traj_pub2n0 = rospy.Publisher('robot2n0/traj_vect',game, queue_size = 20)
traj_publ2n0 = rospy.Publisher('robot2n0/traj_vect_list', path, queue_size=20)
ppathr2n0 = rospy.Publisher('robot2n0/path', path, queue_size=20)
#traj_pub2n1 = rospy.Publisher('robot2n1/traj_vect',game, queue_size = 20)
traj_publ2n1 = rospy.Publisher('robot2n1/traj_vect_list', path, queue_size=20)
ppathr2n1 = rospy.Publisher('robot2n1/path', path, queue_size=20)
gs = 0
rpath1n0 = path()
rpath1n1 = path()
rpath2n0 = path()
rpath2n1 = path()
def rp10callback(msg):
global flag
global rpath1n0
global robot1n0
global ppathr1n0
global traj_publ1n0
flag = 1
global rpath1n0
rpath1n0 = msg
get_traj_all(robot1n0, rpath1n0, ppathr1n0, traj_publ1n0)
"""
program to control robot to make it stay on the trajectory
publishes twist commands
"""
flag = 0
bpose = Pose()
d = 0
ball = p.ball(x=0, y=0)
robot = p.robot(x=0, y=0, ball=ball)
gs = 0
r10cs = 1
r11cs = 1
r20cs = 1
r21cs = 1
r10path = path()
r11path = path()
r20path = path()
r21path = path()
r10tlist = path()
r11tlist = path()
r20tlist = path()
r21tlist = path()
rpose = [
p.robot(x=0, y=0, ball=ball),
p.robot(x=0, y=0, ball=ball),
p.robot(x=0, y=0, ball=ball),
p.robot(x=0, y=0, ball=ball)
]
rpath = [[], [], [], []]
rvects = [[], [], [], []]