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!"
from geometry_msgs.msg import Point, Pose, PoseStamped
from nav_msgs.msg import Path
from controller.srv import *
import sys
import math
import rospy
from Helper import RobotServices
import Helper as h
import random
if __name__ == '__main__':
try:
rospy.init_node("Controller")
rospy.sleep(0.5)
r0 = RobotServices(0)
r1 = RobotServices(1)
numberOfCrossings = 0
'''
gdist1 = 1.0
gdist2 = 2.0
pdist1 = 5.0
pdist2 = 3.0
'''
gdist1 = 1.25
gdist2 = 4.0
pdist1 = 1.5
pdist2 = 1.0
print "Controller setup done"
if calculations_ext > 50:
#print "Reached maximum allowed calculations, returning emergency acceleration value"
#print "Number of calculations done to find satsifying acceleration is: "+str(calculations_ext)
return -0.5
#print "Number of calculations done to find satsifying acceleration is: "+str(calculations_ext+calculations_int1+calculations_int2)
#print "Returned acceleration is: "+str(a)
#print "Current position is: x="+str(r1state.pose.pose.position.x)+", y="+str(r1state.pose.pose.position.y)
return a
if __name__ == '__main__':
try:
rospy.init_node("Controller")
rospy.sleep(0.5)
r0 = RobotServices(0)
r1 = RobotServices(1)
rospy.wait_for_service("Robot" + str(0) + "/calibrate_angle")
r0calibrate = rospy.ServiceProxy("Robot" + str(0) + "/calibrate_angle",
Stop)
rospy.wait_for_service("Robot" + str(1) + "/calibrate_angle")
r1calibrate = rospy.ServiceProxy("Robot" + str(1) + "/calibrate_angle",
Stop)
numberOfCrossings = 0
a_old = 10000
gdist1 = 1.5
gdist2 = 2.0
pdist1 = 4.0
pdist2 = 1.0
calculations_ext)
solution_found = False
return 10000, solution_found, aim_behind
print "Number of calculations done to find satsifying acceleration is: " + str(
calculations_ext + calculations_int1 + calculations_int2)
print "Returned acceleration is: " + str(a)
solution_found = True
return a, solution_found, aim_behind
if __name__ == '__main__':
try:
rospy.init_node("Controller")
rospy.sleep(0.5)
r0 = RobotServices(0)
r1 = RobotServices(1)
numberOfCrossings = 0
gdist1 = 1.0
gdist2 = 2.0
pdist1 = 4.0
pdist2 = 1.0
print "Controller setup done"
#r0.set_speed(0.3)
#r1.set_speed(0.3)
#rospy.sleep(3.0)
#r0.stop()
#r1.stop()
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)
r1 = RobotServices(1)
print "Controller setup done"
while True:
# Both robots go to their start points
p0 = Point()
p0.x = -3.0
p0.y = 0.0
r0.go_to_point(p0)
p1 = Point()
p1.x = 0.0
p1.y = -3.0
r1.go_to_point(p1)
rospy.sleep(0.5)
h.wait_til_both_ready(r0, r1)