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)
