def sendWPList(self, rid, N=None, s=None):
if N:
if s:
print "sending %d wps from ix %d of robot " % (N, s), rid
else:
print "sending %d wps of robot " % (N), rid
else:
print "sending wps of robot ", rid
msg = waypoint_list()
msg.timestamp = self.t_shift
msg.robotid = rid
if s:
nl = s
else:
nl = 0
if N:
nu = nl + N
else:
nu = len(self.wps[self.robot_to_ix[rid]])
msg.n = len(self.wps[self.robot_to_ix[rid]][s:nu])
wps = []
for (t, x, y) in self.wps[self.robot_to_ix[rid]][s:nu]:
wp = waypoint()
""" in milliseconds!! """
wp.timestamp = (t + self.t_shift) * 1e3
""" in mm """
wp.position = [(x + self.x_shift) * 1000.0,
(y + self.y_shift) * 1000.0, -1]
wp.orientation = [0, 0, 0, 1]
wps.append(wp)
msg.waypoints = wps
self.lc.publish("TARGET", msg.encode())
print "sent ", (nu - nl), " waypoints"
def sendWPList(self,rid, N=None, s=None):
if N:
if s:
print "sending %d wps from ix %d of robot "%(N,s), rid
else:
print "sending %d wps of robot "%(N), rid
else:
print "sending wps of robot ", rid
msg = waypoint_list()
msg.timestamp = self.t_shift
msg.robotid = rid
if s:
nl = s
else:
nl = 0
if N:
nu = nl+N
else:
nu = len(self.wps[self.robot_to_ix[rid]])
msg.n = len(self.wps[ self.robot_to_ix[rid]][s:nu])
wps=[]
for (t,x,y) in self.wps[self.robot_to_ix[rid]][s:nu]:
wp = waypoint()
""" in milliseconds """
wp.timestamp = (t+self.t_shift)*1e3
""" in mm """
wp.position = [(x+self.x_shift)*1000.0,(y+self.y_shift)*1000.0,-1]
wp.orientation = [0,0,0,1]
wps.append(wp)
msg.waypoints = wps
self.lc.publish("TARGET", msg.encode())
print "sent ", (nu-nl), " waypoints"
def move_letter(dx, dy):
global last_sent
now_sent = {}
for (rid, (x, y)) in last_sent.items():
nx = x + dx
ny = y + dy
msg = waypoint_list()
msg.timestamp = int(getTime() * 1e6)
msg.robotid = rid
msg.n = 1
wps = []
wp = waypoint()
wp.timestamp = 0
wp.position = [1000 * nx, 1000 * ny, 0]
wp.orientation = [0, 0, 0, 1]
wps = [wp]
msg.waypoints = wps
print "sending ", rid, "to", nx, ny
now_sent[rid] = (nx, ny)
lcm.publish("TARGET", msg.encode())
last_sent = now_sent
def move_letter(dx,dy):
global last_sent
now_sent = {}
for (rid, (x,y)) in last_sent.items():
nx = x+dx
ny = y+dy
msg = waypoint_list()
msg.timestamp = int(getTime() * 1e6)
msg.robotid = rid
msg.n = 1
wps=[]
wp = waypoint()
wp.timestamp = 0
wp.position = [1000*nx,1000*ny,0]
wp.orientation = [0,0,0,1]
wps=[wp]
msg.waypoints = wps
print "sending ",rid,"to",nx,ny
now_sent[rid]=(nx,ny)
lcm.publish("TARGET", msg.encode())
last_sent = now_sent
def send_robots(ids, coordinates, ori, channel, timestamp):
for i in range(len(ids)):
robotid = ids[i]
x, y = coordinates[i]
""" --- no z --- """
o = ori[i]
channel = options.channel
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=1")
msg = waypoint_list()
msg.timestamp = float(timestamp) * 1e3
msg.robotid = robotid
msg.n = 1
wp = waypoint()
wp.timestamp = float(timestamp) * 1e3
""" in mm """
wp.position = [x * 1000, y * 1000, -1]
wp.orientation = [o, 0, 0, 0]
wps = [wp]
msg.waypoints = wps
lc.publish(channel, msg.encode())
def send_wps(wps):
global last_sent
""" assign relay to positions, if any"""
robot_to_loc = {}
loc_to_robot = {}
""" locs: loc_ix -> (x,y) """
locs = {}
dummy = []
""" HACK ALERT!: add dummy locs """
dummy.append((-2, -2))
dummy.append((-2, 6))
# dummy.append(( 5,-2))
# dummy.append(( 5, 6))
dummy.append((-2.5, -2))
dummy.append((-2.5, 6))
# dummy.append(( 6,-2))
# dummy.append(( 6, 6))
dummy.append((-3, -2))
dummy.append((-3, 6))
if len(wps) > len(robots):
print "not enough robots: need ", len(wps), "have", len(robots)
return
if len(wps) < len(robots):
j = 0
while len(wps) < len(robots):
wps.append(dummy[j])
j += 1
for i in range(len(wps)):
""" candidate loc index """
rpos = wps[i]
(rpos_x, rpos_y) = rpos
locs[i] = (rpos_x * SCALE, rpos_y * SCALE)
""" rns: node_id -> (x,y) """
rns = {}
for rix in robots:
nx, ny = nodes[rix].x, nodes[rix].y
rns[rix] = (nx, ny)
if USE_LAP:
loc_to_robot, robot_to_loc = LAP(rns, locs)
else:
""" greedy assignment """
loc_to_robot, robot_to_loc = greedy_AP(rns, locs)
print "assignemnt loc_to_robot: ", loc_to_robot
print "assignemnt robot to loc ", robot_to_loc
last_sent = {}
for (rid, loc) in robot_to_loc.items():
(x, y) = locs[loc]
msg = waypoint_list()
msg.timestamp = int(getTime() * 1e6)
msg.robotid = rid
msg.n = 1
wps = []
wp = waypoint()
wp.timestamp = 0
wp.position = [1000 * x, 1000 * y, 0]
wp.orientation = [0, 0, 0, 1]
wps = [wp]
msg.waypoints = wps
print "sending ", rid, "to", x, y
""" if is not dummy (NOTE: dummy assumed to be negative x)"""
if x > -0.5:
last_sent[rid] = (x, y)
lcm.publish("TARGET", msg.encode())
def send_wps(wps):
global last_sent
""" assign relay to positions, if any"""
robot_to_loc = {}
loc_to_robot={}
""" locs: loc_ix -> (x,y) """
locs={}
dummy=[]
""" HACK ALERT!: add dummy locs """
dummy.append((-2,-2))
dummy.append((-2, 6))
# dummy.append(( 5,-2))
# dummy.append(( 5, 6))
dummy.append((-2.5,-2))
dummy.append((-2.5, 6))
# dummy.append(( 6,-2))
# dummy.append(( 6, 6))
dummy.append((-3,-2))
dummy.append((-3, 6))
if len(wps) > len(robots):
print "not enough robots: need ",len(wps),"have",len(robots)
return
if len(wps) < len(robots):
j=0
while len(wps) < len(robots):
wps.append( dummy[j])
j+=1
for i in range(len(wps)):
""" candidate loc index """
rpos = wps[i]
(rpos_x,rpos_y)=rpos
locs[i]=(rpos_x*SCALE, rpos_y*SCALE)
""" rns: node_id -> (x,y) """
rns={}
for rix in robots:
nx,ny = nodes[rix].x, nodes[rix].y
rns[rix]=(nx,ny)
if USE_LAP:
loc_to_robot,robot_to_loc = LAP(rns,locs)
else:
""" greedy assignment """
loc_to_robot,robot_to_loc = greedy_AP(rns,locs)
print "assignemnt loc_to_robot: ", loc_to_robot
print "assignemnt robot to loc ", robot_to_loc
last_sent = {}
for (rid, loc) in robot_to_loc.items():
(x,y) = locs[loc]
msg = waypoint_list()
msg.timestamp = int(getTime() * 1e6)
msg.robotid = rid
msg.n = 1
wps=[]
wp = waypoint()
wp.timestamp = 0
wp.position = [1000*x,1000*y,0]
wp.orientation = [0,0,0,1]
wps=[wp]
msg.waypoints = wps
print "sending ",rid,"to",x,y
""" if is not dummy (NOTE: dummy assumed to be negative x)"""
if x>-0.5:
last_sent[rid]=(x,y)
lcm.publish("TARGET", msg.encode())
(options, args) = parser.parse_args()
if len(args) != 4:
print "mandatory arguments missing (",len(args),")"
parser.print_help()
exit(-1)
robotid = int(args[0])
x = float(args[1])
y = float(args[2])
""" --- no z --- """
o = float(args[3])
channel = options.channel
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=1")
msg = waypoint_list()
msg.timestamp = float(options.timestamp)*1e3
msg.robotid = robotid
msg.n = 1
wp = waypoint()
""" in milliseconds """
print options.timestamp
wp.timestamp =float(options.timestamp)*1e3
""" in mm """
wp.position = [x,y,-1]
wp.orientation = [o,0,0,0]
wps=[wp]
msg.waypoints = wps
lc.publish(options.channel, msg.encode())
(options, args) = parser.parse_args()
if len(args) != 4:
print "mandatory arguments missing (", len(args), ")"
parser.print_help()
exit(-1)
robotid = int(args[0])
x = float(args[1])
y = float(args[2])
""" --- no z --- """
o = float(args[3])
channel = options.channel
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=1")
msg = waypoint_list()
msg.timestamp = float(options.timestamp) * 1e3
msg.robotid = robotid
msg.n = 1
wp = waypoint()
""" in milliseconds """
print options.timestamp
wp.timestamp = float(options.timestamp) * 1e3
""" in mm """
wp.position = [x, y, -1]
wp.orientation = [o, 0, 0, 0]
wps = [wp]
msg.waypoints = wps
lc.publish(options.channel, msg.encode())
