Merge pull request #55 from CentraleNantesRobotics/develop

Removing previous scans
CentraleNantesRobotics · Mar 27, 2020 · 0957c01 · 0957c01
2 parents cc02382 + 72efcde
commit 0957c01
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 23 deletions.
diff --git a/README.md b/README.md
@@ -68,19 +68,24 @@ Run the main node with:
 
 ![alt img](https://github.com/CentraleNantesRobotics/ping360_sonar/blob/master/img/print.png)
 
+
+Three extra parameters can be set to toggle specific topics, they are set to true by default.
 
-    Three extra parameters can be set to toggle specific topics, they are set to true by default.
-
-        <param name="enableImageTopic" value="True"/> 
-        <param name="enableScanTopic" value="True"/> 
-        <param name="enableDataTopic" value="True"/>
+        <param name="enableImageTopic" value="True or False"/> 
+        <param name="enableScanTopic" value="True or False"/> 
+        <param name="enableDataTopic" value="True or False"/>
 
-	By default, the sensor does a full 360° rotation, but that can be changed by modifying the min and max scan angle values.
+By default, the sensor does a full 360° rotation, but that can be changed by modifying the min and max scan angle values.
 
         <param name="minAngle" value="0"/> 
         <param name="maxAngle" value="400"/> 
-
-	Please note that these angle value are in GRAD and not DEG.
+
+Please note that these angle value are in GRAD and not DEG.
+
+You can also enable or disable the sensor oscillation by setting the **oscillate** parameter.
+
+        <param name="oscillate" value="True or False"/> 
+
 
 ## Nodes
 

diff --git a/src/ping360_sonar/node.py b/src/ping360_sonar/node.py
@@ -151,9 +151,8 @@ def main():
     image = np.zeros((imgSize, imgSize, 1), np.uint8)
 
     # Initial the LaserScan Intensities & Ranges
-    angle_increment = 2 * pi * step / 400
-    ranges = [0] * (FOV // step)
-    intensities = [0] * (FOV // step)
+    ranges = [0]
+    intensities = [0]
 
     # Center point coordinates
     center = (float(imgSize / 2), float(imgSize / 2))
@@ -165,11 +164,6 @@ def main():
         if updated:
             updateSonarConfig(sensor, gain, transmitFrequency,
                               transmitDuration, samplePeriod, numberOfSamples)
-
-            angle_increment = 2 * pi * step / 400
-            ranges = [0] * (FOV // step)
-            intensities = [0] * (FOV // step)
-
         # Get sonar response
         data = getSonarData(sensor, angle)
 
@@ -180,7 +174,6 @@ def main():
 
         # Prepare scan msg
         if enableScanTopic:
-            index = int(((angle - minAngle) * 2 * pi / 400) / angle_increment)
             # Get the first high intensity value
             for detectedIntensity in data:
                 if detectedIntensity >= threshold:
@@ -189,12 +182,12 @@ def main():
                     distance = calculateRange(
                         (1 + detectedIndex), samplePeriod, speedOfSound)
                     if distance >= 0.75 and distance <= sonarRange:
-                        ranges[index] = distance
-                        intensities[index] = detectedIntensity
+                        ranges[0] = distance
+                        intensities[0] = detectedIntensity
                         if debug:
                             print("Object at {} grad : {}m - {}%".format(angle,
-                                                                         ranges[index],
-                                                                         float(intensities[index] * 100 / 255)))
+                                                                         ranges[0],
+                                                                         float(intensities[0] * 100 / 255)))
                         break
             # Contruct and publish Sonar scan msg
             scanDataMsg = generateScanMsg(ranges, intensities, sonarRange, step, maxAngle, minAngle)
@@ -223,14 +216,14 @@ def main():
             publishImage(image, imagePub, bridge)
 
         angle += sign * step
-        if angle > maxAngle:
+        if angle >= maxAngle:
             if not oscillate:
                 angle = minAngle
             else:
                 angle = maxAngle
                 sign = -1
 
-        if angle < minAngle and oscillate:
+        if angle <= minAngle and oscillate:
             sign = 1
             angle = minAngle