The original project plan aimed to modify a submarine kit to create a 3D map of a pool, detect cracks, and clean the pool. However, after the Project Management presentation, faculty feedback determined that the project scope was too ambitious to complete within the given timeline. As a result, the deliverables for cleaning the pool and crack detection were removed.

1. Initial Scope Adjustment:

   • After faculty feedback, the project focus shifted solely to mapping the pool, discarding the cleaning and crack detection objectives.
   • This revised scope was carried into Senior Project 1.

2. PDR (Preliminary Design Review):

   • During the PDR phase, the project was further simplified due to its remaining complexity.
   • It was determined that mapping could be accomplished without requiring the submarine to operate underwater, leading to the decision to move the project to the surface.

The project evolved into the Autonomous Pool Mapper Boat, a surface-level autonomous vehicle capable of mapping a pool in 3D. This pivot allowed for a more focused and achievable objective while still addressing the core goal of mapping confined water environments.

This section highlights the evolution of the project, documenting the challenges and decisions that led to the final, refined objective.

The servo controller was initially intended to manage fins for the submarine. However, after a design pivot, this component is no longer utilized in the current system. Despite its discontinuation, working with servos provided valuable experience in controlling devices using PWM signals, which directly informed our implementation of ESC motor control. This page preserves the archived documentation for reference and potential future applications.

The servo controller was implemented as an action node that subscribed to the topic servo_position to control servo movements. It accepted a float value between -1.0 and 1.0, translating this input into a corresponding servo position relative to its duty cycle.

To test the servo setup:

1. Connect the servo to GPIO pin 12.
2. Build and run both the servo and servo_client nodes.

Note: The system requires the installation of RPI.GPIO.

• The servo node was hardcoded for GPIO pin 12, designated for PWM output.
• Expansion for dual servo control was planned but not implemented.

For more details on the servo node and GPIO control in ROS 2, refer to this Stack Exchange resource.

This wiki page retains the important information about the servo controller while clearly marking it as outdated. It serves as a record of the original implementation and the lessons learned for future reference.

The Garmin speed sensor was originally intended to measure velocity to enhance robot localization. The sensor outputs analog data, which posed a challenge since the Raspberry Pi only has digital ports. To address this, an Arduino Nano was utilized as an analog-to-digital converter (ADC) to facilitate communication with the Raspberry Pi. A custom ROS package was developed to publish velocity data to the Odom topic, integrating it into the localization pipeline.

The image above shows the Garmin speed sensor that was integrated into the system but ultimately not utilized.

The image above shows the hardware diagram of how this was intended to be connected to the Pi 5

Despite the functional integration of the speed sensor and Arduino, the project encountered a critical limitation:

• The Garmin speed sensor relies on a water wheel to measure velocity.
• The boat’s operating speed was insufficient to spin the water wheel, resulting in no usable data being generated.
• As a result, the sensor failed to provide the required velocity measurements, leading to its discontinuation.

This experience highlighted the importance of matching sensor capabilities to the vehicle’s operating conditions. While the Garmin speed sensor was technically functional, its design and physical requirements made it unsuitable for the boat's slow speeds.

This page preserves the attempt to integrate the Garmin speed sensor while documenting the reasons for its failure, ensuring the knowledge can inform future projects.

The BNO085 orientation sensor, in addition to providing orientation data, also offered acceleration data. This led to the idea of utilizing it for robot localization by calculating velocity and distance traveled. However, despite initial attempts, this approach faced significant challenges.

Several issues were encountered during implementation:

• Noise in Data: The acceleration data showed significant noise even when the boat was stationary, causing the system to incorrectly estimate movement.
• Calibration Challenges: Attempts to calibrate the sensor for improved accuracy were unsuccessful, as the calibration process could not be completed following the provided instructions.
• False Localization: The noisy data resulted in the boat "believing" it had traveled when it remained stationary, rendering the data unreliable for localization purposes.

While the approach was ultimately discarded, the experience provided valuable insights:

• Noise Filtering: Implementing techniques such as a low-pass filter or a noise filter could potentially improve the reliability of the acceleration data.
• Sensor Calibration: Proper sensor calibration is crucial for ensuring data accuracy and system reliability.

Although the use of acceleration data for localization was not successful in this project, further experimentation with noise reduction techniques and calibration could make this approach viable. Future work could revisit this concept with more time to refine the implementation.

This page documents the attempt to utilize acceleration data for robot localization and the reasons for its failure, preserving the knowledge for future consideration.

