#print "time to cross [0,0] for robot0 is: "+str(t)
#print "distance to intersection for robot0 is: "+str(r0dist2ip)
#print "velocity of robot0 is: "+str(r0state.twist.twist.linear.x)
v = r1state.twist.twist.linear.x
a = 2 * (r1dist2ip - v * t) / t**2
vfinal = v + a * t
a = opt_acc(vfinal, a, displacement, r1state, v, t, IP_y)
#print "time to cross [0,"+str(displacement)+"] for robot1 is: "+str(t)
#print "distance to initial intersection for robot1 is: "+str(r1dist2ip)
#print "velocity for robot1 is: "+str(v)
#print "new calculated acceleration value is: "+str(a)
if a < a_old:
resp = r1.set_acc(a)
a_old = a
#print "new minimzed acceleration solution found, acceleration for robot1 is now: "+str(a)
#print "--------------------------------------"
else:
resp = r1.set_acc(a_old)
#print "acceleration for robot1 is: "+str(a_old)
#print "--------------------------------------"
rospy.sleep(0.5)
#print "robot0 passed mid"
numberOfCrossings = numberOfCrossings + 1
#possible that r1 has greater final velocity than r0's constant velocity, if r0 doesn't accelerate fast enough
#after the intersection, it could result in collision because of the robots tail. Solution to give both same velocity
+ math.pow(
displacement -
r1.get_state().state.pose.pose.position.y, 2))
a = 2 * (r1dist2ip - v * t) / t**2
vfinal = v + a * t
print "robot1 passes first now!"
if vfinal >= 1.0:
a = -0.5
print "No optimized solution found, emergency break"
print "time to cross [0," + str(
displacement) + "] for robot1 is: " + str(t)
print "distance to intersection for robot1 is: " + str(r1dist2ip)
print "velocity for robot1 is: " + str(v)
print "acceleration for robot1 is: " + str(a)
print "--------------------------------------"
resp = r1.set_acc(a)
while r0.get_state().state.pose.pose.position.x < 0.0:
#ifi r1 has large displacement, keep it from going past the frame
if r1.get_state().state.pose.pose.position.y >= 0.7 * pdist2:
p1 = Point()
p1.x = 0.0
p1.y = pdist2
r1.go_to_point(p1)
rospy.sleep(0.5)
rospy.sleep(0.5)
print "robot0 passed mid"
numberOfCrossings = numberOfCrossings + 1
#possible that r1 has greater final velocity than r0's constant velocity, if r0 doesn't accelerate fast enough
solution_found, aim_behind)
#print "time to cross [0,"+str(displacement)+"] for robot1 is: "+str(t)
#print "distance to initial intersection for robot1 is: "+str(r1dist2ip)
print "Current position is for robot1 is: x=" + str(
r1state.pose.pose.position.x) + ", y=" + str(
r1state.pose.pose.position.y)
print "velocity for robot1 is: " + str(v)
if aim_behind:
print "Robot1 is aiming behind"
else:
print "Robot1 is aiming ahead"
if a < a_old and solution_found:
resp = r1.set_acc(a)
a_old = a
a_solution = a
print "new minimzed acceleration solution found, acceleration for robot1 is now: " + str(
a)
print "--------------------------------------"
elif solution_found:
resp = r1.set_acc(a_old)
print "acceleration for robot1 is: " + str(a_old)
print "--------------------------------------"
else:
resp = r1.set_acc(a_solution)
print "Emergency, no solution found, acceleration for robot1 is: " + str(
a_solution)
print "--------------------------------------"