Merge pull request #37 from CentraleNantesRobotics/develop

Added topic params

README.md

@@ -68,6 +68,13 @@ 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.
+
+        <param name="enableImageTopic" value="True"/> 
+        <param name="enableScanTopic" value="True"/> 
+        <param name="enableDataTopic" value="True"/>
+
 ## Nodes
 
 ### ping360_node
@@ -82,6 +89,7 @@ While continuously rotating the sonar, it publishes two types of messages:
 * **`/ping360_node/sonar/images`** ([sensor_msgs/Image])
 
 	The generated sonar image. 
+	This topic can be toggled using the **enableImageTopic** parameter.
 
 * **`/ping360_node/sonar/data`** ([msg/SonarEcho])
 
@@ -95,6 +103,8 @@ While continuously rotating the sonar, it publishes two types of messages:
 		uint16 speed_of_sound # [m/s]
 		uint8 range      #  range value [m]
 		uint8[] intensities    # intensity data [0-255].  This is the actual data received from the sonar
+
+	This topic can be toggled using the **enableDataTopic** parameter.
 
 * **`/ping360_node/sonar/scan`** ([sensor_msgs/LaserScan])
 
@@ -110,6 +120,8 @@ While continuously rotating the sonar, it publishes two types of messages:
 		float32[] ranges         # calculated ranges that correspond to an intensity > threshold [m] 
 		float32[] intensities    # sensor intensity data [0-255]
 
+	This topic can be toggled using the **enableScanTopic** parameter.
+
 	Note: 
 	- ranges values < range_min or > range_max should be discarded
 	- don't forget to set the frame to *sonar_frame* when using Rviz

launch/example.launch

@@ -14,5 +14,8 @@
         <param name="speedOfSound" value="1500"/>
         <param name="queueSize" value="1"/>
         <param name="threshold" value="100"/>
+        <param name="enableImageTopic" value="True"/> 
+        <param name="enableScanTopic" value="True"/> 
+        <param name="enableDataTopic" value="True"/> 
     </node>
 </launch>

src/ping360_sonar/node.py

@@ -32,6 +32,9 @@
 samplePeriod = None
 updated = False
 firstRequest = True
+enableImageTopic = False
+enableScanTopic = False
+enableDataTopic = False
 
 
 def callback(config, level):
@@ -60,7 +63,9 @@ def callback(config, level):
 
 def main():
     global updated, gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, \
-        speedOfSound, samplePeriod, debug, step, imgSize, queue_size, threshold
+        speedOfSound, samplePeriod, debug, step, imgSize, queue_size, threshold, \
+        enableDataTopic, enableImageTopic, enableScanTopic
+
     # Initialize node
     rospy.init_node('ping360_node')
 
@@ -79,6 +84,10 @@ def main():
     debug = rospy.get_param('~debug', True)
     threshold = int(rospy.get_param('~threshold', 200))  # 0-255
 
+    enableImageTopic = rospy.get_param('~enableImageTopic', True)
+    enableScanTopic = rospy.get_param('~enableScanTopic', True)
+    enableDataTopic = rospy.get_param('~enableDataTopic', True)
+
     # Output and ROS parameters
     step = int(rospy.get_param('~step', 1))
     imgSize = int(rospy.get_param('~imgSize', 500))
@@ -134,52 +143,55 @@ def main():
         data = getSonarData(sensor, angle)
 
         # Contruct and publish Sonar data msg
-        rawDataMsg = generateRawMsg(angle, data, gain, numberOfSamples, transmitFrequency, speedOfSound, sonarRange)
-        rawPub.publish(rawDataMsg)
+        if enableDataTopic:
+            rawDataMsg = generateRawMsg(angle, data, gain, numberOfSamples, transmitFrequency, speedOfSound, sonarRange)
+            rawPub.publish(rawDataMsg)
 
         # Prepare scan msg
-        index = int(round((angle * 2 * pi / 400) / angle_increment))
-
-        # Get the first high intensity value
-        for detectedIntensity in data:
-            if detectedIntensity >= threshold:
-                detectedIndex = data.index(detectedIntensity)
-                # The index+1 represents the number of samples which then can be used to deduce the range
-                distance = calculateRange(
-                    (1 + detectedIndex), samplePeriod, speedOfSound)
-                if distance >= 0.75 and distance <= sonarRange:
-                    ranges[index] = distance
-                    intensities[index] = detectedIntensity
-                    if debug:
-                        print("Object at {} grad : {}m - intensity {}%".format(angle,
-                                                                               ranges[index],
-                                                                               float(intensities[index] * 100 / 255)))
-                    break
-        # Contruct and publish Sonar scan msg
-        scanDataMsg = generateScanMsg(ranges, intensities, sonarRange, step)
-        laserPub.publish(scanDataMsg)
+        if enableScanTopic:
+            index = int(round((angle * 2 * pi / 400) / angle_increment))
+
+            # Get the first high intensity value
+            for detectedIntensity in data:
+                if detectedIntensity >= threshold:
+                    detectedIndex = data.index(detectedIntensity)
+                    # The index+1 represents the number of samples which then can be used to deduce the range
+                    distance = calculateRange(
+                        (1 + detectedIndex), samplePeriod, speedOfSound)
+                    if distance >= 0.75 and distance <= sonarRange:
+                        ranges[index] = distance
+                        intensities[index] = detectedIntensity
+                        if debug:
+                            print("Object at {} grad : {}m - {}%".format(angle,
+                                                                         ranges[index],
+                                                                         float(intensities[index] * 100 / 255)))
+                        break
+            # Contruct and publish Sonar scan msg
+            scanDataMsg = generateScanMsg(ranges, intensities, sonarRange, step)
+            laserPub.publish(scanDataMsg)
 
         # Contruct and publish Sonar image msg
-        linear_factor = float(len(data)) / float(center[0])
-        try:
-            # TODO: check the updated polar logic on the new ping-viewer
-            for i in range(int(center[0])):
-                if(i < center[0]):
-                    pointColor = data[int(i * linear_factor - 1)]
-                else:
-                    pointColor = 0
-                for k in np.linspace(0, step, 8 * step):
-                    theta = 2 * pi * (angle + k) / 400.0
-                    x = float(i) * cos(theta)
-                    y = float(i) * sin(theta)
-                    image[int(center[0] + x)][int(center[1] + y)
-                                              ][0] = pointColor
-        except IndexError:
-            rospy.logwarn(
-                "IndexError: data response was empty, skipping this iteration..")
-            continue
-
-        publishImage(image, imagePub, bridge)
+        if enableImageTopic:
+            linear_factor = float(len(data)) / float(center[0])
+            try:
+                # TODO: check the updated polar logic on the new ping-viewer
+                for i in range(int(center[0])):
+                    if(i < center[0]):
+                        pointColor = data[int(i * linear_factor - 1)]
+                    else:
+                        pointColor = 0
+                    for k in np.linspace(0, step, 8 * step):
+                        theta = 2 * pi * (angle + k) / 400.0
+                        x = float(i) * cos(theta)
+                        y = float(i) * sin(theta)
+                        image[int(center[0] + x)][int(center[1] + y)
+                                                  ][0] = pointColor
+            except IndexError:
+                rospy.logwarn(
+                    "IndexError: data response was empty, skipping this iteration..")
+                continue
+
+            publishImage(image, imagePub, bridge)
 
         angle = (angle + step) % 400  # TODO: allow users to set a scan FOV
         rate.sleep()