def run():
pos1 = [150, 150]
pos2 = [-150, 150]
pos3 = [-150, -150]
pos4 = [150, -150]
b1mov = mov()
b2mov = mov()
b3mov = mov()
b4mov = mov()
b1mov.mode = 1
b2mov.mode = 1
b3mov.mode = 1
b4mov.mode = 1
b1mov.x = pos1[0]
b1mov.y = pos1[1]
b2mov.x = pos2[0]
b2mov.y = pos2[1]
b3mov.x = pos3[0]
b3mov.y = pos3[1]
b4mov.x = pos4[0]
b4mov.y = pos4[1]
rate = rospy.Rate(60)
b1tpub = rospy.Publisher("bot1mov", mov, queue_size=10)
b2tpub = rospy.Publisher("bot2mov", mov, queue_size=10)
b3tpub = rospy.Publisher("bot3mov", mov, queue_size=10)
b4tpub = rospy.Publisher("bot4mov", mov, queue_size=10)
i = 0
while (True):
b1tpub.publish(b1mov)
b2tpub.publish(b2mov)
b3tpub.publish(b3mov)
b4tpub.publish(b4mov)
rate.sleep()
def summa():
global robot
global ball
global ballv
rospy.init_node('autogk', anonymous=True)
r1 = rospy.Publisher('bot1mov', mov, queue_size=10)
rospy.Subscriber('ballpose', Pose, bcallback)
rospy.Subscriber('bot1pose', Pose, botcallback)
rospy.Subscriber('balltwist', Twist, btcallback)
updatebpose(bpose, ball)
updatebtwist(btwist, ballv)
r = mov()
rate = rospy.Rate(10)
while (True):
r.x = 0
r.y = 0
r.mode = 1
r1.publish(r)
rate.sleep()
while (True):
updatebpose(bpose, ball)
updatebtwist(btwist, ballv)
r.x = ball.x
r.y = ball.y
r.thetap = g.angb(ball, robot)
r1.publish(r)
rate.sleep()
def summa():
global robot
global ball
global ballv
global d
global gs
rospy.init_node('autogk',anonymous=True)
r1 = rospy.Publisher('bot1mov',mov, queue_size = 10)
rospy.Subscriber('ballpose', Pose, bcallback)
rospy.Subscriber('bot1pose', Pose, botcallback)
rospy.Subscriber('balltwist', Twist, btcallback)
updatebpose(bpose,ball)
updatebtwist(btwist,ballv)
r = mov()
rate = rospy.Rate(10)
while(True):
r.x = 0
r.y = 0
r.mode = 1
r1.publish(r)
rate.sleep()
while(True):
updatebpose(bpose,ball)
updatebtwist(btwist,ballv)
if not(ballv.x == 0):
m0 = ballv.y/ballv.x
th = m.atan(m0)
th = 3.14 + th
q = m.tan(th/2)
k = 210
xb= ball.x
yb= ball.y
xtg = k
ytg = m0*(k-xb)+yb
if abs(ytg)>=150:
ytg = 0
r.x = xtg
r.y = ytg
r.mode = 1
r.thetap = m.pi
r1.publish(r)
else:
r.x = -210
r.y = 0
r.mode = 1
r.thetap = m.pi
r1.publish(r)
rate.sleep()
def run():
b1mov = mov()
rate = rospy.Rate(60)
pygame.init()
pygame.joystick.init()
b1tpub = rospy.Publisher("bot1mov",mov,queue_size = 10)
i = 0
bn = 0
while(True):
for event in pygame.event.get():
if event.type == pygame.QUIT:
done=True
joystick_count = pygame.joystick.get_count()
for i in range(joystick_count):
joystick = pygame.joystick.Joystick(i)
joystick.init()
name = joystick.get_name()
axes = joystick.get_numaxes()
hats = joystick.get_numhats()
for i in range( hats ):
hat = joystick.get_hat( i )
a,b = hat
buttons = joystick.get_numbuttons()
k = joystick.get_button(0) ## 0 or 1 -> kick
if a == 1:
bn = 1
if b == 1:
bn = 0
if b ==-1:
bn = 2
if a ==-1:
bn = 3
b1mov = pos[bn]
print b1mov.x,b1mov.y,bn
b1tpub.publish(b1mov)
rate.sleep()
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist
import math as m
from geometry_msgs.msg import Pose
from sbhw.msg import mov
bmov1 = mov()
bmov2 = mov()
bmov3 = mov()
bmov4 = mov()
bp1 = Pose()
bp2 = Pose()
bp3 = Pose()
bp4 = Pose()
reached = [0,0,0,0]
def b1p(msg):
global bp1
bp1 = msg
return 0
def b2p(msg):
global bp2
bp2 = msg
return 0
def b3p(msg):
global bp3
bp3 = msg
#!/usr/bin/env python import rospy from std_msgs.msg import Int32 from geometry_msgs.msg import Twist import math as m from geometry_msgs.msg import Pose from sbhw.msg import mov import pygame pos1 = mov() pos2 = mov() pos3 = mov() pos4 = mov() pos1.mode = 1 pos2.mode = 1 pos3.mode = 1 pos4.mode = 1 pos1.x = 200 pos1.y = 150 pos2.x = 200 pos2.y = -150 pos3.x = -200 pos3.y = -150 pos4.x = -200 pos4.y = 150
def talker():
global robot
global ball
global ballv
rospy.init_node('joystick', anonymous=True)
pubd2 = rospy.Publisher('bot2d', Int32, queue_size=10)
pubk2 = rospy.Publisher('bot2k', Int32, queue_size=10)
pubv2 = rospy.Publisher('bot2mov', mov, queue_size=10)
rospy.Subscriber('ballpose', Pose, bcallback)
rospy.Subscriber('bot2pose', Pose, botcallback)
rospy.Subscriber('balltwist', Twist, btcallback)
rate = rospy.Rate(60)
while not rospy.is_shutdown():
pygame.init()
rate = rospy.Rate(60)
pygame.joystick.init()
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
done = True
updatebpose(bpose, ball)
updatebtwist(btwist, ballv)
joystick_count = pygame.joystick.get_count()
for i in range(joystick_count):
joystick = pygame.joystick.Joystick(i)
joystick.init()
name = joystick.get_name()
axes = joystick.get_numaxes()
vx = joystick.get_axis(3)
vx = vx * abs(vx)
vy = joystick.get_axis(4)
vy = vy * abs(vy)
ds = joystick.get_axis(5)
hats = joystick.get_numhats()
for i in range(hats):
hat = joystick.get_hat(i)
a, b = hat
print a, b
buttons = joystick.get_numbuttons()
k = joystick.get_button(0) ## 0 or 1 -> kick
s = joystick.get_button(1) # Switch robot
LB = joystick.get_button(
4) ## 0 or 1 -> anti-clockwise rotation
RB = joystick.get_button(5) ## 0 or 1 -> clockwise rotation
if (LB == 0 and RB == 1):
wz = -0.1
if (LB == 1 and RB == 0):
wz = 0.1
if ((LB == 0 and RB == 0) or (LB == 1 and RB == 1)):
wz = 0
vx = vx * 0.06
vy = -vy * 0.06
print 'vx', vx
print 'vy', vy
if ds < 0:
ds = 0 ## value from 0 to 1
ds = int(ds * 255)
tw = mov()
tw.mode = 2
tw.mag = vx**2 + vy**2
if not (vx == 0) and vx > 0:
tw.thetav = m.atan(vy / vx)
if not (vx == 0) and vx < 0 and vy < 0:
tw.thetav = m.atan(vy / vx) + -m.pi
if not (vx == 0) and vx < 0 and vy > 0:
tw.thetav = m.atan(vy / vx) + m.pi
tw.thetap = g.angb(robot, ball)
print tw.mag, tw.thetav, tw.thetap
pubv2.publish(tw)
pubd2.publish(ds)
pubk2.publish(k)
rate.sleep()