def calculate_utility(self, frontier_point):
# Utility function for a single point
lambda_u = 2.0
utility = lambda_u * (informationGain(
self.mapData, [frontier_point.x, frontier_point.y],
self.info_radius))
return utility
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', '/local_map/local_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)
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, PoseArray, 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 = []
# for i in range(0, n_robots):
# robots.append(robot('/robot_' + str(i + 1)))
# 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("robot_" + str(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('filter', anonymous=False)
# fetching all parameters
map_topic= rospy.get_param('~map_topic','/robot_1/map')
threshold= rospy.get_param('~costmap_clearing_threshold',70)
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
goals_topic= rospy.get_param('~goals_topic','/detected_points')
n_robots = rospy.get_param('~n_robots',1)
rateHz = rospy.get_param('~rate',100)
rate = rospy.Rate(rateHz)
#-------------------------------------------
rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
#---------------------------------------------------------------------------------------------------------------
for i in range(0,n_robots):
globalmaps.append(OccupancyGrid())
for i in range(0,n_robots):
rospy.Subscriber('/robot_'+str(i+1)+'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap)
#wait if map is not received yet
while (len(mapData.data)<1):
pass
#wait if any of robots' global costmap map is not received yet
for i in range(0,n_robots):
while (len(globalmaps[i].data)<1):
pass
global_frame="/"+mapData.header.frame_id
tfLisn=tf.TransformListener()
for i in range(0,n_robots):
tfLisn.waitForTransform(global_frame[1:], 'robot_'+str(i+1)+'/base_link', rospy.Time(0),rospy.Duration(10.0))
rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]])
pub = rospy.Publisher('frontiers', Marker, queue_size=10)
pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)
rospy.loginfo("the map and global costmaps are received")
# wait if no frontier is received yet
while len(frontiers)<1:
pass
points=Marker()
points_clust=Marker()
#Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id= mapData.header.frame_id
points.header.stamp= rospy.Time.now()
points.ns= "markers2"
points.id = 0
points.type = Marker.POINTS
#Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points.action = Marker.ADD;
points.pose.orientation.w = 1.0
points.scale.x=0.2
points.scale.y=0.2
points.color.r = 255.0/255.0
points.color.g = 255.0/255.0
points.color.b = 0.0/255.0
points.color.a=1;
points.lifetime = rospy.Duration();
p=Point()
p.z = 0;
pp=[]
pl=[]
points_clust.header.frame_id= mapData.header.frame_id
points_clust.header.stamp= rospy.Time.now()
points_clust.ns= "markers3"
points_clust.id = 4
points_clust.type = Marker.POINTS
#Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points_clust.action = Marker.ADD;
points_clust.pose.orientation.w = 1.0;
points_clust.scale.x=0.2;
points_clust.scale.y=0.2;
points_clust.color.r = 0.0/255.0
points_clust.color.g = 255.0/255.0
points_clust.color.b = 0.0/255.0
points_clust.color.a=1;
points_clust.lifetime = rospy.Duration();
temppoint=PointStamped()
temppoint.header.frame_id= mapData.header.frame_id
temppoint.header.stamp=rospy.Time(0)
temppoint.point.z=0.0
arraypoints=PointArray()
tempPoint=Point()
tempPoint.z=0.0
#-------------------------------------------------------------------------
#--------------------- Main Loop -------------------------------
#-------------------------------------------------------------------------
while not rospy.is_shutdown():
#-------------------------------------------------------------------------
#Clustering frontier points
centroids=[]
front=copy(frontiers)
if len(front)>1:
ms = MeanShift(bandwidth=0.3)
ms.fit(front)
centroids= ms.cluster_centers_ #centroids array is the centers of each cluster
#if there is only one frontier no need for clustering, i.e. centroids=frontiers
if len(front)==1:
centroids=front
frontiers=copy(centroids)
#-------------------------------------------------------------------------
#clearing old frontiers
z=0
while z<len(centroids):
cond=False
temppoint.point.x=centroids[z][0]
temppoint.point.y=centroids[z][1]
for i in range(0,n_robots):
transformedPoint=tfLisn.transformPoint(globalmaps[i].header.frame_id,temppoint)
x=array([transformedPoint.point.x,transformedPoint.point.y])
cond=(gridValue(globalmaps[i],x)>threshold) or cond
if (cond or (informationGain(mapData,[centroids[z][0],centroids[z][1]],info_radius*0.5))<0.2):
centroids=delete(centroids, (z), axis=0)
z=z-1
z+=1
#-------------------------------------------------------------------------
#publishing
arraypoints.points=[]
for i in centroids:
tempPoint.x=i[0]
tempPoint.y=i[1]
arraypoints.points.append(copy(tempPoint))
filterpub.publish(arraypoints)
pp=[]
for q in range(0,len(frontiers)):
p.x=frontiers[q][0]
p.y=frontiers[q][1]
pp.append(copy(p))
points.points=pp
pp=[]
for q in range(0,len(centroids)):
p.x=centroids[q][0]
p.y=centroids[q][1]
pp.append(copy(p))
points_clust.points=pp
pub.publish(points)
pub2.publish(points_clust)
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', '/global_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',
20.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)
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, PoseArray, callBack)
assigned_pub = rospy.Publisher('assigned_centroid', Marker, queue_size=10)
goal_pub = rospy.Publisher('goal_point', PoseStamped, queue_size=1)
# ---------------------------------------------------------------------------------------------------------------
# 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
# get initial position
listener0 = tf.TransformListener()
cond0 = 0
while cond0 == 0:
try:
(trans0, rot0) = listener0.lookupTransform('/odom', '/base_link',
rospy.Time(0))
cond0 = 1
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
cond0 == 0
p = Point()
p.x = 0
p.y = 0
p.z = 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))
# decrease info gain of centroid that overlap with current goal centroid
infoGain = discount(mapData, array([p.x, p.y]), centroids, infoGain,
info_radius)
# -------------------------------------------------------------------------
# # get number of available/busy robots
# na = [] # available robots --> last goal point is arrived
# nb = [] # busy robots --> last goal point isn't arrived
# 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)
# -------------------------------------------------------------------------
# update position
listener = tf.TransformListener()
cond = 0
while cond == 0:
try:
(trans,
rot) = listener.lookupTransform('/odom', '/base_link',
rospy.Time(0))
cond = 1
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
cond == 0
current_position = array([trans[0] - trans0[0], trans[1] - trans0[1]])
revenue_record = []
centroid_record = []
for ip in range(0, len(centroids)):
cost = norm(current_position - centroids[ip])
threshold = 1
information_gain = infoGain[ip]
if (norm(current_position - centroids[ip]) <= hysteresis_radius):
information_gain *= hysteresis_gain
else:
information_gain *= threshold
# rospy.loginfo("information_gain: " + str(information_gain))
# rospy.loginfo("navi_cost: " + str(cost))
revenue = information_gain * info_multiplier - cost
revenue_record.append(revenue)
centroid_record.append(centroids[ip])
# rospy.loginfo("revenue record: " + str(revenue_record))
# rospy.loginfo("centroid record: " + str(centroid_record))
# -------------------------------------------------------------------------
winner_id = revenue_record.index(max(revenue_record))
# robots[id_record[winner_id]].sendGoal(centroid_record[winner_id])
rospy.loginfo(" chosen centroid ID: " +
str(centroid_record[winner_id]) + " revenue value : " +
str(max(revenue_record)))
# publish assigned centroid
points = Marker()
# Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id = mapData.header.frame_id
points.header.stamp = rospy.Time(0)
points.id = 0
points.type = Marker.POINTS
# Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points.action = Marker.ADD
points.pose.orientation.w = 1.0
points.scale.x = 1.5
points.scale.y = 1.5
points.color.r = 255.0 / 255.0
points.color.g = 255.0 / 255.0
points.color.b = 0.0 / 255.0
points.color.a = 1
points.lifetime = rospy.Duration()
pp = []
p.x = centroid_record[winner_id][0]
p.y = centroid_record[winner_id][1]
pp.append(copy(p))
points.points = pp
assigned_pub.publish(points)
yaw = getYawToUnknown(mapData, centroid_record[winner_id], info_radius)
q = quaternion_from_euler(0.0, 0.0, yaw)
# publisher goal point
goal_point = PoseStamped()
goal_point.header.frame_id = mapData.header.frame_id
goal_point.header.stamp = rospy.Time(0)
goal_point.pose.position.x = centroid_record[winner_id][0]
goal_point.pose.position.y = centroid_record[winner_id][1]
goal_point.pose.orientation = Quaternion(*q)
goal_pub.publish(goal_point)
# -------------------------------------------------------------------------
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()
def node():
print('usao u glavni')
global mapData, global1, global2, global3, globalmaps, current_frontier, all_frontiers, litraIndx, n_robots, namespace_init_count
rospy.init_node('filter', anonymous=False)
# fetching all parameters
map_topic = rospy.get_param('~map_topic', '/map')
threshold = rospy.get_param('~costmap_clearing_threshold', 50)
info_radius = rospy.get_param(
'~info_radius', 1
) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
goals_topic = rospy.get_param('~goals_topic', '/detected_points')
transform_topic = rospy.get_param('~transform_topic',
'/transformed_points')
submap_topic = rospy.get_param('~submap_topic', '/used_submaps')
n_robots = rospy.get_param('~n_robots', 1)
namespace_list = rospy.get_param('~namespace_list', ['alpha'])
namespace_init_count = rospy.get_param('namespace_init_count', 1)
rateHz = rospy.get_param('~rate', 100)
litraIndx = len(namespace_list)
rate = rospy.Rate(rateHz)
#-------------------------------------------
rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
#---------------------------------------------------------------------------------------------------------------
for i in range(0, n_robots):
globalmaps.append(OccupancyGrid())
if len(namespace_list) > 0:
for i in range(0, len(namespace_list)):
print(namespace_list[i])
rospy.Subscriber(
namespace_list[i] + '/move_base/global_costmap/costmap',
OccupancyGrid, globalMap)
elif len(namespace_list) == 0:
rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid,
globalMap)
print('usao u glavni2')
#wait if map is not received yet
while (len(mapData.data) < 1):
pass
print('usao u glavni3')
#wait if any of robots' global costmap map is not received yet
for i in range(0, n_robots):
while (len(globalmaps[i].data) < 1):
pass
global_frame = "/" + mapData.header.frame_id
print('usao u glavni4')
tfLisn = tf.TransformListener()
rospy.sleep(2)
if len(namespace_list) > 0:
for i in range(0, len(namespace_list)):
tfLisn.waitForTransform(global_frame[1:],
namespace_list[i] + '/base_link',
rospy.Time(0), rospy.Duration(10.0))
elif len(namespace_list) == 0:
tfLisn.waitForTransform(global_frame[1:], '/base_link', rospy.Time(0),
rospy.Duration(10.0))
rospy.Subscriber(submap_topic, SubmapList, submapCallBack)
rospy.Subscriber(transform_topic, FrontierTF, transformCallBack)
#rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]])
pub = rospy.Publisher('frontiers', Marker, queue_size=10)
pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)
filterpub2 = rospy.Publisher('filtered_struct', FrontierTF, queue_size=10)
rospy.loginfo("the map and global costmaps are received")
# wait if no frontier is received yet
while len(current_frontier) < 1:
pass
points = Marker()
points_clust = Marker()
#Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id = mapData.header.frame_id
points.header.stamp = rospy.Time.now()
points.ns = "markers2"
points.id = 0
points.type = Marker.POINTS
#Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points.action = Marker.ADD
points.pose.orientation.w = 1.0
points.scale.x = 0.2
points.scale.y = 0.2
points.color.r = 255.0 / 255.0
points.color.g = 255.0 / 255.0
points.color.b = 0.0 / 255.0
points.color.a = 1
points.lifetime = rospy.Duration()
p = Point()
p.z = 0
pp = []
pl = []
points_clust.header.frame_id = mapData.header.frame_id
points_clust.header.stamp = rospy.Time.now()
points_clust.ns = "markers3"
points_clust.id = 4
points_clust.type = Marker.POINTS
#Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points_clust.action = Marker.ADD
points_clust.pose.orientation.w = 1.0
points_clust.scale.x = 0.2
points_clust.scale.y = 0.2
points_clust.color.r = 0.0 / 255.0
points_clust.color.g = 255.0 / 255.0
points_clust.color.b = 0.0 / 255.0
points_clust.color.a = 1
points_clust.lifetime = rospy.Duration()
temppoint = PointStamped()
temppoint.header.frame_id = mapData.header.frame_id
temppoint.header.stamp = rospy.Time(0)
temppoint.point.z = 0.0
arraypoints = PointArray()
tempPoint = Point()
tempPoint.z = 0.0
#-------------------------------------------------------------------------
#--------------------- Main Loop -------------------------------
#-------------------------------------------------------------------------
while not rospy.is_shutdown():
#-------------------------------------------------------------------------
#SVE FRONTIERE PONOVNO TRANSFORMIRATI, U SVAKOM PROLAZU
my_frontiers = copy(current_frontier)
transform(my_frontiers)
transform(all_frontiers)
all_frontiers.extend(my_frontiers)
#print ('koje dobivam', all_frontiers)
all_centroids = copy(all_frontiers)
#print('centroidi su:', all_centroids[0])
#-------------------------------------------------------------------------
#clearing old frontiers
z = 0
#print ('duljina frontiera',len(all_frontiers))
for centroid in all_centroids:
cond = False
temppoint.point.x = centroid.projected.x
temppoint.point.y = centroid.projected.y
for i in range(0, len(namespace_list)):
transformedPoint = tfLisn.transformPoint(
globalmaps[i].header.frame_id, temppoint)
x = array([transformedPoint.point.x, transformedPoint.point.y])
cond = (gridValue(globalmaps[i], x) > threshold) or cond
if (cond or (informationGain(
mapData, [centroid.projected.x, centroid.projected.y],
info_radius * 0.5)) < 0.3):
all_centroids = list(delete(all_centroids, (z), axis=0))
z = z - 1
#print ('deleted centroid')
z += 1
print("resize frontiers from %d to %d" %
(len(all_frontiers), len(all_centroids)))
#-------------------------------------------------------------------------
#------------------------------------------------------------------------
#publishing
arraypoints.points = []
#arraystruct=[]
cnt = 0
for centroid in all_centroids:
tempPoint.x = centroid.projected.x
tempPoint.y = centroid.projected.y
arraypoints.points.append(copy(tempPoint))
#arraystruct.append(copy(centroid))
cnt += 1
filterpub.publish(arraypoints)
#filterpub2.publish(arraystruct)
print("Published transformed centroids:" + str(cnt))
#-------------------------------------------------------------------------
pp = []
for q in all_centroids:
p.x = q.projected.x
p.y = q.projected.y
pp.append(copy(p))
points_clust.points = pp
pub2.publish(points_clust)
print("delete old frontiers")
z = 0
for frontier in all_frontiers:
cond = False
temppoint.point.x = frontier.projected.x
temppoint.point.y = frontier.projected.y
for i in range(0, len(namespace_list)):
transformedPoint = tfLisn.transformPoint(
globalmaps[i].header.frame_id, temppoint)
x = array([transformedPoint.point.x, transformedPoint.point.y])
cond = (gridValue(globalmaps[i], x) > threshold) or cond
if (cond or (informationGain(
mapData, [frontier.projected.x, frontier.projected.y],
info_radius * 0.5)) < 0.2):
all_frontiers = list(delete(all_frontiers, (z), axis=0))
z = z - 1
z += 1
pp = []
for q in all_frontiers:
p.x = q.projected.x
p.y = q.projected.y
pp.append(copy(p))
points.points = pp
pub.publish(points)
rate.sleep()
