From a9752f6f73dcb640fbfae08ef7de2d55444cc5e1 Mon Sep 17 00:00:00 2001 From: Stormiix Date: Fri, 24 Jan 2020 10:42:14 +0100 Subject: [PATCH] add topics toggle parameters --- README.md | 11 +++++ launch/example.launch | 3 ++ src/ping360_sonar/node.py | 98 ++++++++++++++++++++++----------------- 3 files changed, 69 insertions(+), 43 deletions(-) diff --git a/README.md b/README.md index 3e4e7cc..71a1d83 100644 --- a/README.md +++ b/README.md @@ -64,6 +64,12 @@ Run the main node with: + Three extra parameters can be set to toggle specific topics, by default they all are set to true. + + + + + ## Nodes ### ping360_node @@ -78,6 +84,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]) @@ -91,6 +98,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]) @@ -106,6 +115,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 diff --git a/launch/example.launch b/launch/example.launch index 6af8215..c035829 100644 --- a/launch/example.launch +++ b/launch/example.launch @@ -14,5 +14,8 @@ + + + \ No newline at end of file diff --git a/src/ping360_sonar/node.py b/src/ping360_sonar/node.py index 20e5ad3..a614340 100644 --- a/src/ping360_sonar/node.py +++ b/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()