def append_robot_name(self): self.robots = [] for i in range(self.n_robots): self.robots.append(robot('/robot_' + str(i))) for i in range(0, self.n_robots): self.robots[i].sendGoal(self.robots[i].getPosition())
def node(): global frontiers, mapData, global1, global2, global3, globalmaps rospy.init_node('assigner', anonymous=False) # fetching all parameters map_topic = rospy.get_param('~map_topic', '/map') info_radius = rospy.get_param( '~info_radius', 1.0 ) # this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate info_multiplier = rospy.get_param('~info_multiplier', 3.0) hysteresis_radius = rospy.get_param( '~hysteresis_radius', 3.0) # at least as much as the laser scanner range hysteresis_gain = rospy.get_param( '~hysteresis_gain', 2.0) # bigger than 1 (biase robot to continue exploring current region frontiers_topic = rospy.get_param('~frontiers_topic', '/filtered_points') n_robots = rospy.get_param('~n_robots', 1) namespace = rospy.get_param('~namespace', '') namespace_init_count = rospy.get_param('namespace_init_count', 1) delay_after_assignement = rospy.get_param('~delay_after_assignement', 0.5) rateHz = rospy.get_param('~rate', 100) rate = rospy.Rate(rateHz) # ------------------------------------------- rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) rospy.Subscriber(frontiers_topic, PointArray, callBack) # --------------------------------------------------------------------------------------------------------------- # wait if no frontier is received yet while len(frontiers) < 1: pass centroids = copy(frontiers) # wait if map is not received yet while (len(mapData.data) < 1): pass robots = [] if len(namespace) > 0: for i in range(0, n_robots): robots.append(robot(namespace + str(i + namespace_init_count))) elif len(namespace) == 0: robots.append(robot(namespace)) for i in range(0, n_robots): robots[i].sendGoal(robots[i].getPosition()) # ------------------------------------------------------------------------- # --------------------- Main Loop ------------------------------- # ------------------------------------------------------------------------- while not rospy.is_shutdown(): centroids = copy(frontiers) # ------------------------------------------------------------------------- # Get information gain for each frontier point infoGain = [] for ip in range(0, len(centroids)): infoGain.append( informationGain(mapData, [centroids[ip][0], centroids[ip][1]], info_radius)) # ------------------------------------------------------------------------- # get number of available/busy robots na = [] # available robots nb = [] # busy robots for i in range(0, n_robots): if (robots[i].getState() == 1): nb.append(i) else: na.append(i) rospy.loginfo("available robots: " + str(na)) # ------------------------------------------------------------------------- # get dicount and update informationGain for i in nb + na: infoGain = discount(mapData, robots[i].assigned_point, centroids, infoGain, info_radius) # ------------------------------------------------------------------------- revenue_record = [] centroid_record = [] id_record = [] for ir in na: for ip in range(0, len(centroids)): cost = norm(robots[ir].getPosition() - centroids[ip]) threshold = 1 information_gain = infoGain[ip] if (norm(robots[ir].getPosition() - centroids[ip]) <= hysteresis_radius): information_gain *= hysteresis_gain revenue = information_gain * info_multiplier - cost revenue_record.append(revenue) centroid_record.append(centroids[ip]) id_record.append(ir) if len(na) < 1: revenue_record = [] centroid_record = [] id_record = [] for ir in nb: for ip in range(0, len(centroids)): cost = norm(robots[ir].getPosition() - centroids[ip]) threshold = 1 information_gain = infoGain[ip] if (norm(robots[ir].getPosition() - centroids[ip]) <= hysteresis_radius): information_gain *= hysteresis_gain if ((norm(centroids[ip] - robots[ir].assigned_point)) < hysteresis_radius): information_gain = informationGain( mapData, [centroids[ip][0], centroids[ip][1]], info_radius) * hysteresis_gain revenue = information_gain * info_multiplier - cost revenue_record.append(revenue) centroid_record.append(centroids[ip]) id_record.append(ir) rospy.loginfo("revenue record: " + str(revenue_record)) rospy.loginfo("centroid record: " + str(centroid_record)) rospy.loginfo("robot IDs record: " + str(id_record)) # ------------------------------------------------------------------------- if (len(id_record) > 0): winner_id = revenue_record.index(max(revenue_record)) robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) rospy.loginfo(namespace + str(namespace_init_count + id_record[winner_id]) + " assigned to " + str(centroid_record[winner_id])) rospy.sleep(delay_after_assignement) # ------------------------------------------------------------------------- rate.sleep()
def node(): global start_time, start_date, finished, busy_robot_count, map_grid_count, frontiers_topic, simulation_summary_string global frontiers,mapData,global1,global2,global3,globalmaps # global frontiers_topic rospy.init_node('assigner', anonymous=False) # fetching all parameters map_topic= rospy.get_param('~map_topic','/map') info_radius= rospy.get_param('~info_radius',1.0) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate info_multiplier=rospy.get_param('~info_multiplier',3.0) hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0) #at least as much as the laser scanner range hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points') n_robots = rospy.get_param('~n_robots',1) namespace = rospy.get_param('~namespace','') namespace_init_count = rospy.get_param('~namespace_init_count',1) delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5) rateHz = rospy.get_param('~rate',100) task_reset_time_out_s = rospy.get_param('~task_reset_time_out', 4) map_grid_count=rospy.get_param('~map_grid_count',1) simulation_summary_string=rospy.get_param('~simulation_summary_string',"") rate = rospy.Rate(rateHz) #------------------------------------------- rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) rospy.Subscriber(frontiers_topic, PointArray, callBack) #--------------------------------------------------------------------------------------------------------------- # wait if no frontier is received yet while len(frontiers)<1: pass centroids=copy(frontiers) #wait if map is not received yet while (len(mapData.data)<1): pass robots=[] if len(namespace)>0: for i in range(0,n_robots): robots.append(robot(namespace+str(i+namespace_init_count))) elif len(namespace)==0: robots.append(robot(namespace)) for i in range(0,n_robots): robots[i].sendGoal(robots[i].getPosition(), None) #------------------------------------------------------------------------- #--------------------- Main Loop ------------------------------- #------------------------------------------------------------------------- start_time = rospy.get_time() start_date = datetime.datetime.now().strftime('%Y%m%d-%H%M%S') while not rospy.is_shutdown(): centroids=copy(frontiers) #------------------------------------------------------------------------- #Get information gain for each frontier point infoGain=[] for ip in range(0,len(centroids)): infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)) #------------------------------------------------------------------------- #get number of available/busy robots na=[] #available robots nb=[] #busy robots for i in range(0,n_robots): robots[i].cancelIfOverTime(rospy.get_time(), task_reset_time_out_s) if (robots[i].getState()==1): nb.append(i) else: na.append(i) rospy.loginfo("available robots: "+str(na)) busy_robot_count = len(nb) if len(centroids) == 0: for r in na: robots[i].sendGoal(array([0.0, 0.0]), rospy.get_time()) # print("BBBBBBBBBB ", len(frontiers), busy_robot_count) if not finished and len(frontiers) < 1 and busy_robot_count == 0: finish_time = rospy.get_time() # print("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA") print("Start time", start_time, "Finish time", finish_time) try: with open('/home/michael/ros_log/log.txt', 'a+') as f: f.write(simulation_summary_string + ",Topic," + frontiers_topic + ",Stamp," + start_date + ",Start_time,"+ str(start_time) + ",Finish_time," + str(finish_time) + ",Diff," + str(finish_time - start_time) + '\n') # print("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA") finished = True except Exception as e: print(e) #------------------------------------------------------------------------- #get dicount and update informationGain for i in nb+na: infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius) #------------------------------------------------------------------------- revenue_record=[] centroid_record=[] id_record=[] for ir in na: for ip in range(0,len(centroids)): cost=norm(robots[ir].getPosition()-centroids[ip]) threshold=1 information_gain=infoGain[ip] if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): information_gain*=hysteresis_gain revenue=information_gain*info_multiplier-cost revenue_record.append(revenue) centroid_record.append(centroids[ip]) id_record.append(ir) if len(na)<1: revenue_record=[] centroid_record=[] id_record=[] for ir in nb: for ip in range(0,len(centroids)): cost=norm(robots[ir].getPosition()-centroids[ip]) threshold=1 information_gain=infoGain[ip] if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): information_gain*=hysteresis_gain if ((norm(centroids[ip]-robots[ir].assigned_point))<hysteresis_radius): information_gain=informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)*hysteresis_gain revenue=information_gain*info_multiplier-cost revenue_record.append(revenue) centroid_record.append(centroids[ip]) id_record.append(ir) rospy.loginfo("revenue record: "+str(revenue_record)) rospy.loginfo("centroid record: "+str(centroid_record)) rospy.loginfo("robot IDs record: "+str(id_record)) #------------------------------------------------------------------------- if (len(id_record)>0): winner_id=revenue_record.index(max(revenue_record)) robots[id_record[winner_id]].sendGoal(centroid_record[winner_id], rospy.get_time()) rospy.loginfo(namespace+str(namespace_init_count+id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) rospy.sleep(delay_after_assignement) #------------------------------------------------------------------------- rate.sleep()
def node(): global frontiers, mapData, global1, global2, global3, globalmaps rospy.init_node('assigner', anonymous=False) # fetching all parameters map_topic = rospy.get_param('~map_topic', '/map') # this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate info_radius = rospy.get_param('~info_radius', 1.0) info_multiplier = rospy.get_param('~info_multiplier', 3.0) # at least as much as the laser scanner range hysteresis_radius = rospy.get_param('~hysteresis_radius', 3.0) # bigger than 1 (biase robot to continue exploring current region hysteresis_gain = rospy.get_param('~hysteresis_gain', 2.0) frontiers_topic = rospy.get_param('~frontiers_topic', '/filtered_points') n_robots = rospy.get_param('~n_robots', 1) namespace = rospy.get_param('~namespace', '') namespace_init_count = rospy.get_param('namespace_init_count', 1) delay_after_assignement = rospy.get_param('~delay_after_assignement', 0.5) rateHz = rospy.get_param('~rate', 100) threshold = rospy.get_param('~costmap_clearing_threshold', 70) rate = rospy.Rate(rateHz) rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) rospy.Subscriber(frontiers_topic, PointArray, callBack) # --------------------------------------------------------------------------------------------------------------- # wait if no frontier is received yet while len(frontiers) < 1: time.sleep(0.1) pass centroids = copy(frontiers) # wait if map is not received yet while (len(mapData.data) < 1): time.sleep(0.1) pass robots = [] if len(namespace) > 0: for i in range(0, n_robots): robots.append(robot(namespace + str(i + namespace_init_count))) elif len(namespace) == 0: robots.append(robot(namespace)) for i in range(0, n_robots): robots[i].sendGoal(robots[i].getPosition()[0:2]) # ------------------------------------------------------------------------- # --------------------- Main Loop ------------------------------- # ------------------------------------------------------------------------- while not rospy.is_shutdown(): centroids = copy(frontiers) # ------------------------------------------------------------------------- # Get information gain for each frontier point infoGain = [] for fnt in range(0, len(centroids)): infoGain.append( informationGain(mapData, [centroids[fnt][0], centroids[fnt][1]], info_radius)) # ------------------------------------------------------------------------- # get number of available/busy robots availableRobots = [] # available robots busyRobots = [] # busy robots for i in range(0, n_robots): # rospy.loginfo('Robot State: ' + str(robots[i].getState())) if (robots[i].getState() == 1): busyRobots.append(i) # Check for the current goal grid cell x = array([ robots[i].goal.target_pose.pose.position.x, robots[i].goal.target_pose.pose.position.y ]) GoalGridValue = gridValue(mapData, x) if (GoalGridValue > threshold): robots[i].cancelGoal() rospy.loginfo( str.format( 'Canceling Goal for Robot-{0:d} due to threshold: {1:d}', i, GoalGridValue)) else: availableRobots.append(i) # ------------------------------------------------------------------------- # get dicount and update informationGain for i in busyRobots + availableRobots: infoGain = discount(mapData, robots[i].assigned_point, centroids, infoGain, info_radius) # ------------------------------------------------------------------------- reveneus = [] centroidRecord = [] idRecord = [] costN = [] costI = [] costO = [] for rob in availableRobots: for fnt in range(0, len(centroids)): pose = robots[rob].getPosition() costN.append(norm(pose[0:2] - centroids[fnt])) information_gain = infoGain[fnt] # if a frontier is within the vacinity of a certain radius, give it imporatance if (norm(pose[0:2] - centroids[fnt]) <= hysteresis_radius): information_gain *= hysteresis_gain costI.append(information_gain * info_multiplier) costO.append(50 * (atan2(pose[1] - centroids[fnt][1], pose[0] - centroids[fnt][0]) - pose[2])) revenue = costI[-1] - costN[-1] - costO[-1] reveneus.append(revenue) centroidRecord.append(centroids[fnt]) idRecord.append(rob) # ------------------------------------------------------------------------- if (len(idRecord) > 0): idx = reveneus.index(max(reveneus)) robots[idRecord[idx]].sendGoal(centroidRecord[idx]) rospy.loginfo("Robots:" + str(len(availableRobots)) + ", Frontiers:" + str(len(centroids))) rospy.loginfo( str.format("[{:0.2f},{:0.2f}]", *centroidRecord[idx]) + str.format( " assigned to Robot {0:d} -> R:{1:0.2f} I:{2:0.2f} N:{3:0.2f} O:{4:0.2f}", namespace_init_count + idRecord[idx], reveneus[idx], costI[idx], costN[idx], costO[idx])) rospy.sleep(delay_after_assignement) # ------------------------------------------------------------------------- rate.sleep()
def node(): global frontiers, mapData # , global1, global2, global3, globalmaps rospy.init_node('assigner', anonymous=False) # fetching all parameters map_topic = rospy.get_param('~map_topic', '/map') # this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate info_radius = rospy.get_param('~info_radius', 1.0) info_multiplier = rospy.get_param('~info_multiplier', 3.0) # at least as much as the laser scanner range hysteresis_radius = rospy.get_param('~hysteresis_radius', 3.0) # bigger than 1 (biase robot to continue exploring current region hysteresis_gain = rospy.get_param('~hysteresis_gain', 2.0) frontiers_topic = rospy.get_param('~frontiers_topic', '/filtered_points') n_robots = rospy.get_param('~n_robots', 1) namespace = rospy.get_param('~namespace', '') namespace_init_count = rospy.get_param('namespace_init_count', 1) delay_after_assignement = rospy.get_param('~delay_after_assignement', 0.5) rateHz = rospy.get_param('~rate', 100) rate = rospy.Rate(rateHz) # ------------------------------------------- rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) rospy.Subscriber(frontiers_topic, PointArray, callBack) # --------------------------------------------------------------------------------------------------------------- # wait if no frontier is received yet while len(frontiers) < 1: rate.sleep() pass centroids = copy(frontiers) # wait if map is not received yet while (len(mapData.data) < 1): rate.sleep() pass rospy.loginfo('after check') robots = [] robotsTime = [] rejectTime = [] robotsLastPos = [] robots.append(robot('')) robotsTime.append(rospy.Time.now().to_sec()) rejectTime.append(0) robotsLastPos.append(robots[0].getPosition()) rospy.logerr('robot created') for i in range(0, n_robots): robots[i].sendGoal(robots[i].getPosition()) # distance = 0 # ------------------------------------------------------------------------- # --------------------- Main Loop ------------------------------- # ------------------------------------------------------------------------- while not rospy.is_shutdown(): centroids = copy(frontiers) # ------------------------------------------------------------------------- # Get information gain for each frontier point infoGain = [] for ip in range(0, len(centroids)): infoGain.append(informationGain( mapData, [centroids[ip][0], centroids[ip][1]], info_radius)) # ------------------------------------------------------------------------- # get number of available/busy robots na = [] # available robots nb = [] # busy robots for i in range(0, n_robots): if (robots[i].getState() == 1): nb.append(i) else: na.append(i) rospy.loginfo("available robots: "+str(na)) # ------------------------------------------------------------------------- # get dicount and update informationGain for i in nb+na: infoGain = discount( mapData, robots[i].assigned_point, centroids, infoGain, info_radius) # ------------------------------------------------------------------------- revenue_record = [] centroid_record = [] id_record = [] now = rospy.Time.now().to_sec() # distance = norm(robotsLastPos[0] - robots[0].getPosition()) # robotsLastPos[0] = robots[0].getPosition() for ir in na: rospy.loginfo('arrive') for ip in range(0, len(centroids)): cost = norm(robots[ir].getPosition()-centroids[ip]) threshold = 1 information_gain = infoGain[ip] if (norm(robots[ir].getPosition()-centroids[ip]) <= hysteresis_radius): information_gain *= hysteresis_gain revenue = information_gain*info_multiplier-cost revenue_record.append(revenue) centroid_record.append(centroids[ip]) id_record.append(ir) robotsTime[ir] = now robotsLastPos[ir] = robots[i].getPosition() # distance = 0 # rejectTime[ir] = 0 # # try doing sth. to get away from unaccessable point # if len(na) < 1: # revenue_record = [] # centroid_record = [] # id_record = [] # for ir in nb: # for ip in range(0, len(centroids)): # cost = norm(robots[ir].getPosition()-centroids[ip]) # threshold = 1 # information_gain = infoGain[ip] # if (norm(robots[ir].getPosition()-centroids[ip]) <= hysteresis_radius): # information_gain *= hysteresis_gain # if ((norm(centroids[ip]-robots[ir].assigned_point)) < hysteresis_radius): # information_gain = informationGain( # mapData, [centroids[ip][0], centroids[ip][1]], info_radius)*hysteresis_gain # revenue = information_gain*info_multiplier-cost # revenue_record.append(revenue) # centroid_record.append(centroids[ip]) # id_record.append(ir) if len(na) < 1: now = rospy.Time.now().to_sec() for ir in nb: doReset = False if now - robotsTime[ir] > 10: robotsTime[ir] = now if norm(robotsLastPos[ir] - robots[ir].getPosition()) < 0.2: # too less move rospy.logwarn('reset goal... time:%lf' % (now)) # distance = 0 robotsTime[ir] = now robotsLastPos[ir] = robots[i].getPosition() doReset = True if robots[ir].getState() == 5: if rejectTime[ir] == 0: rejectTime = now elif now - rejectTime[ir] > 1.5: doReset = True rejectTime[ir] = 0 # if doReset: for ip in range(0, len(centroids)): cost = norm(robots[ir].getPosition()-centroids[ip]) threshold = 1 information_gain = -infoGain[ip] if (norm(robots[ir].getPosition()-centroids[ip]) <= hysteresis_radius): information_gain *= -hysteresis_gain if ((norm(centroids[ip]-robots[ir].assigned_point)) < hysteresis_radius): information_gain = -informationGain( mapData, [centroids[ip][0], centroids[ip][1]], info_radius)*hysteresis_gain # do reset # if doReset: # information_gain = -information_gain # kill current goal # should I cancel old goal ? # else: # information_gain = np.inf # fobid reset (if the old goal already accessable) revenue = information_gain*info_multiplier-cost if information_gain < 0: if doReset: revenue = 0 - cost else: revenue = - information_gain*info_multiplier - cost revenue_record.append(revenue) centroid_record.append(centroids[ip]) id_record.append(ir) # rospy.loginfo("revenue record: "+str(revenue_record)) # rospy.loginfo("centroid record: "+str(centroid_record)) # rospy.loginfo("robot IDs record: "+str(id_record)) # ------------------------------------------------------------------------- if (len(id_record) > 0): winner_id = revenue_record.index(max(revenue_record)) robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) rospy.loginfo(namespace+str(namespace_init_count + id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) rospy.sleep(delay_after_assignement) # ------------------------------------------------------------------------- rate.sleep()