SW Sonar - Weber-State-Submarine-Project/Submarine GitHub Wiki
Software Implementation for Sonar Systems
This page documents the implementation details of the Front Sonar (Ping2), Side Sonar, and Down Sonar, including their roles, functionalities, and respective ROS 2 node implementations.
Front Sonar (Blue Robotics Ping2)
Description
The Front Sonar utilizes the Ping2 sensor from Blue Robotics for forward distance measurements. It publishes data to the /scan/front topic as a LaserScan message and to /scan/front/points as a PointCloud2 message. It also interacts with an /odom publisher for localization and a /turning subscriber to reset distance buffers during navigation turns.
Key Features:
- Publishes raw and averaged distance data.
- Implements a buffer for smoothing noisy sonar readings.
- Handles outlier readings and ensures robustness during turns.
Code:
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, PointCloud2, PointField
from std_msgs.msg import Header
import numpy as np
import struct
import sys
import site
import time
from std_msgs.msg import Int32
from nav_msgs.msg import Odometry
site.addsitedir('/home/submarine/.local/share/pipx/venvs/bluerobotics-ping/lib/python3.12/site-packages')
from brping import Ping1D
class PingSonarNode(Node):
def __init__(self):
super().__init__('ping_sonar_node')
self.scan_publisher = self.create_publisher(LaserScan, '/scan/front', 10)
self.pointcloud_publisher = self.create_publisher(PointCloud2, '/scan/front/points', 10)
self.odom_publisher = self.create_publisher(Odometry, 'odom', 10)
self.turning_subscriber = self.create_subscription(
Int32,
'/turning',
self.turning_callback,
10)
self.distance_buffer = [] # List of 5 valid Distance Data values
self.BUFFER_SIZE = 5
self.MAX_DISTANCE_JUMP = 1 # Meters
self.TURN_DISTANCE_THRESHOLD = 3 # Meters
self.STUCK_COUNT_LIMIT = 10
self.prev_distance = 0
self.turn_state = 0
self.stuck_count = 0
self.ping = Ping1D()
while True:
self.get_logger().info("Attempting to connect to Ping2...")
self.ping.connect_serial('/dev/ttyUSB0', 115200)
if self.ping.initialize():
self.get_logger().info("Successfully initialized Ping2.")
break
else:
self.get_logger().error("Failed to initialize Ping2! Retrying...")
time.sleep(5) # Wait for 5 seconds before retrying
self.ping.set_mode_auto(1)
#self.get_logger().info(f"Gain Val: {self.ping.get_gain_setting()}")
#self.get_logger().info(f"Range Val: {self.ping.get_range()}")
self.timer = self.create_timer(0.01, self.timer_callback)
def turning_callback(self,msg):
self.turn_state = msg.data
def timer_callback(self):
data = self.ping.get_distance()
if data:
distance = data['distance'] / 1000.0 # Convert mm to meters
prev_distance_lower = self.prev_distance - self.MAX_DISTANCE_JUMP
prev_distance_upper = self.prev_distance + self.MAX_DISTANCE_JUMP
# Reset Values when turning
if self.turn_state == 1 and distance < self.TURN_DISTANCE_THRESHOLD:
self.distance_buffer = []
self.prev_distance = 0
return
# Logic to determine if valid data
if prev_distance_lower < distance < prev_distance_upper or self.prev_distance == 0:
self.distance_buffer.append(distance)
else:
#self.get_logger().warn(f"Distance out of range --- Distance_read: {distance} Previous_Distance: {self.prev_distance} Max_Jump: {self.MAX_DISTANCE_JUMP}")
self.stuck_count += 1
# Corner Case: If sonar reads bad data for STUCK_COUNT_LIMIT, it will reset itself based on new distance
if self.stuck_count >= self.STUCK_COUNT_LIMIT:
self.distance_buffer = []
self.stuck_count = 0
self.prev_distance = distance
# Limits Buffer Size
if len(self.distance_buffer) > self.BUFFER_SIZE:
self.distance_buffer.pop(0)
# Logic to avg distance
if len(self.distance_buffer) == self.BUFFER_SIZE:
avg_distance = sum(self.distance_buffer) / self.BUFFER_SIZE
self.prev_distance = avg_distance
self.publish_scan(avg_distance)
self.publish_pointcloud(avg_distance)
#self.get_logger().info(f'Raw_distance: {distance} Avg_Distance {avg_distance}')
#else:
#self.get_logger().warn("Failed to get distance data from Ping2!")
def publish_scan(self, distance):
scan_msg = LaserScan()
scan_msg.header.stamp = self.get_clock().now().to_msg()
scan_msg.header.frame_id = 'front_sonar'
scan_msg.angle_min = 0.0
scan_msg.angle_max = 0.0
scan_msg.angle_increment = 0.0
scan_msg.time_increment = 0.0
scan_msg.scan_time = 0.1
scan_msg.range_min = 0.5
scan_msg.range_max = 100.0
scan_msg.ranges = [distance]
scan_msg.intensities = []
self.scan_publisher.publish(scan_msg)
def publish_pointcloud(self, distance):
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = 'front_sonar'
points = np.array([distance, 0.0, 0.0](/Weber-State-Submarine-Project/Submarine/wiki/distance,-0.0,-0.0), dtype=np.float32)
pointcloud_msg = PointCloud2()
pointcloud_msg.header = header
pointcloud_msg.height = 1
pointcloud_msg.width = points.shape[0]
pointcloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
]
pointcloud_msg.is_bigendian = False
pointcloud_msg.point_step = 12
pointcloud_msg.row_step = pointcloud_msg.point_step * points.shape[0]
pointcloud_msg.is_dense = True
pointcloud_msg.data = np.asarray(points, np.float32).tobytes()
self.pointcloud_publisher.publish(pointcloud_msg)
def main(args=None):
rclpy.init(args=args)
node = PingSonarNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Side Sonar (DFRobotics IP68 Sonar)
Description
The Side Sonar uses a DFRobotics IP68 sonar sensor to measure distances on the left side of the boat. It publishes data to the /scan/left topic as a LaserScan message and to /scan/left/points as a PointCloud2 message. The sonar operates via UART communication.
Key Features:
- Publishes real-time side distance data.
- Handles distance validation using checksum validation from the sensor.
- Provides fallback mechanisms for stale or invalid readings.
Code:
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, PointCloud2
from std_msgs.msg import Header
import serial
import time
import struct
from sensor_msgs_py import point_cloud2 as pc2
#import sensor_msgs.point_cloud2 as pc2
class SonarPublisher(Node):
def __init__(self):
super().__init__('sonar_publisher')
self.laser_publisher = self.create_publisher(LaserScan, '/scan/left', 10)
self.point_cloud_publisher = self.create_publisher(PointCloud2, '/scan/left/points', 10)
self.timer = self.create_timer(0.1, self.timer_callback)
self.ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=1)
self.COM = 0x55
self.buffer_RTT = [0, 0, 0, 0]
self.Distance = 0
self.Last_dist = 0
def timer_callback(self):
self.ser.write(bytes([self.COM]))
time.sleep(0.1)
if self.ser.in_waiting > 0:
time.sleep(0.004)
if self.ser.read() == b'\xff':
self.buffer_RTT[0] = 0xff
for i in range(1, 4):
self.buffer_RTT[i] = ord(self.ser.read())
CS = (self.buffer_RTT[0] + self.buffer_RTT[1] + self.buffer_RTT[2]) % 256
if self.buffer_RTT[3] == CS:
self.Distance = (self.buffer_RTT[1] << 8) + self.buffer_RTT[2]
distance_in_meters = self.Distance / 1000.0
if self.Distance > 0:
# Publish LaserScan message
laser_msg = LaserScan()
laser_msg.header.stamp = self.get_clock().now().to_msg()
laser_msg.header.frame_id = 'left_sonar'
laser_msg.angle_min = -0.0872665 # -5 degrees in radians
laser_msg.angle_max = 0.0872665 # 5 degrees in radians
laser_msg.angle_increment = 0.174533 # 10 degrees in radians
laser_msg.time_increment = 0.0
laser_msg.scan_time = 0.1
laser_msg.range_min = 0.02 # 2 cm
laser_msg.range_max = 6.0 # 600 cm
laser_msg.ranges = [distance_in_meters,distance_in_meters]
self.laser_publisher.publish(laser_msg)
# Publish PointCloud2 message
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = 'left_sonar'
points = [(distance_in_meters, 0.0, 0.0)]
point_cloud_msg = pc2.create_cloud_xyz32(header, points)
self.point_cloud_publisher.publish(point_cloud_msg)
self.Last_dist = distance_in_meters
else:
# Publish LaserScan message
laser_msg = LaserScan()
laser_msg.header.stamp = self.get_clock().now().to_msg()
laser_msg.header.frame_id = 'left_sonar'
laser_msg.angle_min = -0.0872665 # -5 degrees in radians
laser_msg.angle_max = 0.0872665 # 5 degrees in radians
laser_msg.angle_increment = 0.174533 # 10 degrees in radians
laser_msg.time_increment = 0.0
laser_msg.scan_time = 0.1
laser_msg.range_min = 0.02 # 2 cm
laser_msg.range_max = 6.0 # 600 cm
laser_msg.ranges = [self.Last_dist,self.Last_dist]
self.laser_publisher.publish(laser_msg)
# Publish PointCloud2 message
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = 'left_sonar'
points = [(self.Last_dist, 0.0, 0.0)]
point_cloud_msg = pc2.create_cloud_xyz32(header, points)
self.point_cloud_publisher.publish(point_cloud_msg)
#self.get_logger().info(f'Distance: {distance_in_meters:.3f}m')
def main(args=None):
rclpy.init(args=args)
sonar_publisher = SonarPublisher()
rclpy.spin(sonar_publisher)
sonar_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Down Sonar (DFRobotics IP68 Sonar)
Description
The Down Sonar uses a DFRobotics IP68 sonar sensor to measure depth directly beneath the boat. It publishes data to the /scan/down topic as a LaserScan message and to /scan/down/points as a PointCloud2 message. This sonar ensures accurate depth measurements for creating a 3D map of the pool floor.
Key Features:
- Publishes real-time depth data.
- Implements a reset mechanism for inconsistent readings.
- Ensures robust measurements by comparing current and previous values.
Code
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, PointCloud2
from std_msgs.msg import Header
import serial
import time
import struct
from sensor_msgs_py import point_cloud2 as pc2
#import sensor_msgs.point_cloud2 as pc2
class SonarPublisher(Node):
def __init__(self):
super().__init__('sonar_publisher')
self.laser_publisher = self.create_publisher(LaserScan, '/scan/down', 10)
self.point_cloud_publisher = self.create_publisher(PointCloud2, '/scan/down/points', 10)
self.timer = self.create_timer(0.1, self.timer_callback)
self.ser = serial.Serial('/dev/ttyUSB2', 115200, timeout=1)
self.COM = 0x55
self.buffer_RTT = [0, 0, 0, 0]
self.Distance = 0
self.prev_dist = None
self.reset_counter = 0
def timer_callback(self):
self.ser.write(bytes([self.COM]))
time.sleep(0.1)
if self.ser.in_waiting > 0:
time.sleep(0.004)
if self.ser.read() == b'\xff':
self.buffer_RTT[0] = 0xff
for i in range(1, 4):
self.buffer_RTT[i] = ord(self.ser.read())
CS = (self.buffer_RTT[0] + self.buffer_RTT[1] + self.buffer_RTT[2]) % 256
if self.buffer_RTT[3] == CS:
self.Distance = (self.buffer_RTT[1] << 8) + self.buffer_RTT[2]
distance_in_meters = self.Distance / 1000.0
diff = 100
if distance_in_meters !=0 and self.prev_dist == None:
self.prev_dist = distance_in_meters
if self.prev_dist != None:
diff = abs(distance_in_meters - self.prev_dist)
# Publish LaserScan message
if distance_in_meters != 0 and (diff <= 0.5 or self.reset_counter > 3):
reset_counter = 0
laser_msg = LaserScan()
laser_msg.header.stamp = self.get_clock().now().to_msg()
laser_msg.header.frame_id = 'down_sonar'
laser_msg.angle_min = -0.0872665 # -5 degrees in radians
laser_msg.angle_max = 0.0872665 # 5 degrees in radians
laser_msg.angle_increment = 0.174533 # 10 degrees in radians
laser_msg.time_increment = 0.0
laser_msg.scan_time = 0.1
laser_msg.range_min = 0.02 # 2 cm
laser_msg.range_max = 6.0 # 600 cm
laser_msg.ranges = [distance_in_meters]
self.laser_publisher.publish(laser_msg)
# Publish PointCloud2 message
header = Header()
header.stamp = self.get_clock().now().to_msg()
header.frame_id = 'down_sonar'
points = [(distance_in_meters, 0.0, 0.0)]
point_cloud_msg = pc2.create_cloud_xyz32(header, points)
self.point_cloud_publisher.publish(point_cloud_msg)
#self.get_logger().info(f'Distance: {distance_in_meters:.3f}m')
self.prev_dist = distance_in_meters
if diff >= 0.5:
self.reset_counter += 1
def main(args=None):
rclpy.init(args=args)
sonar_publisher = SonarPublisher()
rclpy.spin(sonar_publisher)
sonar_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()