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