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