CircleFinder: Fixed bug with location and angle computation

minesearch_project/src/circleFinder.py

@@ -41,34 +41,39 @@ class CircleFinder:
         for interval in self.scan.intervals:
             if (interval[1] - interval[0]) >= 5:
                 leftAngle = self.scan.minAngle + interval[0] * self.scan.increment
+                leftAngle = self.scan.rayBearing(interval[0])
                 leftPt = (self.scan.ranges[interval[0]] * math.cos(leftAngle), \
                           self.scan.ranges[interval[0]] * math.sin(leftAngle))
 
                 middle = (interval[0] + interval[1]) / 2
                 middleAngle = self.scan.minAngle + middle * self.scan.increment
+                middleAngle = self.scan.rayBearing(middle)
                 middlePt = (self.scan.ranges[middle] * math.cos(middleAngle), \
                             self.scan.ranges[middle] * math.sin(middleAngle))
 
                 rightAngle = self.scan.minAngle + interval[1] * self.scan.increment
+                rightAngle = self.scan.rayBearing(interval[1])
                 rightPt = (self.scan.ranges[interval[1]] * math.cos(rightAngle), \
                            self.scan.ranges[interval[1]] * math.sin(rightAngle))
 
                 leftTest = (interval[0] + middle) / 2
                 leftTestAngle = self.scan.minAngle + leftTest * self.scan.increment
+                leftTestAngle = self.scan.rayBearing(leftTest)
                 leftTestPt = (self.scan.ranges[leftTest] * math.cos(leftTestAngle), \
                               self.scan.ranges[leftTest] * math.sin(leftTestAngle))
 
                 rightTest = (middle + interval[1]) / 2
                 rightTestAngle = self.scan.minAngle + rightTest * self.scan.increment
+                rightTestAngle = self.scan.rayBearing(rightTest)
                 rightTestPt = (self.scan.ranges[rightTest] * math.cos(rightTestAngle), \
                                self.scan.ranges[rightTest] * math.sin(rightTestAngle))
 
                 circle = rm.compute_circle(leftPt, middlePt, rightPt)
-                if (circle and (circle[1] < 50.0) and \
+                if (circle and (circle[1] < 25.0) and \
                     (abs(rm.cartesian_distance(circle[0], leftTestPt) - \
-                                               circle[1]) < 0.05) and \
+                                               circle[1]) < 0.1) and \
                     (abs(rm.cartesian_distance(circle[0], rightTestPt) - \
-                                               circle[1]) < 0.05)):
+                                               circle[1]) < 0.1)):
                     self.message.header.seq += 1
                     self.message.header.stamp = rospy.Time.now()
                     self.message.x = circle[0][0]