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 = [[],[],[],[]]