The Maxbotix sonar sensors were originally selected for use on the submarine to provide distance measurements. However, during the Preliminary Design Review (PDR), it was discovered that these sensors were designed for air, not water, making them unsuitable for the intended underwater environment.

• Environmental Compatibility: The Maxbotix sonar sensors are optimized for air and do not natively function underwater.
• Missed Information: Although Maxbotix provides an article discussing potential methods for using their sonars underwater (see here), this information was not discovered until after the PDR presentation.
• Uncertain Performance: Even with the guidance provided in the article, there was no guarantee the sensors would perform reliably underwater.

This experience highlighted the importance of thoroughly verifying component specifications and their compatibility with the project’s operating environment early in the design process. It also emphasized the value of exploring all available documentation for hardware before finalizing decisions.

This section documents the challenges encountered with the Maxbotix sonar sensors and explains why they were ultimately not viable for the project.

This approach involved using the front sonar in combination with an IMU for robot localization and determining the boat's position relative to the pool walls. Although promising, it encountered issues during implementation that prevented reliable localization while turning.

1. Localization Errors During Turns:
   • Localization was disrupted when the boat executed 90-degree turns. The system struggled to maintain a consistent sense of position, causing erratic position updates.
2. Yaw Drift and Calibration:
   • The IMU’s yaw calibration required frequent resets, making it challenging to maintain accuracy over extended periods.
3. Complex Pose Calculation:
   • The calculation of x and y positions based on yaw and distance was sensitive to noise, especially when the boat was near walls or in dynamic water conditions.
4. Delayed Recovery:
   • After turns, the system took several seconds to stabilize and reset the origin for position calculations, reducing responsiveness.
5. Logical Errors:
   • The x, y positive and negative positions were reset after each 90 degree turn resulting in incorrect results
6. Simpler Solution:
   • In the end utilizing velocity was easier to manage using our robot_localization node which automatically detects the position using the direction from the IMU and velocity.

1. Localization Stability:
   • Combining sonar data with other sensors such as a downward sonar or external beacons might improve stability.
2. IMU Calibration:
   • Frequent resets and better filtering techniques are necessary to handle yaw drift and ensure precise orientation tracking.
3. Turning Dynamics:
   • The system needs a better way to compensate for position inaccuracies during and immediately after turns.

1. Advanced Filtering:
   • Applying a Kalman filter or particle filter could smooth out noisy inputs and enhance reliability.
2. Integrated Sensors:
   • Using additional sonar sensors or GPS for redundancy could improve localization during complex maneuvers.
3. Simplified Logic:
   • Revisiting pose calculation logic to reduce sensitivity to minor errors and noise could make the system more robust.
4. Fix Logic:
   • Rewrite x,y calculation to add to new starting x,y and change sign negation so it is done to calculate the change in x,y instead of the complete x,y

Below is the code used to implement this approach:

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "nav_msgs/msg/odometry.hpp"
#include "std_msgs/msg/int32.hpp"

class PoseNode : public rclcpp::Node
{
public:
    PoseNode()
    : Node("pose_node")
    {
        // Subscribe to the scan/front topic
        scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan/front", 10, std::bind(&PoseNode::scanCallback, this, std::placeholders::_1));

        // Subscribe to the /imu topic
        imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "/imu", 10, std::bind(&PoseNode::imuCallback, this, std::placeholders::_1));
        
        turn_sub_ = this->create_subscription<std_msgs::msg::Int32>(
            "/turning", 10, std::bind(&PoseNode::turnCallback, this, std::placeholders::_1));

        odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/odom", 10);
    }

