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())