def main():
# Capture user input
my_sargv = rospy.myargv(argv=sargv)
robot_ids = []
start_positions = []
goal_positions = []
for arg in my_sargv[1:]:
robot_id, xi, yi, xf, yf = arg.split(",")
robot_ids.append(int(robot_id))
start_positions.append(Vector2(x=float(xi), y=float(yi)))
goal_positions.append(Vector2(x=float(xf), y=float(yf)))
# Create the orchestrator
orch = ProgramOrchestrator(robot_ids)
# Init ROS elements
rospy.init_node(ROS_NODE_NAME)
step_pub = rospy.Publisher("/terple/decentralized/step",
Empty,
queue_size=1)
combined_paths_pub = rospy.Publisher(
"/terple/decentralized/combined_paths",
DecentralizedCombinedPaths,
queue_size=1)
characteristics_updates_sub = rospy.Subscriber(
"/terple/decentralized/robot_status",
DecentralizedRobotStatus,
lambda m: update_callback(m, orch, step_pub, combined_paths_pub),
queue_size=100)
programs_pub = rospy.Publisher("/terple/decentralized/programs",
DecentralizedProgram,
queue_size=1)
# Publish the program and wait for updates
rospy.sleep(0.5)
programs_pub.publish(
DecentralizedProgram(robot_ids=robot_ids,
start_positions=start_positions,
goal_positions=goal_positions))
rospy.loginfo("Published program.")
rospy.spin()
def point_closest_to(self, vec):
x0, y0, c, d = vec.x, vec.y, -self.a, -self.b
xf = None
yf = None
if self.is_vertical():
yf = y0
xf = self.b
else:
denom = c**2 + 1
xf = (x0 - (c*y0) - (c*d)) / denom
yf = (c * (-x0 + (c*y0)) - d) / denom
vf = Vector2(x=xf,y=yf)
return vf, vec2_translate(vf, -x0, -y0)
def eval_at(self, x):
return Vector2(x=x,y=(self.a*x)+self.b)
def vec2_div(v, c):
return Vector2(
x=v.x/c,
y=v.y/c
)
def vec2_translate(v, dx, dy):
return Vector2(
x=v.x+dx,
y=v.y+dy
)
def calculate_preferred_velocity(self, dt):
v_pref = vec2_div(vec2_diff(self.objective, self.own_ext_char.position), dt)
if vec2_mag(v_pref) > self.max_vel:
angle = atan2(v_pref.y, v_pref.x)
v_pref = Vector2(x=self.max_vel*cos(angle),y=self.max_vel*sin(angle))
return v_pref
#!/usr/bin/env python
import rospy
from terple_msgs.msg import DecentralizedProgram, RobotExternalCharacteristics, DecentralizedRobotReadings, DecentralizedRobotStatus, Vector2
from std_msgs.msg import Empty
from math import sqrt, sin, cos, atan2
from sys import argv as sargv
import numpy as np
ROS_NODE_NAME = "terple_decentralized_terplebot"
ORIGIN = Vector2(x=0,y=0)
# Helper function to safely get a rosparam
def get_rosparam(name):
value = None
if rospy.has_param(name):
value = rospy.get_param(name)
return value
# Helper function to print carriage return
def printr(text):
stdout.write("\r" + text)
stdout.flush()
# Helper function to get the x-y coords of a terple_msgs.Vector2 as a str
def vec2_str(v):
return "({0},{1})".format(v.x,v.y)
# Helper function to get the tranalte a terple_msgs.Vector2 element by (dx,dy)
def vec2_translate(v, dx, dy):
def main(vmax, tau, rA, pA, vA, other_terples):
# Constant colors
COLORA = (0, 0, 255)
COLORB = (255, 0, 0)
# Calculate once and done
rA_scaled = scale_radius(rA)
vel_far_x = MY_CV_VEL_SPACE * 4
# Draw the position-space image
img_pos = white_image_with_axes((MY_CV_SIZE, MY_CV_SIZE, 3))
img_pos = cv2.circle(img_pos, scale_pos(pA), rA_scaled, COLORA, 2)
for rb in other_terples:
img_pos = cv2.circle(img_pos, scale_pos(Vector2(x=rb[1], y=rb[2])),
scale_radius(rb[0]), COLORB, 2)
imgs_voab = []
for other_terple in other_terples:
# Get the first robot in the list to be robot B
rB = other_terple[0]
pB = Vector2(x=other_terple[1], y=other_terple[2])
vB = Vector2(x=other_terple[3], y=other_terple[4])
# Create the test velocity obstacle
voab = DecentralizedRobotPlanner.VelocityObstacleModel(
rA, rB, pA, pB, tau)
# Do misc calculations once and done
vo_diff = DecentralizedRobotPlanner.vec2_diff(vA, vB)
r_circ = int(voab.circle_trunc.r * MY_CV_VEL_SCALE)
# Calculate points (interpolated, for the case of the legs) to test the calculated lines
right_x = voab.points[1].x
line_leg1, line_leg2, line_cross = voab.lines
line_pts = [
scale_vel(l.eval_at(x)) for x, l in
zip([-vel_far_x, -vel_far_x, vel_far_x, vel_far_x, right_x],
[line_leg1, line_leg2, line_leg1, line_leg2, line_cross])
]
# Draw the velocity-space image
img_voab = white_image_with_axes((MY_CV_SIZE, MY_CV_SIZE, 3))
# Shade in the velocity obstacle
for j in range(MY_CV_SIZE):
for i in range(MY_CV_SIZE):
if voab.contains(inv_scale_vel(i, j)):
img_voab[j, i] = (192, 192, 192)
# Draw the robots' velocities
img_voab = cv2.circle(img_voab, scale_vel(vA), rA_scaled, COLORA, 2)
img_voab = cv2.circle(img_voab, scale_vel(vB), scale_radius(rB),
COLORB, 2)
# Draw the circle that truncates the cone
img_voab = cv2.line(img_voab, line_pts[0], line_pts[2], (0, 255, 0), 2)
img_voab = cv2.line(img_voab, line_pts[1], line_pts[3], (0, 255, 0), 2)
img_voab = cv2.line(img_voab, scale_vel(voab.points[0]), line_pts[4],
(0, 255, 0), 2)
img_voab = cv2.circle(img_voab, scale_vel(voab.circle_trunc.position),
5, (128, 0, 128), -1)
img_voab = cv2.circle(img_voab, scale_vel(voab.circle_trunc.position),
r_circ, (128, 0, 128), 1)
img_voab = cv2.circle(img_voab, scale_vel(voab.points[0]), 5,
(255, 255, 0), -1)
img_voab = cv2.circle(img_voab, scale_vel(voab.points[1]), 5,
(0, 165, 255), -1)
# Draw a velocity vA-vB in velocity-space and the closest boundary point if it's in the velocity obstacle
vo_diff_color = None
closest_to_boundary = None
if voab.contains(vo_diff):
vo_diff_color = (0, 0, 192)
vec_u, closest_point, closest_dist = voab.boundary_point_closest_to(
vo_diff)
img_voab = cv2.line(img_voab, scale_vel(vo_diff),
scale_vel(closest_point), (193, 140, 255), 2)
img_voab = cv2.circle(img_voab, scale_vel(closest_point), 5,
(193, 140, 255), -1)
else:
vo_diff_color = (0, 128, 0)
img_voab = cv2.circle(img_voab, scale_vel(vo_diff), 5, vo_diff_color,
-1)
# Finished
imgs_voab.append(img_voab)
# Create a Terplebot
terpleA = DecentralizedRobotPlanner.Terplebot(1, rA, vmax)
terpleA.own_ext_char.position = pA
terpleA.own_ext_char.velocity = vA
terpleA.sensor_readings = DecentralizedRobotReadings(data=[
RobotExternalCharacteristics(robot_id=i + 2,
position=Vector2(x=terp[1], y=terp[2]),
velocity=Vector2(x=terp[3], y=terp[4]),
radius=terp[0])
for i, terp in enumerate(other_terples)
])
terpleA.initialized = True
# Build ORCA for robot A
orcaA = DecentralizedRobotPlanner.build_ORCA_A_tau_for(terpleA, tau)
# Draw the ORCA image for robot A
img_orca = white_image_with_axes((MY_CV_SIZE, MY_CV_SIZE, 3))
# Shade in the acceptable velocities
for j in range(MY_CV_SIZE):
for i in range(MY_CV_SIZE):
if orcaA.contains(inv_scale_vel(i, j)):
img_orca[j, i] = (192, 192, 192)
# Draw the half-plane equations
for leq, toggle in orcaA.orca_tau_AXs:
img_orca = cv2.line(img_orca, scale_vel(leq.eval_at(-vel_far_x)),
scale_vel(leq.eval_at(vel_far_x)),
np.random.randint(255, size=3), 2)
# Draw the optimal velocity of robot A
vA_opt = DecentralizedRobotPlanner.get_opt_vel(vA)
img_orca = cv2.circle(img_orca, scale_vel(vA_opt), 10, (0, 0, 255), -1)
# Draw the new velocity of robot A
v_new_valid, v_new = orcaA.calculate_next_velocity(vA_opt)
if v_new_valid:
img_orca = cv2.circle(img_orca, scale_vel(v_new), 6, (0, 255, 0), -1)
cv2.imshow("Position Space", img_pos)
for i in range(len(imgs_voab)):
cv2.imshow("Velocity Space (B{0})".format(i), imgs_voab[i])
cv2.imshow("ORCAA", img_orca)
cv2.waitKey(0)
cv2.destroyAllWindows()
def inv_scale(nx, ny, s):
vx = (nx - MY_CV_HALF_SIZE) / s
vy = (MY_CV_HALF_SIZE - ny) / s
return Vector2(x=vx, y=vy)
img_orca = cv2.line(img_orca, scale_vel(leq.eval_at(-vel_far_x)),
scale_vel(leq.eval_at(vel_far_x)),
np.random.randint(255, size=3), 2)
# Draw the optimal velocity of robot A
vA_opt = DecentralizedRobotPlanner.get_opt_vel(vA)
img_orca = cv2.circle(img_orca, scale_vel(vA_opt), 10, (0, 0, 255), -1)
# Draw the new velocity of robot A
v_new_valid, v_new = orcaA.calculate_next_velocity(vA_opt)
if v_new_valid:
img_orca = cv2.circle(img_orca, scale_vel(v_new), 6, (0, 255, 0), -1)
cv2.imshow("Position Space", img_pos)
for i in range(len(imgs_voab)):
cv2.imshow("Velocity Space (B{0})".format(i), imgs_voab[i])
cv2.imshow("ORCAA", img_orca)
cv2.waitKey(0)
cv2.destroyAllWindows()
if __name__ == "__main__":
vmax = float(sargv[1])
tau = float(sargv[2])
rA, pAx, pAy, vAx, vAy = tuple(map(float, sargv[3].split(",")))
other_terples = [tuple(map(float, arg.split(","))) for arg in sargv[4:]]
print("A: {0},{1},{2},{3},{4}".format(rA, pAx, pAy, vAx, vAy))
print("The rest: {0}".format(other_terples))
main(tau, vmax, rA, Vector2(x=pAx, y=pAy), Vector2(x=vAx, y=vAy),
other_terples)