I have already been able to calculate the polar histogram, as described in the paper, as well as the smoothing function to eliminate noise. This variable is stored in a numpy array, named
However, the function
computeTheta, pasted below, which computes the steering direction, is only able to compute the proper direction if the valleys (i.e. consecutive sectors in the polar histogram whose obstacle density is below a certain threshold) do not contain the section where a full circle is completed, i.e. the sector corresponding to
To make things clearer, consider these two examples:
- If the histogram contains a peak in the angles between, say,
30º, with the rest of the histogram being a valley, then the steering direction will be computed correctly.
If, however, the peak is contained between, say,
60º, then the valley will start at
60º, go all the way past
360ºand end in
30º, and the steering direction will be computed incorrectly, since this single valley will be considered two valleys, one between
30º, and another between
def computeTheta(self, goal):
thrs = 2. s_max = 18 #We start by calculating the sector corresponding to the direction of the target. target_sector = int((180./np.pi)*np.arctan2(goal - self.VCP, goal - self.VCP)) if target_sector < 0: target_sector += 360 target_sector /= 5 #Next, we assume there is no best sector. best_sector = -1 dist_best_and_target = abs(target_sector - best_sector) #Then, we find the sector within a valley that is closest to the target sector. for k in range(self.Hist.shape): if self.Hist[k] < thrs and abs(target_sector - k) < dist_best_and_target: best_sector = k dist_best_and_target = abs(target_sector - k) #If the sector is still -1, we return it as an error. print (target_sector, best_sector) if best_sector == -1: return -1 #If not, we can proceed... elif best_sector > -1: #... by deciding whether the valley to which the best sector belongs is a "wide" or a "narrow" one. #Assume it's wide. type_of_valley = "Wide" #If we find a sector that contradicts our assumption, we change our minds. for sector in range(best_sector, best_sector + s_max + 1): if sector < self.Hist.shape: if self.Hist[sector] > thrs: type_of_valley = "Narrow" #If it is indeed a wide valley, we return the angle corresponding to the sector (k_n + s_max)/2. if type_of_valley == "Wide": theta = 5*(best_sector + s_max)/2 return theta #Otherwise, we find the far border of the valley and return the angle corresponding to the mean value between the best sector and the far border. elif type_of_valley == "Narrow": for sector in range(best_sector, best_sector + s_max): if self.Hist[sector] < thrs: far_border = sector theta = 5*(best_sector + far_border)/2 return theta
How can I address this issue? Is there a way to treat the histogram as circular? Is there maybe a better way to write this function?
Thank you for your time.