private:
    void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        dist_ = msg->ranges[0];
        if(!got_origin_){
          origin_dist_ = dist_; 
          got_origin_ = true;
        }
        // Calculate the x, y only if we aren't turning 90 degrees
        if(!turning_){
            calcPose();
        }
    }

    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        tf2::Quaternion quat(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
        tf2::Matrix3x3 m(quat);
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);
        
        if (!yaw_initialized_) {
            initial_yaw_ = yaw;
            yaw_initialized_ = true;
        }
        // Raw yaw
        yaw_ = yaw;
        // yaw_scaled should always be 0 to pi/2 and be with respect to the wall it is following
        yaw_scaled_ = std::abs(std::abs(yaw) - std::abs(initial_yaw_));
    }

    void turnCallback(const std_msgs::msg::Int32::SharedPtr msg)
    {
        int turn = msg->data;
        // Comes from behavior tree, true when we are turning 90 degrees, stays on for [3] seconds after turn to allow sensors to correct
        if(turn){
            turning_ = true;
            origin_x_ = x_;
            origin_y_ = y_;
        }
        // Should be true a few seconds after the boat has finished turning 90 degree, resets the "start pos" to be the current x,y
        if(!turn && turning_){
            got_origin_ = false;
            yaw_initialized_ = false;
            turning_ = false;
        }
    }

    void calcPose()
    {
        float delta_dist = origin_dist_ - dist_;
        if((yaw_ > M_PI_4 && yaw_ < 3*M_PI_4) || (yaw_ < -M_PI_4 && yaw_ > -3*M_PI_4)){ 
            x_ = (std::sin(yaw_) * delta_dist) + origin_x_;
            y_ = (std::cos(yaw_) * delta_dist) + origin_y_;
        }

        else if((std::abs(yaw_) >= 3*M_PI_4) || (std::abs(yaw_) <= M_PI_4)){
            x_ = (std::cos(yaw_) * delta_dist) + origin_x_;
            y_ = (std::sin(yaw_) * delta_dist) + origin_y_;
        }

        if(yaw_ >= 0){
            y_ = -y_;
        }

        if(std::abs(yaw_) <= M_PI_2){
            x_ = -x_;
        }
        RCLCPP_INFO(this->get_logger(), "delta_d: %f, x:%f, y:%f, origin_x_: %f, origin_y_: %f yaw_: %f, scaled_yaw_: %f, initial_yaw_: %f, og_dist %f", delta_dist,x_,y_,origin_x_,origin_y_,yaw_,yaw_scaled_,initial_yaw_,origin_dist_);
        auto odom_msg = nav_msgs::msg::Odometry();
        odom_msg.header.stamp = this->now();
        odom_msg.header.frame_id = "odom";
        odom_msg.child_frame_id = "base_link";

        odom_msg.pose.pose.position.x = x_;  
        odom_msg.pose.pose.position.y = y_;
        odom_pub_->publish(odom_msg);      
    }

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;    
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr turn_sub_;    
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
    float dist_ = 0;
    float initial_yaw_ = 0;
    float yaw_ = 0;
    float yaw_scaled_ = 0;
    float origin_dist_ = 0;
    bool got_origin_ = false;
    bool yaw_initialized_ = false;
    bool turning_ = false;
    float x_ = 0;
    float y_ = 0;
    float origin_x_= 0;
    float origin_y_ = 0;

};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PoseNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

An issue was identified with the down sonar struggling to accurately measure distances when navigating the slope of a pool, such as transitioning from the shallow to the deep end. Due to budget constraints that prevented the use of a higher-quality sonar, a potential solution was devised involving the use of a lidar to calculate forward velocity, a low-cost sonar as a front sensor to detect approaching walls, and the Ping2 sonar for depth measurements, which demonstrated better performance on slope transitions.

The system was designed and integrated into the boat, ready for testing at Weber's Pool. During initial testing, the same issue was observed: the down sonar failed to collect data on the slope descent but succeeded on the ascent. Despite this limitation, the data collected was sufficient to generate a functional 3D map. Given the results, the lidar-based velocity solution was deemed unnecessary. Future implementations may consider revisiting this approach, but ideally, the system would rely solely on Ping2 sonars for improved consistency.

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')

import board
import busio
import adafruit_lidarlite

from brping import Ping1D

class PingSonarNode(Node):
    def __init__(self):
        super().__init__('ping_sonar_node')
        self.scan_publisher = self.create_publisher(LaserScan, '/scan', 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.STUCK_COUNT_LIMIT = 10
        self.prev_distance = 0
        self.turn_state = 0
        self.stuck_count = 0

        # Create library object using our Bus I2C port
        i2c = busio.I2C(board.SCL, board.SDA)

        # Default configuration, with only i2c wires
        self.sensor = adafruit_lidarlite.LIDARLite(i2c)

        self.timer = self.create_timer(0.01, self.timer_callback)
    
    def turning_callback(self,msg):
        self.turn_state = msg.data
        

    def timer_callback(self):
        try:
            distance = self.sensor.distance
        except RuntimeError as e:
            return

        if distance:
            distance = distance / 100.0  # Convert cm 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:
                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.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 main(args=None):
    rclpy.init(args=args)
    node = PingSonarNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()