Merge pull request #19 from CentraleNantesRobotics/develop

Fixed bugs related to serial and headless devices
CentraleNantesRobotics · Jan 8, 2020 · fd688d2 · fd688d2
2 parents 0f4fb1e + cc35848
commit fd688d2
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 25 deletions.
diff --git a/launch/example.launch b/launch/example.launch
@@ -1,9 +1,9 @@
 <launch>
-    <env name="emulated_sonar" value="true" />
+    <env name="emulated_sonar" value="false" />
     <node pkg="ping360_sonar" type="ping360_node" name="ping360_node" output="screen">
         <param name="device" value="/dev/ttyUSB0"/>
         <param name="baudrate" value="115200"/>
-        <param name="debug" value="True"/>
+        <param name="debug" value="False"/>
         <param name="imgSize" value="500"/>
         <param name="gain" value="0"/>
         <param name="step" value="1"/>

diff --git a/src/ping360_sonar/node.py b/src/ping360_sonar/node.py
@@ -19,7 +19,7 @@
 
 
 def callback(config, level):
-     global updated, gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, speedOfSound, samplePeriod, debug, step, imgSize, queue_size, p, threshold
+     global updated, gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, speedOfSound, samplePeriod, debug, step, imgSize, queue_size, threshold
      # Update Ping 360 Parameters
      gain = config['gain']
      numberOfSamples = config['numberOfSamples']
@@ -37,6 +37,9 @@ def callback(config, level):
      return config
 
 def main():
+     sensor = Ping360(device, baudrate)
+     print("Initialized: %s" % sensor.initialize())
+
      rospy.init_node('ping360_node')
      srv = Server(sonarConfig, callback)
 
@@ -50,7 +53,7 @@ def main():
      laserPub = rospy.Publisher("/ping360_node/sonar/scan", LaserScan, queue_size=queue_size)
 
      # Initialize and configure the sonar
-     updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples)
+     updateSonarConfig(sensor, gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples)
 
      # Create a new mono-channel image
      image = np.zeros((imgSize, imgSize, 1), np.uint8)
@@ -66,13 +69,12 @@ def main():
      rate = rospy.Rate(100) # 100hz
 
      while not rospy.is_shutdown():
-
           # Update to the latest config data
           if updated:
-               updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples)
+               updateSonarConfig(sensor, gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples)
 
           # Get sonar response
-          data = getSonarData(angle)
+          data = getSonarData(sensor, angle)
 
           # Contruct and publish Sonar data msg
           rawDataMsg = generateRawMsg(angle, data) # TODO: check for empty responses
@@ -110,24 +112,20 @@ def main():
                continue
 
           angle = (angle + step) % 400
-          if debug:
-               cv2.imshow("PolarPlot",image)
-               cv2.waitKey(1)
-          else:
-               cv2.destroyAllWindows()
           publishImage(image, imagePub, bridge)
           rate.sleep()
 
-def getSonarData(angle):
+def getSonarData(sensor, angle):
      """
      Transmits the sonar angle and returns the sonar intensities
      Params:
+          sensor  Ping360 (Sensor class)
           angle int (Gradian Angle)
      Return:
           data list Intensities from 0 - 255 
      """
-     p.transmitAngle(angle)
-     data = bytearray(getattr(p,'_data'))
+     sensor.transmitAngle(angle)
+     data = bytearray(getattr(sensor,'_data'))
      return [k for k in data]
 
 def generateRawMsg(angle, data):
@@ -225,13 +223,13 @@ def getSamplePeriod(samplePeriod, _samplePeriodTickDuration = 25e-9):
      """  Sample period in ns """
      return samplePeriod * _samplePeriodTickDuration
 
-def updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples):
+def updateSonarConfig(sensor, gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples):
      global updated
-     p.set_gain_setting(gain)
-     p.set_transmit_frequency(transmitFrequency)
-     p.set_transmit_duration(transmitDuration)
-     p.set_sample_period(samplePeriod)
-     p.set_number_of_samples(numberOfSamples)
+     sensor.set_gain_setting(gain)
+     sensor.set_transmit_frequency(transmitFrequency)
+     sensor.set_transmit_duration(transmitDuration)
+     sensor.set_sample_period(samplePeriod)
+     sensor.set_number_of_samples(numberOfSamples)
      updated = False
 
 # Ping 360 Parameters
@@ -253,7 +251,4 @@ def updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, n
 step = rospy.get_param('~step', 1)
 imgSize = rospy.get_param('~imgSize', 500)
 queue_size= rospy.get_param('~queueSize', 1)
-
-p = Ping360(device, baudrate)
-updated = False
-print("Initialized: %s" % p.initialize())
+updated = False