Dev/sensors (#20)

* Added bar30 and ping360 nodes

* Added sonar code

* updated launchfile for bar30

* Updated with dos2unix

* added param for fluid den

* Fixed temp/depth reversal

Co-authored-by: Hank <hkmpham147@gmail.com>

launch/sensor.launch

@@ -0,0 +1,8 @@
+<launch>
+    <node name="bar30" pkg="wurov" type="bar30.py">
+        <rosparam>
+            rate: 5
+            fluidDensity: 997
+        </rosparam>
+    </node>
+</launch>

launch/sonar.launch

@@ -0,0 +1,9 @@
+<launch>
+    <node name="ping360" pkg="wurov" type="ping360.py">
+        <rosparam>
+            rate: 5
+            interface: "/dev/ttyUSB0"
+            baudrate: 115200
+        </rosparam>
+    </node>
+</launch>

plugins/sensors/bar30.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+
+import rospy
+import numpy as np
+import ms5837
+from std_msgs.msg import Float32
+
+
+class bar30:
+    def __init__(self):
+        """
+        bar30 node is used to get temperature and depth readings from a bar30 sensor
+        duration is taken from the launch file param server
+        """
+
+        rospy.init_node('temperature', anonymous=True)
+
+        self.depth_publisher = rospy.Publisher('vehicle/temperature', Float32, queue_size=3)
+        self.temp_publisher = rospy.Publisher('vehicle/depth', Float32, queue_size=3)
+
+        publishDuration = rospy.get_param(f"/{rospy.get_name()}/rate")
+
+        self.sensor = ms5837.MS5837_30BA()
+        self.sensor.init()
+
+        try:
+            fluidDensity = rospy.get_param(f"/{rospy.get_name()}/fluidDensity")
+            self.sensor.setFluidDensity(fluidDensity)
+        except Exception as e:
+            rospy.loginfo("No fluid density specified, using default")
+
+        rospy.Timer(rospy.Duration(publishDuration), self.publisher)
+
+        rospy.spin()
+
+    def publisher(self, data):
+        self.sensor.read()
+
+        self.temp_publisher.publish(self.sensor.depth())
+        self.depth_publisher.publish(self.sensor.temperature())
+
+if __name__ == '__main__':
+    bar30() 

plugins/sonar/ping360.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python3
+
+import rospy
+import numpy as np
+from brping import Ping360
+from sensor_msgs.msg import LaserScan
+import math
+
+class ping360:
+    def __init__(self):
+        """
+        ping360 node is used to get temperature and depth readings from a ping360 sensor
+        duration is taken from the launch file param server
+        """
+
+        rospy.init_node('temperature', anonymous=True)
+
+        self._publisher = rospy.Publisher('vehicle/ping360', LaserScan, queue_size=3)
+
+        publishDuration = rospy.get_param(f"/{rospy.get_name()}/rate")
+        interface = rospy.get_param(f"/{rospy.get_name()}/interface")
+        baudrate = rospy.get_param(f"/{rospy.get_name()}/baudrate")
+
+        self.sensor = Ping360()
+        self.sensor.connect_serial(interface, baudrate)
+        self.sensor.initialize()
+
+        rospy.Timer(rospy.Duration(publishDuration), self.publisher)
+
+        rospy.spin()
+
+    def publisher(self, data):
+        msg = LaserScan()
+        msg.angle_min = 0
+        msg.angle_max = 6.28319 #360 degrees
+        msg.angle_increment = 0.314159 #20 gradians
+
+        msg.ranges = []
+        data = []
+        for val in range(20):
+            scan = self.sensor.transmitAngle(val * 20) #angle in gradians
+            data = scan.data
+            msg.ranges.append(-1)
+            for detectedIntensity in data:
+                if detectedIntensity >= 200:
+                    detectedIndex = data.index(detectedIntensity)
+                    rangeVal = (1+detectedIndex)  * 1481 * 25e-9 * 80/ 2
+                    if rangeVal > 0.75:
+                        msg.ranges.pop()
+                        msg.ranges.append(rangeVal)
+                        break
+
+        self._publisher.publish(msg)
+
+if __name__ == '__main__':
+    ping360() 

requirements.txt

@@ -15,3 +15,6 @@ adafruit-circuitpython-servokit
 opencv-python
 numpy
 rospy
+bluerobotics-tsys01
+python-smbus
+bluerobotics-ping