From 83939becd63dcf934803caf773aa3e4860188561 Mon Sep 17 00:00:00 2001 From: Brennan <55165406+brennanmk@users.noreply.github.com> Date: Thu, 9 Jun 2022 17:41:37 -0400 Subject: [PATCH] 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 --- launch/sensor.launch | 8 +++ launch/sonar.launch | 9 +++ plugins/{sensors => camera}/camera0.py | 0 .../{sensors => camera}/camera_subscriber.py | 0 .../imu_data_fxas21002c_fxos8700.py | 0 plugins/{sensors => imu}/simulate_imu_data.py | 0 .../thruster}/thruster_sensor.py | 0 plugins/sensors/__init__.py | 0 plugins/sensors/bar30.py | 43 ++++++++++++++ plugins/sonar/ping360.py | 56 +++++++++++++++++++ requirements.txt | 3 + 11 files changed, 119 insertions(+) create mode 100644 launch/sensor.launch create mode 100644 launch/sonar.launch rename plugins/{sensors => camera}/camera0.py (100%) mode change 100755 => 100644 rename plugins/{sensors => camera}/camera_subscriber.py (100%) mode change 100755 => 100644 rename plugins/{sensors => imu}/imu_data_fxas21002c_fxos8700.py (100%) rename plugins/{sensors => imu}/simulate_imu_data.py (100%) rename plugins/{sensors => motors/thruster}/thruster_sensor.py (100%) delete mode 100644 plugins/sensors/__init__.py create mode 100755 plugins/sensors/bar30.py create mode 100755 plugins/sonar/ping360.py diff --git a/launch/sensor.launch b/launch/sensor.launch new file mode 100644 index 0000000..46d2160 --- /dev/null +++ b/launch/sensor.launch @@ -0,0 +1,8 @@ + + + + rate: 5 + fluidDensity: 997 + + + diff --git a/launch/sonar.launch b/launch/sonar.launch new file mode 100644 index 0000000..e37a7d7 --- /dev/null +++ b/launch/sonar.launch @@ -0,0 +1,9 @@ + + + + rate: 5 + interface: "/dev/ttyUSB0" + baudrate: 115200 + + + diff --git a/plugins/sensors/camera0.py b/plugins/camera/camera0.py old mode 100755 new mode 100644 similarity index 100% rename from plugins/sensors/camera0.py rename to plugins/camera/camera0.py diff --git a/plugins/sensors/camera_subscriber.py b/plugins/camera/camera_subscriber.py old mode 100755 new mode 100644 similarity index 100% rename from plugins/sensors/camera_subscriber.py rename to plugins/camera/camera_subscriber.py diff --git a/plugins/sensors/imu_data_fxas21002c_fxos8700.py b/plugins/imu/imu_data_fxas21002c_fxos8700.py similarity index 100% rename from plugins/sensors/imu_data_fxas21002c_fxos8700.py rename to plugins/imu/imu_data_fxas21002c_fxos8700.py diff --git a/plugins/sensors/simulate_imu_data.py b/plugins/imu/simulate_imu_data.py similarity index 100% rename from plugins/sensors/simulate_imu_data.py rename to plugins/imu/simulate_imu_data.py diff --git a/plugins/sensors/thruster_sensor.py b/plugins/motors/thruster/thruster_sensor.py similarity index 100% rename from plugins/sensors/thruster_sensor.py rename to plugins/motors/thruster/thruster_sensor.py diff --git a/plugins/sensors/__init__.py b/plugins/sensors/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/plugins/sensors/bar30.py b/plugins/sensors/bar30.py new file mode 100755 index 0000000..0d19161 --- /dev/null +++ b/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() diff --git a/plugins/sonar/ping360.py b/plugins/sonar/ping360.py new file mode 100755 index 0000000..9f21f7c --- /dev/null +++ b/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() diff --git a/requirements.txt b/requirements.txt index 9c5c86b..3434b39 100755 --- a/requirements.txt +++ b/requirements.txt @@ -15,3 +15,6 @@ adafruit-circuitpython-servokit opencv-python numpy rospy +bluerobotics-tsys01 +python-smbus +bluerobotics-ping