def summa():
global flag
rospy.init_node('randomstrikert1',anonymous=True)
r1 = rospy.Publisher('robot1n0/ptg',goalmsg, queue_size = 10)
rospy.Subscriber('robot1n0/reached',Int32,callback)
rate = rospy.Rate(0.5)
r = goalmsg()
rate.sleep()
r.posetogo.position.x = 50
r.posetogo.orientation.w = 1
r.status = 1
r1.publish(r)
rate.sleep()
r.posetogo.position.x = 150
r.posetogo.orientation.w = 1
r.status = 1
r1.publish(r)
rate.sleep()
flag = 1
r.posetogo.position.x = 250
a = rnd.randint(1,2)
if a == 2:
a = -1
b = rnd.randint(1,80)
n = a*b
r.posetogo.position.y = n
r.posetogo.orientation.z = -a*m.sin(b/200.0)
r.posetogo.orientation.w = 1
r.status = 2
r1.publish(r)
rate.sleep()
def summa():
global flag
global bpose
global btwist
rospy.init_node('movcheck', anonymous=True)
r1 = rospy.Publisher('robot1n0/ptg', goalmsg, queue_size=10)
r = goalmsg()
rate = rospy.Rate(10)
r.posetogo.position.x = 300
r.posetogo.position.y = 300
r.posetogo.orientation.z = 0
r.posetogo.orientation.w = 1
r.status = 1
r1.publish(r)
rate.sleep()
rate.sleep()
rate.sleep()
i = 0
while (True):
r.posetogo.position.x = 300 + 40 * m.sin(2 * 3.14 * i / 100)
r.posetogo.position.y = 300 + 40 * m.cos(2 * 3.14 * i / 100)
r.posetogo.orientation.z = 0
r.posetogo.orientation.w = 1
r.status = 1
r1.publish(r)
i = i + 1
rate.sleep()
def run():
rospy.Subscriber('/robot1n0/vtg', dirvect, r10vcallback)
rospy.Subscriber('/robot1n1/vtg', dirvect, r11vcallback)
rospy.Subscriber('/robot2n0/vtg', dirvect, r20vcallback)
rospy.Subscriber('/robot2n1/vtg', dirvect, r21vcallback)
rospy.Subscriber('/robot1n0/pose', Pose, r10pcallback)
rospy.Subscriber('/robot1n1/pose', Pose, r11pcallback)
rospy.Subscriber('/robot2n0/pose', Pose, r20pcallback)
rospy.Subscriber('/robot2n1/pose', Pose, r21pcallback)
r10pub = rospy.Publisher('/robot1n0/ptg', goalmsg, queue_size=10)
r11pub = rospy.Publisher('/robot1n1/ptg', goalmsg, queue_size=10)
r20pub = rospy.Publisher('/robot2n0/ptg', goalmsg, queue_size=10)
r21pub = rospy.Publisher('/robot2n1/ptg', goalmsg, queue_size=10)
r10ptg = goalmsg()
r11ptg = goalmsg()
r20ptg = goalmsg()
r21ptg = goalmsg()
rate = rospy.Rate(20)
while (True):
r10ptg = findgoal(r10v, r10p)
r11ptg = findgoal(r11v, r11p)
r20ptg = findgoal(r20v, r20p)
r21ptg = findgoal(r21v, r21p)
if not r10ptg.status == 0:
r10pub.publish(r10ptg)
r10ptg.status = 0
if not r11ptg.status == 0:
r11pub.publish(r11ptg)
r11ptg.status = 0
if not r20ptg.status == 0:
r20pub.publish(r20ptg)
r20ptg.status = 0
if not r21ptg.status == 0:
r21pub.publish(r21ptg)
r21ptg.status = 0
rate.sleep()
def summa():
global robot
global ball
global d
global gs
rospy.init_node('autogk', anonymous=True)
r1 = rospy.Publisher('robot2n1/ptg', goalmsg, queue_size=10)
rospy.Subscriber('ballpose', Pose, bcallback)
rospy.Subscriber('robot1n0/pose', Pose, botcallback)
rospy.Subscriber('balltwist', Twist, btcallback)
rospy.Subscriber('game/dribbler', Int32, dcallback)
rospy.Subscriber('game/status', Int32, gcallback)
rospy.Subscriber('robot2n1/reached', Int32, callback)
updatebpose(bpose, ball)
updatebtwist(btwist, ball)
r = goalmsg()
rate = rospy.Rate(10)
while (True):
r.posetogo.position.x = 250
r.posetogo.position.y = 0
r.posetogo.orientation.z = 0
r.posetogo.orientation.w = 1
r.status = 1
r1.publish(r)
rate.sleep()
while (True):
updatebpose(bpose, ball)
updatebtwist(btwist, ball)
if not (ball.xd == 0):
if d == 0:
m0 = ball.yd / ball.xd
else:
m0 = (ball.y - robot.y) / (ball.x - robot.x)
th = m.atan(m0)
th = 3.14 + th
q = m.tan(th / 2)
k = 420
xb = ball.x
yb = ball.y
xtg = k
ytg = m0 * (k - xb) + yb
if abs(ytg) >= 400:
ytg = 0
r.posetogo.position.x = xtg
r.posetogo.position.y = ytg
r.posetogo.orientation.z = q
r.posetogo.orientation.w = 1
r.status = 1
r1.publish(r)
if gs == 1 or gs == 2 or gs == 3:
break
rate.sleep()
def findgoal(v, robot):
go = goalmsg()
if v.status == 1:
r = v.speed
qz = m.tan(v.ptheta / 2)
po = Pose()
po.position.x = robot.x + r * m.cos(v.vtheta)
po.position.y = robot.y + r * m.sin(v.vtheta)
po.orientation.w = 1
po.orientation.z = qz
go.posetogo = po
go.status = 1
return go
else:
qz = m.tan(v.ptheta / 2)
po = Pose()
po.position.x = robot.x
po.position.y = robot.y
po.orientation.w = 1
po.orientation.z = qz
go.posetogo = po
go.status = 1
return go
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 r4f =1 tag=0 gs = 0 r1=[]
from std_msgs.msg import Float64
import math as ma
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from tf.transformations import euler_from_quaternion, quaternion_from_euler
k=0
check=0
theta=0
i=0
x_b=x_r=0
y_b=y_r=0
xd=yd=0
xo=yo=0
vel_b=vel_r=0
sol_pub=rospy.Publisher('robot1n0/ptg',goalmsg,queue_size=10)
dat=goalmsg()
def botcb(data):
global x_r,y_r
x_r=data.position.x
y_r=data.position.y
def botwistcb(data):
global vel_r
xd_r=data.linear.x
yd_r=data.linear.y
vel_r=ma.sqrt(xd_r**2 + yd_r**2)
def ballcallback(msg):
global x_b,y_b
x_b=msg.position.x