def handle_UpdateSTeFMap(self, req):
self.bin_counts_matrix = np.reshape(
req.data, [self.width, self.height, self.num_bins])
self.bin_counts_matrix_accumulated = self.bin_counts_matrix_accumulated + self.bin_counts_matrix
self.update_entropy_map(req.update_time)
#normalize the histogram before update fremen in case they are not
for r in range(0, int(self.width)):
for c in range(0, int(self.height)):
max_count = np.amax(self.bin_counts_matrix[r][c][:])
if max_count > 0:
for b in range(0, int(self.num_bins)):
self.bin_counts_matrix[r][c][
b] = 100 * self.bin_counts_matrix[r][c][
b] / max_count
fremenarray_msg = FremenArrayGoal()
fremenarray_msg.operation = 'add'
fremenarray_msg.time = req.update_time
fremenarray_msg.states = np.reshape(
self.bin_counts_matrix, self.width * self.height * self.num_bins)
self.fremenarray_client.send_goal(fremenarray_msg)
self.fremenarray_client.wait_for_result()
fremenarray_result = self.fremenarray_client.get_result()
rospy.loginfo(fremenarray_result.message)
return fremenarray_result.message
def handle_GetSTeFMap(self,req):
mSTefMap = STeFMapMsg()
mSTefMap.header.stamp = rospy.get_rostime()
mSTefMap.header.frame_id = self.frame_id
mSTefMap.prediction_time = req.prediction_time
mSTefMap.x_min = self.x_min
mSTefMap.x_max = self.x_max
mSTefMap.y_min = self.y_min
mSTefMap.y_max = self.y_max
mSTefMap.cell_size = self.grid_size
mSTefMap.rows = self.width
mSTefMap.columns = self.height
fremenarray_msg = FremenArrayGoal()
fremenarray_msg.operation = 'predict'
fremenarray_msg.order = req.order
fremenarray_msg.time = req.prediction_time
self.fremenarray_client.send_goal(fremenarray_msg)
if not self.fremenarray_client.wait_for_result(rospy.Duration(10.0)):
print("Error getting the STeF-Map")
mSTefMap = STeFMapMsg()
return mSTefMap
fremenarray_result = self.fremenarray_client.get_result()
prob_matrix = np.reshape(fremenarray_result.probabilities,(self.width,self.height,self.num_bins))
# iterate through all the cell and get also the bin with the maximum probability and the associated angle
index = 0
for r in range(0,mSTefMap.rows):
for c in range(0,mSTefMap.columns):
stefmap_cell = STeFMapCellMsg()
stefmap_cell.row = int(r)
stefmap_cell.column = int(c)
stefmap_cell.x = float(self.x_min + self.grid_size*0.5 + self.grid_size * r)
stefmap_cell.y = float(self.y_min + self.grid_size*0.5 + self.grid_size * c)
stefmap_cell.probabilities = [float(prob_matrix[r,c,0]) ,
float(prob_matrix[r,c,1]) ,
float(prob_matrix[r,c,2]) ,
float(prob_matrix[r,c,3]) ,
float(prob_matrix[r,c,4]) ,
float(prob_matrix[r,c,5]) ,
float(prob_matrix[r,c,6]) ,
float(prob_matrix[r,c,7]) ]
max_number = -1
for b in range(0,self.num_bins):
if prob_matrix[r,c,b] > max_number:
max_number = prob_matrix[r,c,b]
max_orientation = b
stefmap_cell.best_angle = max_orientation*360/self.num_bins
mSTefMap.cells.append(stefmap_cell)
index = index + 1
self.stefmap_pub.publish(mSTefMap)
print("STeFMap sent!")
return mSTefMap
def __init__(self):
#parameters
self.grid_size = rospy.get_param('~grid_size', 1) #meters
self.x_min = rospy.get_param('~x_min', -50) #meters
self.x_max = rospy.get_param('~x_max', 50) #meters
self.y_min = rospy.get_param('~y_min', -50) #meters
self.y_max = rospy.get_param('~y_max', 50) #meters
self.num_bins = rospy.get_param('~num_bins',
8) #bins dividing the circumference
self.frame_id = rospy.get_param('~frame_id', "/map")
# initialize visibility map
self.width = int(
(self.x_max - self.x_min) / self.grid_size) ## [cells]
self.height = int(
(self.y_max - self.y_min) / self.grid_size) ## [cells]
# connect to fremenarray
rospy.loginfo("waiting for FremenArray.....")
self.fremenarray_client = actionlib.SimpleActionClient(
'/fremenarray', FremenArrayAction)
self.fremenarray_client.wait_for_server()
rospy.loginfo("FremenArray ready!")
# initiliatize stefmap
rospy.loginfo("Initializing STeF-map.....")
self.bin_counts_matrix = np.ones([
self.width, self.height, self.num_bins
]) * -1 #initialize cells as unexplored/unknow
self.bin_counts_matrix_accumulated = np.zeros(
[self.width, self.height, self.num_bins])
self.entropy_map_store = []
fremenarray_msg = FremenArrayGoal()
fremenarray_msg.operation = 'add'
fremenarray_msg.time = 0
fremenarray_msg.states = np.reshape(
self.bin_counts_matrix, self.width * self.height * self.num_bins)
self.fremenarray_client.send_goal(fremenarray_msg)
self.fremenarray_client.wait_for_result()
fremenarray_result = self.fremenarray_client.get_result()
rospy.loginfo(fremenarray_result.message)
rospy.loginfo("STeF-map ready!")
# create stef map service and topic
get_stefmap_service = rospy.Service('get_stefmap', GetSTeFMap,
self.handle_GetSTeFMap)
update_stefmap_service = rospy.Service('update_stefmap', UpdateSTeFMap,
self.handle_UpdateSTeFMap)
self.entropy_map_pub = rospy.Publisher('/entropy_map',
OccupancyGrid,
queue_size=1,
latch=True)
self.stefmap_pub = rospy.Publisher('/stefmap',
STeFMapMsg,
queue_size=1,
latch=True)
self.run()
def get_map_at_time(time, order, xmin, xmax, ymin, ymax, cell_size):
rows = int((ymax - ymin) / cell_size)
cols = int((xmax - xmin) / cell_size)
rospy.init_node('fremenarray_client')
fremenarray_client = actionlib.SimpleActionClient('/fremenarray',
FremenArrayAction)
fremenarray_client.wait_for_server()
frem_msg = FremenArrayGoal()
frem_msg.operation = 'predict'
frem_msg.order = order
frem_msg.time = time
fremenarray_client.send_goal(frem_msg)
fremenarray_client.wait_for_result()
fremenarray_result = fremenarray_client.get_result()
stef_map = np.reshape(fremenarray_result.probabilities, (rows, cols, 8))
return stef_map
def update_fremen_models(self,timer):
if self.visibility_map_received == False:
rospy.loginfo("No visibility_map received yet")
#before normalizing the counts, update the spatial entropy model
bin_counts_matrix_normalised = self.bin_counts_matrix
bin_counts_1darray = np.reshape(self.bin_counts_matrix,self.width*self.height*self.num_bins)
self.bin_counts_matrix = np.ones([self.width,self.height,self.num_bins])*-1 #initialize cells as unexplored/unknow
cells_visible = 0
# Normalize the count matrix for each cell, giving the orientation with the maximum counts the max value
for r in range(0,int(self.width)):
for c in range(0,int(self.height)):
max_count = np.amax(bin_counts_matrix_normalised[r][c][:])
if max_count > 0:
for b in range(0,int(self.num_bins)):
bin_counts_matrix_normalised[r][c][b] = 100*bin_counts_matrix_normalised[r][c][b]/max_count
if max_count == 0:
cells_visible = cells_visible + 1
fremenarray_msg = FremenArrayGoal()
fremenarray_msg.operation = 'add'
fremenarray_msg.time = rospy.get_time()
fremenarray_msg.states = np.reshape(bin_counts_matrix_normalised,self.width*self.height*self.num_bins)
self.fremenarray_client.send_goal(fremenarray_msg)
self.fremenarray_client.wait_for_result()
fremenarray_result = self.fremenarray_client.get_result()
rospy.loginfo(fremenarray_result.message)
self.update_entropy_map()
if self.save_histograms:
with open(self.histograms_to_save_file,"a") as hfile:
hfile.write(str(fremenarray_msg.time))
hfile.write(",")
for j in range(0,self.width*self.height*self.num_bins):
hfile.write(str(int(bin_counts_1darray[j])))
if j != self.width*self.height*self.num_bins-1:
hfile.write(",")
hfile.write("\n")
def __init__(self):
#parameters
self.grid_size = rospy.get_param('~grid_size',1)#meters
self.x_min = rospy.get_param('~x_min',-50)#meters
self.x_max = rospy.get_param('~x_max',50) #meters
self.y_min = rospy.get_param('~y_min',-50)#meters
self.y_max = rospy.get_param('~y_max',50) #meters
self.interval_time = rospy.get_param('~interval_time',600)#seconds
self.num_bins = rospy.get_param('~num_bins',8) #bins dividing the circumference
self.frame_id = rospy.get_param('~frame_id',"map")
self.load_data = rospy.get_param('~load_data',False)
self.data_to_load_file = rospy.get_param('~data_to_load_file',"test_data.txt") # meters
self.save_histograms = rospy.get_param('~save_histograms',False) # meters
self.histograms_to_save_file = rospy.get_param('~histograms_to_save_file',"test_histograms.txt") # meters
self.people_detections_topic_1 = rospy.get_param('~people_detections_topic_1',"/people_detections_1")
self.people_detections_topic_2 = rospy.get_param('~people_detections_topic_2',"/people_detections_2")
self.people_detections_topic_3 = rospy.get_param('~people_detections_topic_3',"/people_detections_3")
self.people_detections_topic_4 = rospy.get_param('~people_detections_topic_4',"/people_detections_4")
self.people_detections_topic_5 = rospy.get_param('~people_detections_topic_5',"/people_detections_5")
self.people_detections_topic_6 = rospy.get_param('~people_detections_topic_6',"/people_detections_6")
self.people_detections_topic_7 = rospy.get_param('~people_detections_topic_7',"/people_detections_7")
self.people_detections_topic_8 = rospy.get_param('~people_detections_topic_8',"/people_detections_8")
self.people_detections_topic_9 = rospy.get_param('~people_detections_topic_9',"/people_detections_9")
#variables
self.width = int((self.x_max-self.x_min)/self.grid_size) ## [cells]
self.height = int((self.y_max-self.y_min)/self.grid_size) ## [cells]
self.tf_listener = tf.TransformListener()
# connect to fremenarray
rospy.loginfo("waiting for FremenArray.....")
self.fremenarray_client = actionlib.SimpleActionClient('/fremenarray', FremenArrayAction)
self.fremenarray_client.wait_for_server()
rospy.loginfo("FremenArray ready!")
# initiliatize stefmap
rospy.loginfo("Initializing STeF-map.....")
self.bin_counts_matrix = np.ones([self.width,self.height,self.num_bins])*-1 #initialize cells as unexplored/unknow
self.bin_counts_matrix_accumulated = np.zeros([self.width,self.height,self.num_bins])
fremenarray_msg=FremenArrayGoal()
fremenarray_msg.operation = 'add'
fremenarray_msg.time = 0
fremenarray_msg.states = np.reshape(self.bin_counts_matrix,self.width*self.height*self.num_bins)
self.fremenarray_client.send_goal(fremenarray_msg)
self.fremenarray_client.wait_for_result()
fremenarray_result = self.fremenarray_client.get_result()
rospy.loginfo(fremenarray_result.message)
rospy.loginfo("STeF-map ready!")
# create topic publishers
self.entropy_map_pub = rospy.Publisher('/entropy_map', OccupancyGrid, queue_size=1,latch=True)
self.stefmap_pub = rospy.Publisher('/stefmap', STeFMapMsg, queue_size=1, latch=True)
# create services
stefmap_service = rospy.Service('get_stefmap', GetSTeFMap, self.handle_GetSTeFMap)
# Before creating the timers to start updating the fremen models, check wheter there is some data to load
if self.load_data:
rospy.loginfo("Starting to load data")
try:
with open(self.data_to_load_file,"r") as file:
for line in file:
current_line = line.split(',')
timestamp = int(float(current_line[0]))
states = []
for i in range(1,len(current_line)):
states.append(int(current_line[i]))
try:
self.bin_counts_matrix = np.reshape(states,[self.width,self.height,self.num_bins])
except:
rospy.loginfo("Data to load dimension is not the same as the dimension defined for the stefmap")
continue
#normalize the histogram before update to fremen
for r in range(0,int(self.width)):
for c in range(0,int(self.height)):
max_count = np.amax(self.bin_counts_matrix[r][c][:])
if max_count > 0:
for b in range(0,int(self.num_bins)):
self.bin_counts_matrix_accumulated[r][c][b] = self.bin_counts_matrix_accumulated[r][c][b] + self.bin_counts_matrix[r][c][b]
self.bin_counts_matrix[r][c][b] = 100*self.bin_counts_matrix[r][c][b]/max_count
fremenarray_msg=FremenArrayGoal()
fremenarray_msg.operation = 'add'
fremenarray_msg.time = timestamp
fremenarray_msg.states = np.reshape(self.bin_counts_matrix,self.width*self.height*self.num_bins)
self.fremenarray_client.send_goal(fremenarray_msg)
self.fremenarray_client.wait_for_result()
fremenarray_result = self.fremenarray_client.get_result()
rospy.loginfo(fremenarray_result.message)
rospy.loginfo("All data loaded")
self.update_entropy_map()
self.bin_counts_matrix = np.ones([self.width,self.height,self.num_bins])*-1
except:
rospy.loginfo("No data to load file found")
# subscribe to topics
rospy.Subscriber("/visibility_map", OccupancyGrid, self.visibility_map_callback,queue_size=1)
self.visibility_map_received = False
rospy.Subscriber(self.people_detections_topic_1, PoseArray, self.people_detections_callback,queue_size=10)
rospy.Subscriber(self.people_detections_topic_2, PoseArray, self.people_detections_callback,queue_size=10)
rospy.Subscriber(self.people_detections_topic_3, PoseArray, self.people_detections_callback,queue_size=10)
rospy.Subscriber(self.people_detections_topic_4, PoseArray, self.people_detections_callback,queue_size=10)
rospy.Subscriber(self.people_detections_topic_5, PoseArray, self.people_detections_callback,queue_size=10)
rospy.Subscriber(self.people_detections_topic_6, PoseArray, self.people_detections_callback,queue_size=10)
rospy.Subscriber(self.people_detections_topic_7, PoseArray, self.people_detections_callback,queue_size=10)
rospy.Subscriber(self.people_detections_topic_8, PoseArray, self.people_detections_callback,queue_size=10)
rospy.Subscriber(self.people_detections_topic_9, PoseArray, self.people_detections_callback,queue_size=10)
# create timers
self.update_fremen_timer = rospy.Timer(rospy.Duration(self.interval_time),self.update_fremen_models)
self.run()