def talker():
pub = rospy.Publisher('/trajectory', Pose2D_Array, queue_size=10)
rospy.init_node('talker', anonymous=True)
rospy.Subscriber("/y_r0", Pose2D, robot_position)
rospy.Subscriber("/ball", Pose2D, destination_position)
_id = "/b_r0"
rospy.Subscriber(_id, Pose2D, obstacle_position, (_id))
rate = rospy.Rate(1) # 10hz
aux = 1
while not rospy.is_shutdown():
# arr = Pose2D_Array()
# for i in range(10):
# pose = init_pose()
# pose.x = 100 * ( i + 1 )
# pose.y = 150 * ( i + 1 ) * aux
# pose.theta +=0.7853 * i
# arr.poses.append(pose)
# aux *= -1
# print "The array is:", arr
arr = Pose2D_Array()
arr.poses.append(pos)
arr.poses.append(des)
pub.publish(arr)
# hello_str = "hello world %s" % rospy.get_time()
# rospy.loginfo(hello_str)
# pub.publish(hello_str)
rate.sleep()
def showTraj():
arr = Pose2D_Array()
arr.poses.append(p)
t = Pose2D()
t.x = p.x + cos(switchCoord(p.theta)) * radius # float(input("tr x: "))
t.y = p.y + sin(switchCoord(p.theta)) * radius # float(input("tr y: "))
t.theta = p.theta
arr.poses.append(t)
arr.poses.append(p)
t = Pose2D()
t.x = p.x + cos(switchCoord(p.theta) +
half_arc) * radius # float(input("tr x: "))
t.y = p.y + sin(switchCoord(p.theta) +
half_arc) * radius # float(input("tr y: "))
t.theta = p.theta
arr.poses.append(t)
arr.poses.append(p)
t = Pose2D()
t.x = p.x + cos(switchCoord(p.theta) -
half_arc) * radius # float(input("tr x: "))
t.y = p.y + sin(switchCoord(p.theta) -
half_arc) * radius # float(input("tr y: "))
t.theta = p.theta
arr.poses.append(t)
traj.publish(arr)
def talker():
global Pose2D
rospy.init_node('talker', anonymous = True)
pub = rospy.Publisher('/trayectoria1', Pose2D_Array, queu_size = 6)
rospy.Subscriber("/y_r0", Pose2D, miPos)
rospy.Subscriber("/ball", Pose2D, miPos)
while not rospy.is_shutdown():
arr = Pose2D_Array()
for i in range(6):
pose = init_pose()
pose.x = 100 * (i + 1)
pose.y = 100 * (i + 1) * aux
pose.theta += 0.785 *i
aux *= -1
print "the array is: ", arr
pub.publish(arr)
rate.sleep()
def publish(final_path):
global pub
global path
arr = Pose2D_Array()
for p in final_path:
pose = Pose2D()
pose.x = p[0]
pose.y = p[1]
pose.theta = p[2]
arr.poses.append(pose)
path = arr
pub.publish(arr)
print 'Trajectory published'
def talker():
pub = rospy.Publisher('/trajectory', Pose2D_Array, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(1) # 10hz
aux = 1
while not rospy.is_shutdown():
arr = Pose2D_Array()
for i in range(10):
pose = init_pose()
pose.x = 100 * (i + 1)
pose.y = 150 * (i + 1) * aux
pose.theta += 0.7853 * i
arr.poses.append(pose)
aux *= -1
print "The array is:", arr
pub.publish(arr)
# hello_str = "hello world %s" % rospy.get_time()
# rospy.loginfo(hello_str)
# pub.publish(hello_str)
rate.sleep()
def talker():
pub = rospy.Publisher('/trajectory', Pose2D_Array, queue_size=10)
pc = rospy.Publisher('pcPub', msg, queue_size=10)
rospy.init_node('pc', anonymous=True)
rospy.Subscriber("/b_r0", Pose2D, callbackBall)
rospy.Subscriber("/b_r1", Pose2D, callbackObs0)
rospy.Subscriber("/b_r2", Pose2D, callbackObs1)
rospy.Subscriber("/b_r3", Pose2D, callbackObs2)
rospy.Subscriber("/b_r4", Pose2D, callbackObs3)
rospy.Subscriber("/b_r5", Pose2D, callbackObs4)
rospy.Subscriber("/b_r6", Pose2D, callbackObs5)
rospy.Subscriber("/b_r7", Pose2D, callbackObs6)
rospy.Subscriber("/ball", Pose2D, callbackObs7)
rospy.Subscriber("/y_r0", Pose2D, callbackChancla)
rospy.Subscriber("ardu", msg, callbackArdu)
rate = rospy.Rate(1) # 10hz
aux = 1
while not rospy.is_shutdown():
arr = Pose2D_Array()
for i in range(10):
pose = init_pose()
pose.x = 100 * ( i + 1 )
pose.y = 150 * ( i + 1 ) * aux
pose.theta +=0.7853 * i
arr.poses.append(pose)
aux *= -1
print "The array is:", arr
pub.publish(arr)
#pub.publish(callback)
# hello_str = "hello world %s" % rospy.get_time()
# rospy.loginfo(hello_str)
# pub.publish(hello_str)
rate.sleep()
rospy.spin()
def talker():
global obcount
rospy.Subscriber("y_r0", Pose2D, updatePlayer)
rospy.Subscriber("ball", Pose2D, updateBall)
traj = rospy.Publisher('/trajectory', Pose2D_Array, queue_size=10)
ball = rospy.Publisher('/ball', Pose2D, queue_size=10)
pl = rospy.Publisher('/y_r0', Pose2D, queue_size=10)
obs = []
for i in range(11):
obs.append(rospy.Publisher('/b_r{}'.format(i), Pose2D, queue_size=30))
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(1) # 10hz
aux = 1
while not rospy.is_shutdown():
i = raw_input("choose p or b or t or o: ")
if i == "p":
p.x = float(input("pl x: "))
p.y = float(input("pl y: "))
p.theta = float(input('pl t: '))
pl.publish(p)
arr = Pose2D_Array()
arr.poses.append(p)
t = Pose2D()
t.x = p.x+cos(showT(p.theta))*250 # float(input("tr x: "))
t.y = p.y+sin(showT(p.theta))*250 # float(input("tr y: "))
t.theta = p.theta
arr.poses.append(t)
# print "The array is:", arr
traj.publish(arr)
elif i == 'b':
b.x = float(input("ball x: "))
b.y = float(input("ball y: "))
ball.publish(b)
elif i == 't':
p.theta += float(input("dt: "))*2*pi/360 # deg to rad
pl.publish(p)
elif i == 'o':
x = random()*2000-1000
y = random()*2000-1000
obs[obcount].publish(Pose2D(x, y, 0))
obs[obcount].publish(Pose2D(x, y, 0))
obs[obcount].publish(Pose2D(x, y, 0))
obcount+=1
obcount%=5
elif i == 'br':
b = Pose2D(random()*2000-1000, random()*2000-1000, 0)
ball.publish(b)
elif i == 'pr':
p = Pose2D(random()*2000-1000, random()*2000-1000, random()*2*pi)
pl.publish(p)
elif i == 'c':
for o in obs:
o.publish(Pose2D(10000,10000,0))
o.publish(Pose2D(10000,10000,0))
o.publish(Pose2D(10000,10000,0))
#arr = Pose2D_Array()
# arr.poses.append(p)
#t = Pose2D()
# t.x = p.x+cos(showT(p.theta))*250 # float(input("tr x: "))
# t.y = p.y+sin(showT(p.theta))*250 # float(input("tr y: "))
#t.theta = p.theta
# arr.poses.append(t)
# traj.publish(arr)
'''
def planner():
pub = rospy.Publisher('/trajectory', Pose2D_Array, queue_size=10)
rospy.Subscriber('/y_r0', Pose2D, callbackSelf)
rospy.Subscriber('/ball', Pose2D, callbackTarget)
rospy.Subscriber('/b_r0', Pose2D, callbackObs0)
rospy.Subscriber('/b_r2', Pose2D, callbackObs2)
rospy.Subscriber('/b_r1', Pose2D, callbackObs1)
rospy.Subscriber('/b_r3', Pose2D, callbackObs3)
rospy.Subscriber('/b_r4', Pose2D, callbackObs4)
rospy.Subscriber('/b_r5', Pose2D, callbackObs5)
rospy.Subscriber('/b_r6', Pose2D, callbackObs6)
rospy.Subscriber('/b_r7', Pose2D, callbackObs7)
rospy.init_node('planner')
rate = rospy.Rate(2)
# time.sleep(1)
while not rospy.is_shutdown():
pointSelf= Point(int(poseSelf.x),int(poseSelf.y))
pointTarget= Point(int(poseTarget.x),int(poseTarget.y))
arr = Pose2D_Array() #contiene la salida del nodo
arr.poses.append(poseSelf) #el primer punto siempre es donde se encuentra el robot actualmente
obstacles= calcCircs()
trajectory = Segment(pointSelf,pointTarget)
intersections= calcIntersect(obstacles, trajectory)
numIntersect= intDiscriminator(intersections)
if numIntersect==0:
arr.poses.append(poseTarget)
else:
while not numIntersect==0 :
newP=[]
index=[]
if len(intersections[0])==2 :
index.append(0)
newP.append(recalc(intersections[0],Point(poseObs0.x,poseObs0.y), obstacles[0]))
if len(intersections[1])==2 :
index.append(1)
newP.append(recalc(intersections[1],Point(poseObs1.x,poseObs1.y), obstacles[1]))
if len(intersections[2])==2 :
index.append(2)
newP.append(recalc(intersections[2],Point(poseObs2.x,poseObs2.y), obstacles[2]))
if len(intersections[3])==2 :
index.append(3)
newP.append(recalc(intersections[3],Point(poseObs3.x,poseObs3.y), obstacles[3]))
if len(intersections[4])==2 :
index.append(4)
newP.append(recalc(intersections[4],Point(poseObs4.x,poseObs4.y), obstacles[4]))
if len(intersections[5])==2 :
index.append(5)
newP.append(recalc(intersections[5],Point(poseObs5.x,poseObs5.y), obstacles[5]))
if len(intersections[6])==2 :
index.append(6)
newP.append(recalc(intersections[6],Point(poseObs6.x,poseObs6.y), obstacles[6]))
if len(intersections[7])==2 :
index.append(7)
newP.append(recalc(intersections[7],Point(poseObs7.x,poseObs7.y), obstacles[7]))
print "New points calced: ", newP
puntosOrdenados= arrange(pointSelf, newP)
ntraj=[]
ntraj.append(pointSelf)
for x in range(len(puntosOrdenados)):
poseAdd= init_pose()
laux=newP[puntosOrdenados[x]]
poseAdd.x=int(laux[0].x)
poseAdd.y= int(laux[0].y)
poseAdd.theta= 0
ntraj.append(laux[0])
arr.poses.append(poseAdd)
ntraj.append(pointTarget)
arr.poses.append(poseTarget)
# nintersect=[]
# for x in xrange(len(puntosOrdenados)):
# seg= Segment(ntraj[x],ntraj[x+1])
# nintersect= calcIntersect(obstacles, seg)
# numIntersect= intDiscriminator(nintersect)
numIntersect=0
print "The array is:", arr
pub.publish(arr)
# while True:
# b=1
rate.sleep()