def reset(t1, t2, r1, r2, r3, r4, ball):
global gs
r1 = p.robot(x=t1[0][0], y=t1[0][1], yaw=0, ball=ball)
r2 = p.robot(x=t1[1][0], y=t1[1][1], yaw=0, ball=ball)
r3 = p.robot(x=t2[0][0], y=t2[0][1], yaw=3.14, ball=ball)
r4 = p.robot(x=t2[1][0], y=t2[1][1], yaw=3.14, ball=ball)
ball = p.ball(x=0, y=0)
gs = 0
def posepub():
pub = rospy.Publisher('robot1n1/pose', Pose, queue_size=10)
pubball = rospy.Publisher('ballpose', Pose, queue_size=10)
pose = Pose()
bpose = Pose()
rate = rospy.Rate(60)
pg.init()
x = -50
y = 0
mybot = p.robot(x=x,y=y)
ball = p.ball()
i =0
while not rospy.is_shutdown():
for event in pg.event.get():
if event.type == pg.QUIT:
sys.exit()
if (i < 10):
mybot.movebot(1,0,0.2)
i =i+1
[ball.xd,ball.yd] = p.collRb(mybot,ball)
ball.updatestate()
bpose.position.x = ball.x
bpose.position.y = ball.y
p.walleffect(mybot)
p.walleffect(ball)
pose.position.x = mybot.x
pose.position.y = mybot.y
pose.orientation.z = m.tan(mybot.theta/2)
pose.orientation.w =1
bpose.orientation.w =1
pub.publish(pose)
pubball.publish(bpose)
rate.sleep()
def posepub(xtg, ytg, x, y, ango, bx, by, ang, k):
pub = rospy.Publisher('robot1n1/pose', Pose, queue_size=10)
pubball = rospy.Publisher('ballpose', Pose, queue_size=10)
pose = Pose()
bpose = Pose()
k = k
rate = rospy.Rate(60)
pg.init()
ball = p.ball(x=bx, y=by)
mybot = p.robot(x=x, y=y, yaw=ango, ball=ball)
mybotpid = pid.pid(x=x, y=y, ball=ball, angle=ango)
ko = 0
while not rospy.is_shutdown():
for event in pg.event.get():
if event.type == pg.QUIT:
sys.exit()
mybotpid.gtg(xtg, ytg, mybot, ball, thtg=ang)
if mybot.dribble == 0:
p.collRb(mybot, ball)
bpose.position.x = ball.x
bpose.position.y = ball.y
p.walleffect(mybot)
p.walleffect(ball)
pose.position.x = mybot.x
pose.position.y = mybot.y
pose.orientation.z = m.tan(mybot.theta / 2)
pose.orientation.w = 1
bpose.orientation.w = 1
pub.publish(pose)
pubball.publish(bpose)
if (p.dist(mybot.x, mybot.y, xtg, ytg) < 10
and abs(mybot.theta - angle) < 0.1 and ball.speed <= 3
and ko == 0):
if k == 1 and mybot.dribble == 1:
mybot.kick(ball, 5)
ko = 1
#print ball.speed
if (p.dist(mybot.x, mybot.y, xtg, ytg) < 10
and abs(mybot.theta - angle) < 0.1 and ball.speed <= 3
and ko == 1):
return mybot.x, mybot.y, mybot.theta, ball.x, ball.y
oldx = mybot.x
oldy = mybot.y
rate.sleep()
rospy.Subscriber('robot2n0/ptg',goalmsg,r20callback) # for native to node p2p control
rospy.Subscriber('robot2n1/ptg',goalmsg,r21callback) # for native to node p2p control
rospy.Subscriber('robot1n0/traj_vect',game,r10tcallback) # for the trajectory node bypass
rospy.Subscriber('robot1n1/traj_vect',game,r11tcallback) # for the trajectory node bypass
rospy.Subscriber('robot2n0/traj_vect',game,r20tcallback) # for the trajectory node bypass
rospy.Subscriber('robot2n1/traj_vect',game,r21tcallback) # for the trajectory node bypass
rospy.Subscriber('ballpose', Pose,ballposecallback)
rospy.Subscriber('balltwist',Twist,balltwistcallback)
rospy.Subscriber('robot1n0/pose',Pose,r10posecallback)
rospy.Subscriber('robot1n1/pose',Pose,r11posecallback)
rospy.Subscriber('robot2n0/pose',Pose,r20posecallback)
rospy.Subscriber('robot2n1/pose',Pose,r21posecallback)
rospy.Subscriber('robot1n0/twist',Twist,r10twistcallback)
rospy.Subscriber('robot1n1/twist',Twist,r11twistcallback)
rospy.Subscriber('robot2n0/twist',Twist,r20twistcallback)
rospy.Subscriber('robot2n1/twist',Twist,r21twistcallback)
rospy.Subscriber('robot1n0/cselect',Int32,r10selcallback)
rospy.Subscriber('robot1n1/cselect',Int32,r11selcallback)
rospy.Subscriber('robot2n0/cselect',Int32,r20selcallback)
rospy.Subscriber('robot2n1/cselect',Int32,r21selcallback)
rospy.spin()
if __name__ == '__main__':
rospy.init_node('p2p', anonymous=True)
r1.append(p.robot(x= 0.0,y=0.0, yaw = 0, ball = ball))
r1.append(p.robot(x= 0.0,y= 0.0, yaw = 0, ball = ball))
r2.append(p.robot(x= 0.0,y=0.0, yaw = 3.14, ball = ball))
r2.append(p.robot(x= 0.0,y=0.0, yaw = 3.14, ball = ball))
run()
from geometry_msgs.msg import Pose, Twist
from sbsim.msg import goalmsg
import controller as c
from std_msgs.msg import Int32
from std_msgs.msg import Float64
from sbsim.msg import dribble
import random as rnd
import time
import tf
flag = 1
bpose = Pose()
btwist = Twist()
d = 0
ball = p.ball(x=0, y=0)
robot = p.robot(x=0, y=0, ball=ball)
gs = 0
def callback(msg):
global flag
flag = 0
def bcallback(msg):
global bpose
bpose = msg
def btcallback(msg):
global btwist
from sbsim.msg import goalmsg
import controller as c
from std_msgs.msg import Int32
from std_msgs.msg import Float64
from sbsim.msg import dribble
from sbsim.msg import game
from sbsim.msg import path
import random as rnd
import time
import tf
flag = 0
bpose = Pose()
d = 0
ball1n0 = p.ball(x=0, y=0)
robot1n0 = p.robot(x=0, y=0, ball=ball1n0)
ball1n1 = p.ball(x=0, y=0)
robot1n1 = p.robot(x=0, y=0, ball=ball1n1)
ball2n0 = p.ball(x=0, y=0)
robot2n0 = p.robot(x=0, y=0, ball=ball2n0)
ball2n1 = p.ball(x=0, y=0)
robot2n1 = p.robot(x=0, y=0, ball=ball2n1)
"""
program to generate trajectories given goals
"""
#traj_pub1n0 = rospy.Publisher('robot1n0/traj_vect',game, queue_size = 20)
traj_publ1n0 = rospy.Publisher('robot1n0/traj_vect_list', path, queue_size=20)
ppathr1n0 = rospy.Publisher('robot1n0/path', path, queue_size=20)
#traj_pub1n1 = rospy.Publisher('robot1n1/traj_vect',game, queue_size = 20)
def game(t1, t2):
global r10msg
global r11msg
global r20msg
global r21msg
global d
global gs
global r1f
global r2f
global r3f
global r4f
rospy.Subscriber('game/status', Int32, rulecheck)
robotsubinit()
pubball = rospy.Publisher('ballpose', Pose, queue_size=10)
pubbtwist = rospy.Publisher('balltwist', Twist, queue_size=10)
drib = rospy.Publisher('game/dribbler', Int32, queue_size=10)
yis = rospy.Publisher('game/dribdist', Float64, queue_size=10)
pr1 = []
pr2 = []
a, r1r = robotpubinit(1, 0)
pr1.append(a)
a, r2r = robotpubinit(1, 1)
pr1.append(a)
a, r3r = robotpubinit(2, 0)
pr2.append(a)
a, r4r = robotpubinit(2, 1)
pr2.append(a)
btwist = Twist()
rate = rospy.Rate(60)
while True:
ball = p.ball(x=0, y=0)
bpose = Pose()
r1 = []
r2 = []
r1.append(p.robot(x=t1[0][0], y=t1[0][1], yaw=0, ball=ball))
r1.append(p.robot(x=t1[1][0], y=t1[1][1], yaw=0, ball=ball))
r2.append(p.robot(x=t2[0][0], y=t2[0][1], yaw=3.14, ball=ball))
r2.append(p.robot(x=t2[1][0], y=t2[1][1], yaw=3.14, ball=ball))
rpose = [Pose(), Pose(), Pose(), Pose()]
updatebpose(bpose, ball)
updatebtwist(btwist, ball)
updaterpose(rpose[0], r1[0])
updaterpose(rpose[1], r1[1])
updaterpose(rpose[2], r2[0])
updaterpose(rpose[3], r2[1])
pr1[0].publish(rpose[0])
pr1[1].publish(rpose[1])
pr2[0].publish(rpose[2])
pr2[1].publish(rpose[3])
pubball.publish(bpose)
while not rospy.is_shutdown():
if gs == 0:
c.control(r10msg, r1[0], ball)
c.control(r11msg, r1[1], ball)
c.control(r20msg, r2[0], ball)
c.control(r21msg, r2[1], ball)
p.collRR(r1[0], r2[0])
p.collRR(r1[0], r2[1])
p.collRR(r1[0], r1[1])
p.collRR(r1[1], r2[0])
p.collRR(r1[1], r2[1])
p.collRR(r2[0], r2[1])
dribbletest(r1[0], r1[1], r2[0], r2[1])
ball.updatestate(d)
updatebpose(bpose, ball)
updatebtwist(btwist, ball)
x1 = updaterpose(rpose[0], r1[0])
x2 = updaterpose(rpose[1], r1[1])
x3 = updaterpose(rpose[2], r2[0])
x4 = updaterpose(rpose[3], r2[1])
x = [x1, x2, x3, x4]
y = max(x)
yis.publish(y)
r1r.publish(r1f)
r2r.publish(r2f)
r3r.publish(r3f)
r4r.publish(r4f)
pr1[0].publish(rpose[0])
pr1[1].publish(rpose[1])
pr2[0].publish(rpose[2])
pr2[1].publish(rpose[3])
pubball.publish(bpose)
pubbtwist.publish(btwist)
drib.publish(d)
rate.sleep()
else:
dribbletest(r1[0], r1[1], r2[0], r2[1])
updatebpose(bpose, ball)
updatebtwist(btwist, ball)
x1 = updaterpose(rpose[0], r1[0])
x2 = updaterpose(rpose[1], r1[1])
x3 = updaterpose(rpose[2], r2[0])
x4 = updaterpose(rpose[3], r2[1])
x = [x1, x2, x3, x4]
y = max(x)
yis.publish(y)
r1r.publish(r1f)
r2r.publish(r2f)
r3r.publish(r3f)
r4r.publish(r4f)
pr1[0].publish(rpose[0])
pr1[1].publish(rpose[1])
pr2[0].publish(rpose[2])
pr2[1].publish(rpose[3])
pubball.publish(bpose)
pubbtwist.publish(btwist)
drib.publish(d)
rate.sleep()
break
def updatebpose(a,b):
b.x = a.position.x
b.y = a.position.y
def updaterpose(a,b):
b.x = a.position.x
b.y = a.position.y
b.theta = 2 * m.atan(a.orientation.z)
if __name__ == '__main__':
rospy.init_node('rules',anonymous=True)
statuspub = rospy.Publisher('game/status', Int32, queue_size=10)
rate = rospy.Rate(30)
subinit()
b = p.ball(x = ball.position.x,y = ball.position.y)
r1 = p.robot(x =0 ,y =0,yaw =0 ,ball =b)
r2 = p.robot(x =0 ,y =0,yaw =0 ,ball =b)
r3 = p.robot(x =0 ,y =0,yaw =0 ,ball =b)
r4 = p.robot(x =0 ,y =0,yaw =0 ,ball =b)
updaterpose(r10,r1)
updaterpose(r11,r2)
updaterpose(r20,r3)
updaterpose(r21,r4)
while(True):
b.x = ball.position.x
b.y = ball.position.y
updaterpose(r10,r1)
updaterpose(r11,r2)
updaterpose(r20,r3)
updaterpose(r21,r4)
f = boundcheck(b)
def gamefun(t1, t2):
global d
global gs
global r1f
global r2f
global r3f
global r4f
global r1
global r2
global ball
rospy.Subscriber('game/status', Int32, rulecheck)
rospy.Subscriber('ctrl_rx', game, ctrlcallback)
pubball = rospy.Publisher('ballpose', Pose, queue_size=10)
pubbtwist = rospy.Publisher('balltwist', Twist, queue_size=10)
drib = rospy.Publisher('game/dribbler', Int32, queue_size=10)
yis = rospy.Publisher('game/dribdist', Float64, queue_size=10)
pr1 = []
tr1 = []
pr2 = []
tr2 = []
a, r1r, t = robotpubinit(1, 0)
pr1.append(a)
tr1.append(t)
a, r2r, t = robotpubinit(1, 1)
pr1.append(a)
tr1.append(t)
a, r3r, t = robotpubinit(2, 0)
pr2.append(a)
tr2.append(t)
a, r4r, t = robotpubinit(2, 1)
pr2.append(a)
tr2.append(t)
btwist = Twist()
rate = rospy.Rate(60)
while True:
bpose = Pose()
r1.append(p.robot(x=t1[0][0], y=t1[0][1], yaw=0, ball=ball))
r1.append(p.robot(x=t1[1][0], y=t1[1][1], yaw=0, ball=ball))
r2.append(p.robot(x=t2[0][0], y=t2[0][1], yaw=3.14, ball=ball))
r2.append(p.robot(x=t2[1][0], y=t2[1][1], yaw=3.14, ball=ball))
rpose = [Pose(), Pose(), Pose(), Pose()]
rtwist = [Twist(), Twist(), Twist(), Twist()]
updatebpose(bpose, ball)
updatebtwist(btwist, ball)
updaterpose(rpose[0], r1[0])
updaterpose(rpose[1], r1[1])
updaterpose(rpose[2], r2[0])
updaterpose(rpose[3], r2[1])
updatertwist(rtwist[0], r1[0])
updatertwist(rtwist[1], r1[1])
updatertwist(rtwist[2], r2[0])
updatertwist(rtwist[3], r2[1])
pr1[0].publish(rpose[0])
pr1[1].publish(rpose[1])
pr2[0].publish(rpose[2])
pr2[1].publish(rpose[3])
tr1[0].publish(rtwist[0])
tr1[1].publish(rtwist[1])
tr2[0].publish(rtwist[2])
tr2[1].publish(rtwist[3])
pubball.publish(bpose)
pubbtwist.publish(btwist)
while not rospy.is_shutdown():
if gs == 0:
p.collRR(r1[0], r2[0])
p.collRR(r1[0], r2[1])
p.collRR(r1[0], r1[1])
p.collRR(r1[1], r2[0])
p.collRR(r1[1], r2[1])
p.collRR(r2[0], r2[1])
p.walleffect(r1[0])
p.walleffect(r1[1])
p.walleffect(r2[0])
p.walleffect(r2[1])
p.collRb(r1[0], ball)
p.collRb(r1[1], ball)
p.collRb(r2[0], ball)
p.collRb(r2[1], ball)
dribbletest(r1[0], r1[1], r2[0], r2[1])
ball.updatestate(d)
updatebpose(bpose, ball)
updatebtwist(btwist, ball)
x1 = updaterpose(rpose[0], r1[0])
x2 = updaterpose(rpose[1], r1[1])
x3 = updaterpose(rpose[2], r2[0])
x4 = updaterpose(rpose[3], r2[1])
x = [x1, x2, x3, x4]
y = max(x)
yis.publish(y)
r1r.publish(r1f)
r2r.publish(r2f)
r3r.publish(r3f)
r4r.publish(r4f)
pr1[0].publish(rpose[0])
pr1[1].publish(rpose[1])
pr2[0].publish(rpose[2])
pr2[1].publish(rpose[3])
pubball.publish(bpose)
pubbtwist.publish(btwist)
tr1[0].publish(rtwist[0])
tr1[1].publish(rtwist[1])
tr2[0].publish(rtwist[2])
tr2[1].publish(rtwist[3])
drib.publish(d)
rate.sleep()
else:
dribbletest(r1[0], r1[1], r2[0], r2[1])
updatebpose(bpose, ball)
updatebtwist(btwist, ball)
x1 = updaterpose(rpose[0], r1[0])
x2 = updaterpose(rpose[1], r1[1])
x3 = updaterpose(rpose[2], r2[0])
x4 = updaterpose(rpose[3], r2[1])
x = [x1, x2, x3, x4]
y = max(x)
yis.publish(y)
r1r.publish(r1f)
r2r.publish(r2f)
r3r.publish(r3f)
r4r.publish(r4f)
pr1[0].publish(rpose[0])
pr1[1].publish(rpose[1])
pr2[0].publish(rpose[2])
pr2[1].publish(rpose[3])
tr1[0].publish(rtwist[0])
tr1[1].publish(rtwist[1])
tr2[0].publish(rtwist[2])
tr2[1].publish(rtwist[3])
pubball.publish(bpose)
pubbtwist.publish(btwist)
drib.publish(d)
rate.sleep()
break
from std_msgs.msg import Float64
from sbsim.msg import dribble
from sbsim.msg import game
from sbsim.msg import hw
import numpy as np
import tf
d = 0
r1f = 1
r2f = 1
r3f = 1
r4f = 1
ball = p.ball(x=0, y=0)
r1 = [p.robot(x=0, y=0, ball=ball), p.robot(x=0, y=0, ball=ball)]
r2 = [p.robot(x=0, y=0, ball=ball), p.robot(x=0, y=0, ball=ball)]
gs = 0
pctrl = rospy.Publisher('ctrl_rx', game, queue_size=20)
def botcallback1(msg):
global r1
global ball
r1[0].x = msg.position.x
r1[0].y = msg.position.y
quat = [
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
def __init__(self, x, y, angle, ball):
self.bot = p.robot(x=x, y=y, ball=ball, yaw=angle)
prod = 0.055
self.kpp = 0.1
self.kpv = prod / self.kpp
self.kpr = 0.02
import random as rnd import numpy as np import matplotlib.pyplot as plt import time import tf """ 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 = [[],[],[],[]]
