state.pose.pose.position.y**2)
time.sleep(0.1)
r.stop()
permission.acquire()
r.steer_towards(p)
r.set_speed(0.2)
while not r.is_ready().robot_ready:
time.sleep(0.1)
if __name__ == '__main__':
try:
rospy.init_node("Controller")
rospy.sleep(0.5)
r0 = RobotServices(0)
r1 = RobotServices(1)
startTime = rospy.get_time()
print "Controller setup done"
a = 5.0
global asking_range
asking_range = 2.5
global permission
permission = threading.Semaphore()
while True:
run_controller(r0, r1)
tmp_r = r0
r0 = r1
r1 = tmp_r
from geometry_msgs.msg import Point, Pose, PoseStamped
from nav_msgs.msg import Path
from controller.srv import *
import sys
import math
#import scipy as sp
import rospy
from Helper import RobotServices
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!"