def __init__(self): # This sets up a variable for every component on this form. # For example, if we've drawn a button called "send_button", we can # refer to it as self.send_button: self.init_components() # Any code you write here will run when the form opens. #Uncomment as required. self.running= False self.reset = True self.dt = self.timer.interval/(1e7) self.first = True self.zoom = False self.t = 0 #SET SCALE (pixels per m, or unit used in code) self.xu =5 self.old = 1 self.paths = [] self.cur_path = [] self.trail= [] self.mousedown = False self.arrowdown = False self.mouse = physics.vector3(0,0) electron = physics.ball(self.me, self.radius) electron.charge = -self.e electron.name = "Electron" electron.E = physics.vector3(0, 1e3, 0) electron.B = physics.vector3(0, 0, 5e-3) positron = physics.ball(self.me, self.radius) positron.charge = self.e positron.name = "Positron" positron.E = physics.vector3(0, 1e3, 0) positron.B = physics.vector3(0, 0, 5e-3) alpha = physics.ball(4*self.mp, self.radius) alpha.charge = 2*self.e alpha.name = "Alpha Particle" alpha.E = physics.vector3(0, 1e5, 0) alpha.B = physics.vector3(0, 0, 5e-1) self.particle_options = [electron, positron, alpha] self.buttons = [] for index, particle in enumerate(self.particle_options): button = RadioButton(text= particle.name, value = index, group_name = "radio_particles") self.buttons.append(button) if index == 0: button.selected = True self.grid_particles.add_component(button, row = "A", col_xs = 2*index + 1, width_xs = 2) self.custom = RadioButton(text = "Custom", value = len(self.particle_options), group_name = "radio_particles") self.grid_particles.add_component(self.custom, row ="A", col_xs = 2*len(self.particle_options) + 1, width_xs =2) self.buttons.append(self.custom) self.param_boxes = self.buttons + self.grid_custom.get_components() + self.grid_fields.get_components() + self.grid_vel.get_components()
def initialize(self): #particles index = 0 for i, button in enumerate(self.buttons): if button.selected: index = i if index == len(self.particle_options): mass = 0.1 Charge = 0 if float(self.txt_mass.text)>0: mass = float(self.txt_mass.text) if len(self.txt_charge.text)>0: charge = float(self.txt_charge.text) self.ball = physics.ball(mass, self.radius, self.ball.pos.x, self.ball.pos.y) self.ball.name = self.txt_name.text self.ball.charge = charge else: self.ball = self.particle_options[index] v = physics.vector3(self.v, 0) if len(self.txt_xsp.text)>0 and len(self.txt_ysp.text)>0: v = physics.vector3(float(self.txt_xsp.text), float(self.txt_ysp.text)) self.ball.vel = v #fields self.E = physics.vector3(float(self.txt_E_x.text),float(self.txt_E_y.text), float(self.txt_E_z.text)) self.B = physics.vector3(float(self.txt_B_x.text),float(self.txt_B_y.text), float(self.txt_B_z.text)) self.trail = [self.ball.pos] self.draw_all()
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 __init__(self): # This sets up a variable for every component on this form. # For example, if we've drawn a button called "send_button", we can # refer to it as self.send_button: self.init_components() # Any code you write here will run when the form opens. #Uncomment as required. self.running= False self.reset = True self.zoom = False self.bug = physics.ball(1, self.bugradius) self.N = int(self.txt_N.text) self.L = float(self.txt_L.text) self.v = float(self.txt_v.text) self.a = 2*math.pi/self.N self.dt = self.timer.interval self.t = 0 #list of parameter inputs self.param_boxes = [] self.param_boxes.append(self.txt_N) self.param_boxes.append(self.txt_L) self.param_boxes.append(self.txt_v)
def __init__(self): # This sets up a variable for every component on this form. # For example, if we've drawn a button called "send_button", we can # refer to it as self.send_button: self.init_components() # Any code you write here will run when the form opens. #Uncomment as required. self.running = False self.reset = True self.zoom = False self.bug = physics.ball(1, self.bugradius) self.N = int(self.txt_N.text) self.L = float(self.txt_L.text) self.v = float(self.txt_v.text) self.a = 2 * math.pi / self.N self.dt = self.timer.interval self.t = 0 #list of parameter inputs self.param_boxes = [] self.param_boxes.append(self.txt_N) self.param_boxes.append(self.txt_L) self.param_boxes.append(self.txt_v)
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 init_ball(self): xsp = 0 ysp = 0 ang = round(float(self.txt_ang.text),2)*math.pi/180 v =float(self.txt_spd.text) if v<50: self.v = v if ang>0: xsp = self.v*math.cos(ang) ysp = self.v*math.sin(ang) self.ball = physics.ball(1, 0.2) self.ball.pos = physics.vector3(self.ball.radius, self.ball.radius+self.floor_height) self.ball.vel = physics.vector3(xsp, ysp) self.path = 0
def init_ball(self): xsp = 0 ysp = 0 ang = round(float(self.txt_ang.text), 2) * math.pi / 180 v = float(self.txt_spd.text) if v < 50: self.v = v if ang > 0: xsp = self.v * math.cos(ang) ysp = self.v * math.sin(ang) self.ball = physics.ball(1, 0.2) self.ball.pos = physics.vector3(self.ball.radius, self.ball.radius + self.floor_height) self.ball.vel = physics.vector3(xsp, ysp) self.path = 0
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()
def __init__(self): # This sets up a variable for every component on this form. # For example, if we've drawn a button called "send_button", we can # refer to it as self.send_button: self.init_components() # Any code you write here will run when the form opens. #Uncomment as required. self.running = False self.reset = True self.zoom = False self.bug = physics.ball(1, self.bugradius) self.path = [self.bug.pos, self.bug.pos] self.counter = 0 self.first = True self.dt = self.timer.interval self.t = 0 #list of parameter inputs self.param_boxes = []
def __init__(self): # This sets up a variable for every component on this form. # For example, if we've drawn a button called "send_button", we can # refer to it as self.send_button: self.init_components() # Any code you write here will run when the form opens. #Uncomment as required. self.running= False self.reset = True self.zoom = False self.bug = physics.ball(1, self.bugradius) self.path = [self.bug.pos, self.bug.pos] self.counter = 0 self.first = True self.dt = self.timer.interval self.t = 0 #list of parameter inputs self.param_boxes = []
def init_ball(self): xsp = 0 ysp = 0 self.v = 0 ang = round(float(self.txt_ang.text))*math.pi/180 v = float(self.txt_v.text) if 20>=v>0: self.v = v if ang>0: xsp = self.v*math.cos(ang) ysp = self.v*math.sin(ang) self.ball = physics.ball(1, 0.1) self.ball.pos = physics.vector3(self.ball.radius, self.ball.radius+self.floor_height) self.ball.vel = physics.vector3(xsp, ysp) self.d = self.v**2/self.g.mag() d =self.d if d>0: self.xu = self.cw/(d+3*self.ball.radius) self.path = 0
def init_ball(self): xsp = 0 ysp = 0 self.v = 0 ang = round(float(self.txt_ang.text)) * math.pi / 180 v = float(self.txt_v.text) if 20 >= v > 0: self.v = v if ang > 0: xsp = self.v * math.cos(ang) ysp = self.v * math.sin(ang) self.ball = physics.ball(1, 0.1) self.ball.pos = physics.vector3(self.ball.radius, self.ball.radius + self.floor_height) self.ball.vel = physics.vector3(xsp, ysp) self.d = self.v**2 / self.g.mag() d = self.d if d > 0: self.xu = self.cw / (d + 3 * self.ball.radius) self.path = 0
def initialize(self): #particles index = 0 for i, button in enumerate(self.buttons): if button.selected: index = i if index == len(self.particle_options): mass = 0.1 Charge = 0 if float(self.txt_mass.text) > 0: mass = float(self.txt_mass.text) if len(self.txt_charge.text) > 0: charge = float(self.txt_charge.text) self.ball = physics.ball(mass, self.radius, self.ball.pos.x, self.ball.pos.y) self.ball.name = self.txt_name.text self.ball.charge = charge else: self.ball = self.particle_options[index] v = physics.vector3(self.v, 0) if len(self.txt_xsp.text) > 0 and len(self.txt_ysp.text) > 0: v = physics.vector3(float(self.txt_xsp.text), float(self.txt_ysp.text)) self.ball.vel = v #fields self.E = physics.vector3(float(self.txt_E_x.text), float(self.txt_E_y.text), float(self.txt_E_z.text)) self.B = physics.vector3(float(self.txt_B_x.text), float(self.txt_B_y.text), float(self.txt_B_z.text)) self.trail = [self.ball.pos] self.draw_all()
def timer_1_tick (self, **event_args): # This method is called Every [interval] seconds self.dt = self.timer_1.interval dt = self.dt self.cw = self.can_lab.get_width() self.ch = self.can_lab.get_height() #initalize balls at first iteration if self.first: #pixels per m, based on large ball diameter, canvas 10 ball widths self.xu = self.cw/(self.bigrad*2*10) self.balls = [] for i in range(0,4): x = physics.ball(1, self.bigrad) self.balls.append(x) self.slider1 = draw.slider(self.can_slid, mini= 0.1, maxi = 4, stepsize = 0.1, start=1) self.slider1.indicator = True self.slider1.draw() self.param_boxes.append(self.slider1) self.init_balls() self.init_balls_pos() self.first = False self.xu = self.cw/(self.bigrad*2*10)*self.slider1.value #draw everything self.draw_all() if self.reset: self.init_balls() if self.running: for ball in self.balls: ball.move(dt) self.check(self.balls[0], self.balls[1], True) self.check(self.balls[2], self.balls[3], False) KElab = 0.5 *(self.balls[0].mass*self.balls[0].vel.mag()**2 + self.balls[1].mass*self.balls[1].vel.mag()**2) MOlab = self.balls[0].momentum() + self.balls[1].momentum() KEzmf = 0.5 *(self.balls[2].mass*self.balls[2].vel.mag()**2 + self.balls[3].mass*self.balls[3].vel.mag()**2) MOzmf = self.balls[2].momentum() + self.balls[3].momentum() zmfvel = self.balls[0].zmf_vel(self.balls[1]) self.lbl_kelab.text = "Total KE = {0}J".format(repr(round(KElab, 3))) self.lbl_kezmf.text = "Total KE = {0}J".format(repr(round(KEzmf, 3))) self.lbl_molab.text = "Total Momentum = ({0}, {1}) kgms-1".format(repr(round(MOlab.x, 3)), repr(round(MOlab.y, 3))) self.lbl_mozmf.text = "Total Momentum = ({0}, {1}) kgms^-1".format(repr(round(MOzmf.x, 3)), repr(round(MOzmf.y, 3))) self.lbl_zmfvel.text = "v = ({0}, {1}) ms^-1".format(repr(round(zmfvel.x, 3)), repr(round(zmfvel.y, 3))) self.lbl_box.text = "(Box: {0}m x {1}m)".format(repr(round(self.cw/self.xu, 3)), repr(round(self.ch/self.xu, 3))) self.lbl_vx_1.text = repr(round(self.balls[0].vel.x,3)) self.lbl_vy_1.text = repr(round(self.balls[0].vel.y,3)) self.lbl_vx_2.text = repr(round(self.balls[1].vel.x,3)) self.lbl_vy_2.text = repr(round(self.balls[1].vel.y,3)) if self.zoom: self.xu += (self.newxu-self.oldxu)/20 self.init_balls_pos() if -0.05 <=(self.xu - self.newxu) <= 0.05: self.zoom = False self.slider1.draw()
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
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 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)
def __init__(self): # This sets up a variable for every component on this form. # For example, if we've drawn a button called "send_button", we can # refer to it as self.send_button: self.init_components() # Any code you write here will run when the form opens. #Uncomment as required. self.running = False self.reset = True self.dt = self.timer.interval / (1e7) self.first = True self.zoom = False self.t = 0 #SET SCALE (pixels per m, or unit used in code) self.xu = 5 self.old = 1 self.paths = [] self.cur_path = [] self.trail = [] self.mousedown = False self.arrowdown = False self.mouse = physics.vector3(0, 0) electron = physics.ball(self.me, self.radius) electron.charge = -self.e electron.name = "Electron" electron.E = physics.vector3(0, 1e3, 0) electron.B = physics.vector3(0, 0, 5e-3) positron = physics.ball(self.me, self.radius) positron.charge = self.e positron.name = "Positron" positron.E = physics.vector3(0, 1e3, 0) positron.B = physics.vector3(0, 0, 5e-3) alpha = physics.ball(4 * self.mp, self.radius) alpha.charge = 2 * self.e alpha.name = "Alpha Particle" alpha.E = physics.vector3(0, 1e5, 0) alpha.B = physics.vector3(0, 0, 5e-1) self.particle_options = [electron, positron, alpha] self.buttons = [] for index, particle in enumerate(self.particle_options): button = RadioButton(text=particle.name, value=index, group_name="radio_particles") self.buttons.append(button) if index == 0: button.selected = True self.grid_particles.add_component(button, row="A", col_xs=2 * index + 1, width_xs=2) self.custom = RadioButton(text="Custom", value=len(self.particle_options), group_name="radio_particles") self.grid_particles.add_component( self.custom, row="A", col_xs=2 * len(self.particle_options) + 1, width_xs=2) self.buttons.append(self.custom) self.param_boxes = self.buttons + self.grid_custom.get_components( ) + self.grid_fields.get_components() + self.grid_vel.get_components()
def timer_1_tick(self, **event_args): # This method is called Every [interval] seconds self.dt = self.timer_1.interval dt = self.dt self.cw = self.can_lab.get_width() self.ch = self.can_lab.get_height() #initalize balls at first iteration if self.first: #pixels per m, based on large ball diameter, canvas 10 ball widths self.xu = self.cw / (self.bigrad * 2 * 10) self.balls = [] for i in range(0, 4): x = physics.ball(1, self.bigrad) self.balls.append(x) self.slider1 = draw.slider(self.can_slid, mini=0.1, maxi=4, stepsize=0.1, start=1) self.slider1.indicator = True self.slider1.draw() self.param_boxes.append(self.slider1) self.init_balls() self.init_balls_pos() self.first = False self.xu = self.cw / (self.bigrad * 2 * 10) * self.slider1.value #draw everything self.draw_all() if self.reset: self.init_balls() if self.running: for ball in self.balls: ball.move(dt) self.check(self.balls[0], self.balls[1], True) self.check(self.balls[2], self.balls[3], False) KElab = 0.5 * (self.balls[0].mass * self.balls[0].vel.mag()**2 + self.balls[1].mass * self.balls[1].vel.mag()**2) MOlab = self.balls[0].momentum() + self.balls[1].momentum() KEzmf = 0.5 * (self.balls[2].mass * self.balls[2].vel.mag()**2 + self.balls[3].mass * self.balls[3].vel.mag()**2) MOzmf = self.balls[2].momentum() + self.balls[3].momentum() zmfvel = self.balls[0].zmf_vel(self.balls[1]) self.lbl_kelab.text = "Total KE = {0}J".format(repr(round(KElab, 3))) self.lbl_kezmf.text = "Total KE = {0}J".format(repr(round(KEzmf, 3))) self.lbl_molab.text = "Total Momentum = ({0}, {1}) kgms-1".format( repr(round(MOlab.x, 3)), repr(round(MOlab.y, 3))) self.lbl_mozmf.text = "Total Momentum = ({0}, {1}) kgms^-1".format( repr(round(MOzmf.x, 3)), repr(round(MOzmf.y, 3))) self.lbl_zmfvel.text = "v = ({0}, {1}) ms^-1".format( repr(round(zmfvel.x, 3)), repr(round(zmfvel.y, 3))) self.lbl_box.text = "(Box: {0}m x {1}m)".format( repr(round(self.cw / self.xu, 3)), repr(round(self.ch / self.xu, 3))) self.lbl_vx_1.text = repr(round(self.balls[0].vel.x, 3)) self.lbl_vy_1.text = repr(round(self.balls[0].vel.y, 3)) self.lbl_vx_2.text = repr(round(self.balls[1].vel.x, 3)) self.lbl_vy_2.text = repr(round(self.balls[1].vel.y, 3)) if self.zoom: self.xu += (self.newxu - self.oldxu) / 20 self.init_balls_pos() if -0.05 <= (self.xu - self.newxu) <= 0.05: self.zoom = False self.slider1.draw()
""" if gmsg.status == 0 bot is stationary if gmsg.status == 1 bot moves to location in gmsg if gmsg.status == 2 ball kicked at target location if rxycs == 1 p2p control pass if rxycs == 2 trajectory command bypass if rxycs == 3 extremal control """ flag=0 ctrl=game() traj_ctrl = game() ctrl.kick=0 ball = p.ball(x = 0,y = 0) bpose = Pose() btwist = Twist() rpose = [Pose(),Pose(),Pose(),Pose()] rtwist =[Twist(),Twist(),Twist(),Twist()] r10msg = goalmsg() r11msg = goalmsg() r20msg = goalmsg() r21msg = goalmsg() r10cs = 1 r11cs = 1 r20cs = 1 r21cs = 1 r1f = 1 r2f = 1 r3f =1
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)