import Helper as h
if __name__ == '__main__':
try:
rospy.init_node("Controller")
rospy.sleep(0.5)
r0 = RobotServices(0)
print "Controller setup done"
a = 1.0
while True:
# Both robots go to their start points
p0 = Point()
p0.x = -a
p0.y = 0.0
r0.go_to_point(p0)
rospy.sleep(0.5)
while not r0.is_ready().robot_ready:
rospy.sleep(1.0)
print "+++++++++++++++++++++++"
print "Reached first point!"
print "+++++++++++++++++++++++"
# Both robots aim at the other side of the intersections
p1 = Point()
p1.x = -a
p1.y = a
r0.go_to_point(p1)
rospy.sleep(0.5)
while not r0.is_ready().robot_ready:
pdist2 = 1.0
print "Controller setup done"
r0.set_speed(0.3)
r1.set_speed(0.3)
rospy.sleep(1.0)
r0.stop()
r1.stop()
while True:
# Both robots go to their start points
p0 = Point()
p0.x = -pdist1
p0.y = 0.0
r0.go_to_point(p0)
p1 = Point()
p1.x = 0.0
p1.y = -pdist1
r1.go_to_point(p1)
rospy.sleep(0.5)
h.wait_til_both_ready(r0, r1)
print "Both robots are now at starting points"
# Both robots aim at the other side of the intersections
p0 = Point()
p0.x = pdist2
p0.y = 0.0
r0.aim_at_point(p0)
p1 = Point()
p1.x = 0.0