r0.steer_towards(p0)
r0.set_speed(rand0)
r1.steer_towards(p1)
r1.set_speed(rand1)
if rand0 < rand1:
rospy.sleep(rand1 * 6.0)
else:
rospy.sleep(rand0 * 6.0)
print "Both robots are now at correct speeds"
print "speed set for robot0 is: " + str(rand0)
print "speed set for robot1 is: " + str(rand1)
#is it a good idea to store state in variable?
while r0.get_state(
).state.pose.pose.position.x < 0.0 and r1.get_state(
).state.pose.pose.position.y <= 0.8 * pdist2:
r0state = r0.get_state().state
r1state = r1.get_state().state
r0_angle = robot_angle(r0state)
r1_angle = robot_angle(r1state)
IP = intersection_point(r0state.pose.pose.position.x,
r0state.pose.pose.position.y, r0_angle,
r1state.pose.pose.position.x,
r1state.pose.pose.position.x, r1_angle)
IP_x = IP[0]
IP_y = IP[1]
#print "Intersection point is: x="+str(IP_x)+", y="+str(IP_y)
if rand0 <= 0.25 or rand0 >= 0.4 or rand1 <= 0.25 or rand1 >= 0.4:
rospy.sleep(1.5)
print "Both robots are now at correct speeds"
print "speed set for robot0 is: " + str(rand0)
print "speed set for robot1 is: " + str(rand1)
# Calculate how long it will take for r0 to reach ip
#dont think its a good idea to store state in variable
#r0state = r0.get_state().state
#r1state = r1.get_state().state
if rand0 <= 0.25 or rand0 >= 0.4 or rand1 <= 0.25 or rand1 >= 0.4:
displacement = -gdist2
else:
displacement = -gdist1
r0dist2ip = math.sqrt(
math.pow(r0.get_state().state.pose.pose.position.x, 2) +
math.pow(r0.get_state().state.pose.pose.position.y, 2))
r1dist2ip = math.sqrt(
math.pow(r1.get_state().state.pose.pose.position.x, 2) +
math.pow(
displacement -
r1.get_state().state.pose.pose.position.y, 2))
t = r0dist2ip / r0.get_state().state.twist.twist.linear.x
print "--------------------------------------"
print "number of crossings is: " + str(numberOfCrossings)
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(
r0.get_state().state.twist.twist.linear.x)
v = r1.get_state().state.twist.twist.linear.x
r0.steer_towards(p0)
r0.set_speed(rand0)
r1.steer_towards(p1)
r1.set_speed(rand1)
if rand0 < rand1:
rospy.sleep(2.0)
else:
rospy.sleep(2.0)
print "Both robots are now at correct speeds"
print "speed set for robot0 is: " + str(rand0)
print "speed set for robot1 is: " + str(rand1)
#is it a good idea to store state in variable?
while r0.get_state(
).state.pose.pose.position.x < 0.0 and r1.get_state(
).state.pose.pose.position.y <= 0.8 * pdist2:
r0state = r0.get_state().state
r1state = r1.get_state().state
r0_angle = robot_angle(r0state)
r1_angle = robot_angle(r1state)
IP = intersection_point(r0state.pose.pose.position.x,
r0state.pose.pose.position.y, r0_angle,
r1state.pose.pose.position.x,
r1state.pose.pose.position.x, r1_angle)
IP_x = IP[0]
IP_y = IP[1]
#print "Intersection point is: x="+str(IP_x)+", y="+str(IP_y)
displacement = -gdist1