Merge pull request #15 from CentraleNantesRobotics/develop

Feature: Ping360 for SLAM applications

.all-contributorsrc

@@ -24,6 +24,15 @@
         "code",
         "test"
       ]
+    },
+    {
+      "login": "tomlogan501",
+      "name": "tomlogan501",
+      "avatar_url": "https://avatars3.githubusercontent.com/u/56969577?v=4",
+      "profile": "https://github.com/tomlogan501",
+      "contributions": [
+        "ideas"
+      ]
     }
   ],
   "contributorsPerLine": 7,

README.md

@@ -91,6 +91,10 @@ While continuously rotating the sonar, it publishes two types of messages:
 		uint8 range      #  range value [m]
 		uint8[] intensities    # intensity data [0-255].  This is the actual data received from the sonar
 
+* **`/ping360_node/sonar/scan`** ([sensor_msgs/LaserScan])
+
+	Publishes a LaserScan msg with ranges detected with a certain intensity threshold.
+
 
 ## Bugs & Feature Requests
 
@@ -113,6 +117,7 @@ Please report bugs and request features using the [Issue Tracker](https://github
   <tr>
     <td align="center"><a href="https://github.com/Hameck"><img src="https://avatars2.githubusercontent.com/u/14954732?v=4" width="100px;" alt=""/><br /><sub><b>Henrique Martinez Rocamora</b></sub></a><br /><a href="https://github.com/CentraleNantesRobotics/ping360_sonar_python/commits?author=Hameck" title="Code">💻</a> <a href="https://github.com/CentraleNantesRobotics/ping360_sonar_python/commits?author=Hameck" title="Tests">⚠️</a></td>
     <td align="center"><a href="https://stormix.co"><img src="https://avatars2.githubusercontent.com/u/18377687?v=4" width="100px;" alt=""/><br /><sub><b>Anas Mazouni</b></sub></a><br /><a href="https://github.com/CentraleNantesRobotics/ping360_sonar_python/commits?author=Stormiix" title="Code">💻</a> <a href="https://github.com/CentraleNantesRobotics/ping360_sonar_python/commits?author=Stormiix" title="Tests">⚠️</a></td>
+    <td align="center"><a href="https://github.com/tomlogan501"><img src="https://avatars3.githubusercontent.com/u/56969577?v=4" width="100px;" alt=""/><br /><sub><b>tomlogan501</b></sub></a><br /><a href="#ideas-tomlogan501" title="Ideas, Planning, & Feedback">🤔</a></td>
   </tr>
 </table>
 

cfg/sonar.cfg

@@ -13,5 +13,6 @@ gen.add("transmitFrequency",    int_t,    0, "Transmit Frequency (kHz)", 740, 50
 gen.add("speedOfSound",    int_t,    0, "Speed of sound (m/s)", 1450, 1450, 1550)
 gen.add("step",    int_t,    0, "Filling Step", 1, 1,10)
 gen.add("queueSize",    int_t,    0, "Queue Size", 1, 0,10)
+gen.add("threshold", int_t, 0, "Intensity threshold", 200, 0, 255)
 
 exit(gen.generate(PACKAGE, "ping360_sonar", "sonar"))

launch/example.launch

@@ -12,5 +12,6 @@
         <param name="sonarRange" value="1"/>
         <param name="speedOfSound" value="1500"/>
         <param name="queueSize" value="1"/>
+        <param name="threshold" value="100"/>
     </node>
 </launch>

src/ping360_sonar/node.py

@@ -11,15 +11,15 @@
 from cv_bridge import CvBridge, CvBridgeError
 from dynamic_reconfigure.server import Server
 from sensor_msgs.msg import Image
-from std_msgs.msg import String
+from sensor_msgs.msg import LaserScan
 
 from ping360_sonar.cfg import sonarConfig
 from ping360_sonar.msg import SonarEcho
 from sensor import Ping360
 
 
 def callback(config, level):
-     global updated, gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, speedOfSound, samplePeriod, debug, step, imgSize, queue_size, p
+     global updated, gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, speedOfSound, samplePeriod, debug, step, imgSize, queue_size, p, threshold
      # Update Ping 360 Parameters
      gain = config['gain']
      numberOfSamples = config['numberOfSamples']
@@ -31,6 +31,7 @@ def callback(config, level):
      debug = config['debug']
      step = config['step']
      queue_size = config['queueSize']
+     threshold = config['threshold']
      rospy.loginfo("Reconfigure Request")
      updated = True
      return config
@@ -46,17 +47,23 @@ def main():
      # Topic publishers
      imagePub = rospy.Publisher("/ping360_node/sonar/images", Image, queue_size=queue_size)
      rawPub = rospy.Publisher("/ping360_node/sonar/data", SonarEcho, queue_size=queue_size)
+     laserPub = rospy.Publisher("/ping360_node/sonar/scan", LaserScan, queue_size=queue_size)
 
      # Initialize and configure the sonar
      updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples)
 
      # Create a new mono-channel image
      image = np.zeros((imgSize, imgSize, 1), np.uint8)
 
+     # Initial the LaserScan Intensities & Ranges
+     angle_increment = 2*pi*step / 400
+     ranges = [0]*(400 // step)
+     intensities = [0]*(400 // step)
+
      # Center point coordinates
      center = (float(imgSize/2),float(imgSize/2))
 
-     rate = rospy.Rate(100) # 10hz
+     rate = rospy.Rate(100) # 100hz
 
      while not rospy.is_shutdown():
 
@@ -71,6 +78,20 @@ def main():
           rawDataMsg = generateRawMsg(angle, data) # TODO: check for empty responses
           rawPub.publish(rawDataMsg)
 
+          # Contruct and publish Sonar scan msg
+          scanDataMsg = generateScanMsg(ranges, intensities) # TODO: check for empty responses
+          index = int(round((angle * 2 * pi / 400) / angle_increment))
+          detectedRange = next((x for x in data if x >= threshold), None) # Get the first high intensity value
+          if detectedRange:
+               detectedIndex = data.index(detectedRange) # Get the intensity index
+               distance = calculateRange((1 + detectedIndex), samplePeriod, speedOfSound) # The index+1 represents the number of samples which then can be used to deduce the range
+               # TODO: if distance >= 0.75 and distance <= sonarRange: 
+               ranges[index] = min(distance, ranges[index]) * 100 # In some instance where multiple measures are done in a single angle_increment, we take the closest one
+               intensities[index] = detectedRange if distance <= ranges[index] else intensities[index]
+               if debug:
+                    print("Detected object at {} grad : {}m - intensity {}%".format(angle, ranges[index], float(intensities[index]*100/255)))
+          laserPub.publish(scanDataMsg)
+
           # Contruct and publish Sonar image msg
           linear_factor = float(len(data)) / float(center[0]) #TODO: this should probably be range/pixelsize
           try:
@@ -128,13 +149,41 @@ def generateRawMsg(angle, data):
      msg.intensities = data
      return msg
 
+def generateScanMsg(ranges, intensities):
+     """
+     Generates the laserScan message for the scan topic
+     Params:
+          angle int (Gradian Angle)
+          data list List of intensities
+     """
+     msg = LaserScan()
+     msg.header.stamp = rospy.Time.now()
+     msg.header.frame_id = 'sonar_frame'
+     msg.angle_min = 0
+     msg.angle_max = 2 * pi
+     msg.angle_increment = 2*pi*step / 400
+     msg.time_increment = 0
+     msg.range_min = .75
+     msg.range_max = sonarRange
+     msg.ranges = ranges
+     msg.intensities = intensities
+
+     return msg
+
 def publishImage(image, imagePub, bridge):
      try:
           imagePub.publish(bridge.cv2_to_imgmsg(image, "mono8"))
      except CvBridgeError as e:
           rospy.logwarn("Failed to publish sensor image")
           print(e)
 
+def calculateRange(numberOfSamples, samplePeriod, speedOfSound, _samplePeriodTickDuration = 25e-9):
+     # type: (float, int, float, float) -> float
+     """
+      Calculate the range based in the duration
+     """
+     return numberOfSamples * speedOfSound * _samplePeriodTickDuration * samplePeriod / 2
+
 def calculateSamplePeriod(distance, numberOfSamples, speedOfSound, _samplePeriodTickDuration = 25e-9):
      # type: (float, int, int, float) -> float
      """
@@ -198,6 +247,8 @@ def updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, n
 transmitDuration = adjustTransmitDuration(sonarRange, samplePeriod, speedOfSound)
 debug = rospy.get_param('~debug', True)
 
+threshold = rospy.get_param('~threshold', 200) # 0-255
+
 # Output and ROS parameters
 step = rospy.get_param('~step', 1)
 imgSize = rospy.get_param('~imgSize', 500)