From 10a6fe23559f2d2b6adb6928c4b13a8a128e7bbb Mon Sep 17 00:00:00 2001 From: Nikil-Shyamsunder Date: Sat, 23 Nov 2024 12:55:37 -0500 Subject: [PATCH 01/23] Added CV package boilerplate --- src/sailboat_vision/package.xml | 18 +++++++++++++ src/sailboat_vision/resource/sailboat_vision | 0 .../sailboat_vision/__init__.py | 0 src/sailboat_vision/setup.cfg | 4 +++ src/sailboat_vision/setup.py | 25 +++++++++++++++++++ src/sailboat_vision/test/test_copyright.py | 25 +++++++++++++++++++ src/sailboat_vision/test/test_flake8.py | 25 +++++++++++++++++++ src/sailboat_vision/test/test_pep257.py | 23 +++++++++++++++++ 8 files changed, 120 insertions(+) create mode 100644 src/sailboat_vision/package.xml create mode 100644 src/sailboat_vision/resource/sailboat_vision create mode 100644 src/sailboat_vision/sailboat_vision/__init__.py create mode 100644 src/sailboat_vision/setup.cfg create mode 100644 src/sailboat_vision/setup.py create mode 100644 src/sailboat_vision/test/test_copyright.py create mode 100644 src/sailboat_vision/test/test_flake8.py create mode 100644 src/sailboat_vision/test/test_pep257.py diff --git a/src/sailboat_vision/package.xml b/src/sailboat_vision/package.xml new file mode 100644 index 0000000..affba91 --- /dev/null +++ b/src/sailboat_vision/package.xml @@ -0,0 +1,18 @@ + + + + sailboat_vision + 0.0.0 + Computer Vision + Fiona Zheng + Apache License 2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/sailboat_vision/resource/sailboat_vision b/src/sailboat_vision/resource/sailboat_vision new file mode 100644 index 0000000..e69de29 diff --git a/src/sailboat_vision/sailboat_vision/__init__.py b/src/sailboat_vision/sailboat_vision/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/sailboat_vision/setup.cfg b/src/sailboat_vision/setup.cfg new file mode 100644 index 0000000..2a2cd16 --- /dev/null +++ b/src/sailboat_vision/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sailboat_vision +[install] +install_scripts=$base/lib/sailboat_vision diff --git a/src/sailboat_vision/setup.py b/src/sailboat_vision/setup.py new file mode 100644 index 0000000..f3c8b26 --- /dev/null +++ b/src/sailboat_vision/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'sailboat_vision' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='root', + maintainer_email='root@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/src/sailboat_vision/test/test_copyright.py b/src/sailboat_vision/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/sailboat_vision/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/sailboat_vision/test/test_flake8.py b/src/sailboat_vision/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/sailboat_vision/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/sailboat_vision/test/test_pep257.py b/src/sailboat_vision/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/sailboat_vision/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From af4cb411cc270060e1a8ef517a45178ce85fe83d Mon Sep 17 00:00:00 2001 From: Nikil-Shyamsunder Date: Sat, 23 Nov 2024 13:56:06 -0500 Subject: [PATCH 02/23] Update Docker image with CV commands --- setup/Dockerfile | 296 ++++++++++++++++++++--------------------- setup/requirements.txt | 2 + 2 files changed, 150 insertions(+), 148 deletions(-) diff --git a/setup/Dockerfile b/setup/Dockerfile index f3fd31a..1e96a1c 100644 --- a/setup/Dockerfile +++ b/setup/Dockerfile @@ -1,148 +1,148 @@ -############################################## -# Created from template ros2.dockerfile.jinja -############################################## - -########################################### -# Base image -########################################### -FROM ubuntu:22.04 AS base - -ENV DEBIAN_FRONTEND=noninteractive - -# Install language -RUN apt-get update && apt-get install -y \ - locales \ - && locale-gen en_US.UTF-8 \ - && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ - && rm -rf /var/lib/apt/lists/* -ENV LANG en_US.UTF-8 - -# Install timezone -RUN ln -fs /usr/share/zoneinfo/UTC /etc/localtime \ - && export DEBIAN_FRONTEND=noninteractive \ - && apt-get update \ - && apt-get install -y tzdata \ - && dpkg-reconfigure --frontend noninteractive tzdata \ - && rm -rf /var/lib/apt/lists/* - -RUN apt-get update && apt-get -y upgrade \ - && rm -rf /var/lib/apt/lists/* - -# Install common programs -RUN apt-get update && apt-get install -y --no-install-recommends \ - curl \ - gnupg2 \ - lsb-release \ - sudo \ - software-properties-common \ - wget \ - && rm -rf /var/lib/apt/lists/* - -# Install ROS2 -RUN sudo add-apt-repository universe \ - && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ - && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ - && apt-get update && apt-get install -y --no-install-recommends \ - ros-humble-ros-base \ - python3-argcomplete \ - && rm -rf /var/lib/apt/lists/* - -ENV ROS_DISTRO=humble -ENV AMENT_PREFIX_PATH=/opt/ros/humble -ENV COLCON_PREFIX_PATH=/opt/ros/humble -ENV LD_LIBRARY_PATH=/opt/ros/humble/lib -ENV PATH=/opt/ros/humble/bin:$PATH -ENV PYTHONPATH=/opt/ros/humble/local/lib/python3.10/dist-packages:/opt/ros/humble/lib/python3.10/site-packages -ENV ROS_PYTHON_VERSION=3 -ENV ROS_VERSION=2 -ENV DEBIAN_FRONTEND= - -########################################### -# Develop image -########################################### -FROM base AS dev - -ENV DEBIAN_FRONTEND=noninteractive -RUN apt-get update && apt-get install -y --no-install-recommends \ - bash-completion \ - build-essential \ - cmake \ - gdb \ - git \ - openssh-client \ - python3-argcomplete \ - python3-pip \ - ros-dev-tools \ - ros-humble-ament-* \ - vim \ - && rm -rf /var/lib/apt/lists/* - -RUN rosdep init || echo "rosdep already initialized" - -ARG USERNAME=ros -ARG USER_UID=1000 -ARG USER_GID=$USER_UID - -# Check if "ubuntu" user exists, delete it if it does, then create the desired user -RUN if getent passwd ubuntu > /dev/null 2>&1; then \ - userdel -r ubuntu && \ - echo "Deleted existing ubuntu user"; \ - fi && \ - groupadd --gid $USER_GID $USERNAME && \ - useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME && \ - echo "Created new user $USERNAME" - -# Add sudo support for the non-root user -RUN apt-get update && apt-get install -y sudo \ - && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ - && chmod 0440 /etc/sudoers.d/$USERNAME \ - && rm -rf /var/lib/apt/lists/* - -# Set up autocompletion for user -RUN apt-get update && apt-get install -y git-core bash-completion \ - && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \ - && echo "if [ -f /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash ]; then source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash; fi" >> /home/$USERNAME/.bashrc \ - && rm -rf /var/lib/apt/lists/* - -ENV DEBIAN_FRONTEND= -ENV AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 - -########################################### -# Full image -########################################### -FROM dev AS full - -ENV DEBIAN_FRONTEND=noninteractive -# Install the full release -RUN apt-get update && apt-get install -y --no-install-recommends \ - ros-humble-desktop \ - && rm -rf /var/lib/apt/lists/* -ENV DEBIAN_FRONTEND= -ENV LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib - -########################################### -# Additional steps for requirements.txt and setup_ros.sh -########################################### - -# Copy the requirements.txt and src -COPY requirements.txt /home/ros2_user/ros2_ws/ - -# Copy the setup script into the container -COPY setup_ros.sh /home/ros2_user/setup_ros.sh - -# Make the script executable -RUN chmod +x /home/ros2_user/setup_ros.sh - -# Install Necessary Packages -RUN pip3 install -r /home/ros2_user/ros2_ws/requirements.txt - -# Install RosBridge and other WebDev Packages -RUN sudo apt update \ -&& sudo apt install curl \ -&& sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo tee /usr/share/keyrings/ros-archive-keyring.gpg > /dev/null \ -&& echo "deb [signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ -&& sudo apt-get -y install ros-humble-rosbridge-server - -# Set the entry point to run the setup script on container start -ENTRYPOINT ["/home/ros2_user/setup_ros.sh"] - +############################################## +# Created from template ros2.dockerfile.jinja +############################################## + +########################################### +# Base image +########################################### +FROM ubuntu:22.04 AS base + +ENV DEBIAN_FRONTEND=noninteractive + +# Install language +RUN apt-get update && apt-get install -y \ + locales \ + && locale-gen en_US.UTF-8 \ + && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ + && rm -rf /var/lib/apt/lists/* +ENV LANG=en_US.UTF-8 + +# Install timezone +RUN ln -fs /usr/share/zoneinfo/UTC /etc/localtime \ + && export DEBIAN_FRONTEND=noninteractive \ + && apt-get update \ + && apt-get install -y tzdata \ + && dpkg-reconfigure --frontend noninteractive tzdata \ + && rm -rf /var/lib/apt/lists/* + +RUN apt-get update && apt-get -y upgrade \ + && rm -rf /var/lib/apt/lists/* + +# Install common programs +RUN apt-get update && apt-get install -y --no-install-recommends \ + curl \ + gnupg2 \ + lsb-release \ + sudo \ + software-properties-common \ + wget \ + && rm -rf /var/lib/apt/lists/* + +# Install ROS2 +RUN sudo add-apt-repository universe \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ + && apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-ros-base \ + python3-argcomplete \ + && rm -rf /var/lib/apt/lists/* + +ENV ROS_DISTRO=humble +ENV AMENT_PREFIX_PATH=/opt/ros/humble +ENV COLCON_PREFIX_PATH=/opt/ros/humble +ENV LD_LIBRARY_PATH=/opt/ros/humble/lib +ENV PATH=/opt/ros/humble/bin:$PATH +ENV PYTHONPATH=/opt/ros/humble/local/lib/python3.10/dist-packages:/opt/ros/humble/lib/python3.10/site-packages +ENV ROS_PYTHON_VERSION=3 +ENV ROS_VERSION=2 +ENV DEBIAN_FRONTEND= + +########################################### +# Develop image +########################################### +FROM base AS dev + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y --no-install-recommends \ + bash-completion \ + build-essential \ + cmake \ + gdb \ + git \ + openssh-client \ + python3-argcomplete \ + python3-pip \ + ros-dev-tools \ + ros-humble-ament-* \ + vim \ + && rm -rf /var/lib/apt/lists/* + +RUN rosdep init || echo "rosdep already initialized" + +ARG USERNAME=ros +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +# Check if "ubuntu" user exists, delete it if it does, then create the desired user +RUN if getent passwd ubuntu > /dev/null 2>&1; then \ + userdel -r ubuntu && \ + echo "Deleted existing ubuntu user"; \ + fi && \ + groupadd --gid $USER_GID $USERNAME && \ + useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME && \ + echo "Created new user $USERNAME" + +# Add sudo support for the non-root user +RUN apt-get update && apt-get install -y sudo \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + && rm -rf /var/lib/apt/lists/* + +# Set up autocompletion for user +RUN apt-get update && apt-get install -y git-core bash-completion \ + && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \ + && echo "if [ -f /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash ]; then source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash; fi" >> /home/$USERNAME/.bashrc \ + && rm -rf /var/lib/apt/lists/* + +ENV DEBIAN_FRONTEND= +ENV AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 + +########################################### +# Full image +########################################### +FROM dev AS full + +ENV DEBIAN_FRONTEND=noninteractive +# Install the full release +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-desktop \ + && rm -rf /var/lib/apt/lists/* +ENV DEBIAN_FRONTEND= +ENV LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib + +########################################### +# Additional steps for requirements.txt and setup_ros.sh +########################################### + +# Copy the requirements.txt and src +COPY requirements.txt /home/ros2_user/ros2_ws/ + +# Copy the setup script into the container +COPY setup_ros.sh /home/ros2_user/setup_ros.sh + +# Make the script executable +RUN chmod +x /home/ros2_user/setup_ros.sh + +# Install Necessary Packages +RUN pip3 install -r /home/ros2_user/ros2_ws/requirements.txt + +# Install RosBridge and other WebDev Packages +RUN sudo apt-get -y update \ +&& sudo apt-get -y install curl \ +&& sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo tee /usr/share/keyrings/ros-archive-keyring.gpg > /dev/null \ +&& echo "deb [signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ +&& sudo apt-get -y install ros-humble-rosbridge-server + +# Set the entry point to run the setup script on container start +ENTRYPOINT ["/home/ros2_user/setup_ros.sh"] + diff --git a/setup/requirements.txt b/setup/requirements.txt index 458d33d..a482cb8 100644 --- a/setup/requirements.txt +++ b/setup/requirements.txt @@ -3,3 +3,5 @@ pyserial==3.5 utm==0.7.0 geopy==2.4.1 rosdoc2 +opencv-python==4.8.0.74 + From 47d9ccd02da5c92fa88df4da2bf0ef1d4201bfde Mon Sep 17 00:00:00 2001 From: fz246 Date: Sat, 23 Nov 2024 14:01:07 -0500 Subject: [PATCH 03/23] initial buoy detection webcam code --- src/sailboat_launch/config/config_sim.yaml | 1 - .../sailboat_vision/buoy_detection.py | 56 ++++++++++++++ .../sailboat_vision/camera_node.py | 74 +++++++++++++++++++ src/sailboat_vision/setup.py | 1 + 4 files changed, 131 insertions(+), 1 deletion(-) create mode 100644 src/sailboat_vision/sailboat_vision/buoy_detection.py create mode 100644 src/sailboat_vision/sailboat_vision/camera_node.py diff --git a/src/sailboat_launch/config/config_sim.yaml b/src/sailboat_launch/config/config_sim.yaml index 497a4b3..5112eee 100644 --- a/src/sailboat_launch/config/config_sim.yaml +++ b/src/sailboat_launch/config/config_sim.yaml @@ -16,4 +16,3 @@ sailbot: #Namespace radio: ros__parameters: simulated: true - diff --git a/src/sailboat_vision/sailboat_vision/buoy_detection.py b/src/sailboat_vision/sailboat_vision/buoy_detection.py new file mode 100644 index 0000000..86ee190 --- /dev/null +++ b/src/sailboat_vision/sailboat_vision/buoy_detection.py @@ -0,0 +1,56 @@ +import cv2 +import numpy as np + +class BuoyDetectorCV: + def __init__(self, hsv_lower=None, hsv_upper=None, detection_threshold=100): + # Initialize parameters with defaults or provided values + self.hsv_lower = np.array(hsv_lower if hsv_lower else [0, 120, 180]) + self.hsv_upper = np.array(hsv_upper if hsv_upper else [10, 160, 255]) + self.detection_threshold = detection_threshold + + def update_parameters(self, hsv_lower=None, hsv_upper=None, detection_threshold=None): + """Update parameters dynamically.""" + if hsv_lower: + self.hsv_lower = np.array(hsv_lower) + if hsv_upper: + self.hsv_upper = np.array(hsv_upper) + if detection_threshold is not None: + self.detection_threshold = detection_threshold + + def increase_contrast(self, frame): + """Enhance frame contrast.""" + lab = cv2.cvtColor(frame, cv2.COLOR_BGR2LAB) + l, a, b = cv2.split(lab) + clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8)) + l_clahe = clahe.apply(l) + lab_clahe = cv2.merge((l_clahe, a, b)) + return cv2.cvtColor(lab_clahe, cv2.COLOR_LAB2BGR) + + def reduce_glare(self, frame): + """Reduce glare from the frame.""" + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + _, glare_mask = cv2.threshold(blurred, 220, 255, cv2.THRESH_BINARY) + glare_mask_inv = cv2.bitwise_not(glare_mask) + return cv2.bitwise_and(frame, frame, mask=glare_mask_inv) + + def process_frame(self, frame): + """Process a single frame to detect the buoy.""" + frame = self.reduce_glare(frame) + frame = self.increase_contrast(frame) + + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(hsv, self.hsv_lower, self.hsv_upper) + + kernel = np.ones((5, 5), np.uint8) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) + + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if contours: + largest_contour = max(contours, key=cv2.contourArea) + if cv2.contourArea(largest_contour) > self.detection_threshold: + x, y, w, h = cv2.boundingRect(largest_contour) + buoy_center = (x + w // 2, y + h // 2) + return buoy_center, frame + return None, frame diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py new file mode 100644 index 0000000..0e3a3c7 --- /dev/null +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -0,0 +1,74 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Point +from buoy_detection import BuoyDetectorCV +import cv2 + +class BuoyDetectorNode(Node): + def __init__(self): + super().__init__('buoy_detector_node') + + self.declare_parameter('video_source', 0) + self.declare_parameter('hsv_lower', [0, 120, 180]) + self.declare_parameter('hsv_upper', [10, 160, 255]) + self.declare_parameter('detection_threshold', 100) + self.declare_parameter('timer_period', 0.1) # Timer period for frame processing + + self.video_source = self.get_parameter('video_source').value + hsv_lower = self.get_parameter('hsv_lower').value + hsv_upper = self.get_parameter('hsv_upper').value + detection_threshold = self.get_parameter('detection_threshold').value + self.timer_period = self.get_parameter('timer_period').value + + self.detector = BuoyDetectorCV(hsv_lower, hsv_upper, detection_threshold) + + self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) + + # OpenCV video capture + self.cap = cv2.VideoCapture(self.video_source) + if not self.cap.isOpened(): + self.get_logger().error(f'Failed to open video source {self.video_source}') + raise RuntimeError(f'Cannot open video source {self.video_source}') + + self.timer = self.create_timer(self.timer_period, self.process_frame) + + self.get_logger().info('Buoy Detector Node Initialized') + + def process_frame(self): + """Process a single frame and publish the buoy position.""" + ret, frame = self.cap.read() + if not ret: + self.get_logger().error('Failed to read frame from video source') + return + + buoy_center, _ = self.detector.process_frame(frame) + + if buoy_center: + point_msg = Point() + point_msg.x, point_msg.y, point_msg.z = buoy_center[0], buoy_center[1], 0.0 + self.position_publisher.publish(point_msg) + self.get_logger().info(f'Detected buoy at: {buoy_center}') + + def update_detector_parameters(self): + """Update the CV detector parameters dynamically.""" + hsv_lower = self.get_parameter('hsv_lower').value + hsv_upper = self.get_parameter('hsv_upper').value + detection_threshold = self.get_parameter('detection_threshold').value + + self.detector.update_parameters(hsv_lower, hsv_upper, detection_threshold) + self.get_logger().info(f"Updated parameters: HSV Lower: {hsv_lower}, HSV Upper: {hsv_upper}, Detection Threshold: {detection_threshold}") + + def destroy_node(self): + """Release the video capture and destroy the node.""" + self.cap.release() + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = BuoyDetectorNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/sailboat_vision/setup.py b/src/sailboat_vision/setup.py index f3c8b26..b1a86d1 100644 --- a/src/sailboat_vision/setup.py +++ b/src/sailboat_vision/setup.py @@ -20,6 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'buoy_detection = sailboat_vision.buoy_detection.camera_node:main' ], }, ) From f325c6a38cafba491b7c18086af0ff44f0d076c9 Mon Sep 17 00:00:00 2001 From: fz246 Date: Sat, 23 Nov 2024 14:04:53 -0500 Subject: [PATCH 04/23] update config file for buoy detection --- src/sailboat_launch/config/config.yaml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/sailboat_launch/config/config.yaml b/src/sailboat_launch/config/config.yaml index 065147b..b9b006a 100644 --- a/src/sailboat_launch/config/config.yaml +++ b/src/sailboat_launch/config/config.yaml @@ -12,3 +12,9 @@ sailbot: #Namespace ros__parameters: teensy_port: "/dev/ttyS8" simulated: false + computer_vision: + ros__parameters: + video_source: 0 + hsv_lower: [0, 120, 180] + hsv_upper: [10, 160, 255] + detection_threshold: 100 \ No newline at end of file From ed41309bd91081c4b166cea5f1dca3f180499123 Mon Sep 17 00:00:00 2001 From: asun121 Date: Sat, 23 Nov 2024 14:11:53 -0500 Subject: [PATCH 05/23] Initial CV tracker --- .../src/Control Tasks/TrackerControlTask.cpp | 32 +++++++++++++++++++ .../src/Control Tasks/TrackerControlTask.hpp | 20 ++++++++++++ teensy/src/MainControlLoop.cpp | 4 ++- teensy/src/MainControlLoop.hpp | 2 ++ teensy/src/Monitors/SerialMonitor.cpp | 1 + teensy/src/constants.hpp | 5 +++ teensy/src/sfr.cpp | 2 +- teensy/src/sfr.hpp | 1 + 8 files changed, 65 insertions(+), 2 deletions(-) create mode 100644 teensy/src/Control Tasks/TrackerControlTask.cpp create mode 100644 teensy/src/Control Tasks/TrackerControlTask.hpp diff --git a/teensy/src/Control Tasks/TrackerControlTask.cpp b/teensy/src/Control Tasks/TrackerControlTask.cpp new file mode 100644 index 0000000..c9bca59 --- /dev/null +++ b/teensy/src/Control Tasks/TrackerControlTask.cpp @@ -0,0 +1,32 @@ +#include "TrackerControlTask.hpp" + +TrackerControlTask::TrackerControlTask() +{ + tracker_servo.attach(constants::servo::TRACKER_PIN); +} + +void TrackerControlTask::execute() +{ + if (sfr::serial::update_servos) + { + if( sfr::buoy_displacement > 0) + { + actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MAX_ANGLE)); + } + else + { + actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MIN_ANGLE)); + } + } +} + + +uint32_t ServoControlTask::angle_to_pwm(uint8_t angle) +{ + return map(angle, 0, 90, 1050, 1300); // TODO: find out what these numbers mean +} + +void ServoControlTask::actuate_servo(Servo &servo, uint32_t pwm) +{ + servo.write(pwm); +} \ No newline at end of file diff --git a/teensy/src/Control Tasks/TrackerControlTask.hpp b/teensy/src/Control Tasks/TrackerControlTask.hpp new file mode 100644 index 0000000..09a72f2 --- /dev/null +++ b/teensy/src/Control Tasks/TrackerControlTask.hpp @@ -0,0 +1,20 @@ +#pragma once +#include + +#include "constants.hpp" +#include "sfr.hpp" + +class TrackerControlTask +{ +public: + TrackerControlTask(); + void execute(); + +} + +private: + uint32_t angle_to_pwm(uint8_t angle); + void actuate_servo(Servo &servo, uint32_t pwm); + + Servo tracker_servo; +}; \ No newline at end of file diff --git a/teensy/src/MainControlLoop.cpp b/teensy/src/MainControlLoop.cpp index 988eab4..25904b5 100644 --- a/teensy/src/MainControlLoop.cpp +++ b/teensy/src/MainControlLoop.cpp @@ -4,7 +4,8 @@ MainControlLoop::MainControlLoop() : anemometer_monitor(), serial_monitor(), servo_control_task(), - serial_control_task() + serial_control_task(), + tracker_control_task() { delay(1000); } @@ -15,4 +16,5 @@ void MainControlLoop::execute() serial_monitor.execute(); servo_control_task.execute(); serial_control_task.execute(); + tracker_control_task.execute(); } diff --git a/teensy/src/MainControlLoop.hpp b/teensy/src/MainControlLoop.hpp index c54d7fd..9bbdf27 100644 --- a/teensy/src/MainControlLoop.hpp +++ b/teensy/src/MainControlLoop.hpp @@ -4,6 +4,7 @@ #include "Monitors/SerialMonitor.hpp" #include "Control Tasks/ServoControlTask.hpp" #include "Control Tasks/SerialControlTask.hpp" +//#include "Control Tasks/TrackerControlTask.hpp" #include "sfr.hpp" @@ -18,4 +19,5 @@ class MainControlLoop SerialMonitor serial_monitor; ServoControlTask servo_control_task; SerialControlTask serial_control_task; + // TrackerControlTask tracker_control_task; }; \ No newline at end of file diff --git a/teensy/src/Monitors/SerialMonitor.cpp b/teensy/src/Monitors/SerialMonitor.cpp index 2df3cf3..0cb827c 100644 --- a/teensy/src/Monitors/SerialMonitor.cpp +++ b/teensy/src/Monitors/SerialMonitor.cpp @@ -25,6 +25,7 @@ void SerialMonitor::execute() sfr::serial::update_servos = true; sfr::servo::sail_angle = sfr::serial::buffer[0]; sfr::servo::rudder_angle = sfr::serial::buffer[1]; + sfr::serial::buoy_displacement = sfr::serial::buffer[2]; } else if (packet_started) { diff --git a/teensy/src/constants.hpp b/teensy/src/constants.hpp index d531ab8..b546d4a 100644 --- a/teensy/src/constants.hpp +++ b/teensy/src/constants.hpp @@ -17,6 +17,11 @@ namespace constants constexpr uint8_t RUDDER_MIN_ANGLE = 0; constexpr uint8_t RUDDER_MAX_ANGLE = 90; + + constexpr uint8_t TRACKER_PIN = 5; //TODO: change to correct pin number + constexpr uint8_t TRACKER_MIN_ANGLE = 0; + constexpr uint8_t TRACKER_MAX_ANGLE = 90; + } namespace serial { diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp index 2c722e1..e3ba0e9 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -16,7 +16,7 @@ namespace sfr namespace serial { bool update_servos = false; - + int8_t buoy_displacement = 0; uint8_t dropped_packets = 0; uint8_t buffer[2] = {0}; diff --git a/teensy/src/sfr.hpp b/teensy/src/sfr.hpp index bcdd7e8..c0db6a7 100644 --- a/teensy/src/sfr.hpp +++ b/teensy/src/sfr.hpp @@ -22,6 +22,7 @@ namespace sfr { extern bool update_servos; extern bool send_telemetry; + extern int8_t buoy_displacement; extern uint8_t dropped_packets; From 6486ba32515279cb093d448e437269e81d76c5ea Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 22 Feb 2025 12:40:49 -0500 Subject: [PATCH 06/23] Adjust buoy tracking with servo --- .../sailboat_main/teensy/teensy.py | 5 ++-- .../sailboat_vision/buoy_detection.py | 13 +++++++++-- .../sailboat_vision/camera_node.py | 18 +++++++++------ .../src/Control Tasks/SerialControlTask.cpp | 1 + .../src/Control Tasks/TrackerControlTask.cpp | 23 ++++++++++++++----- .../src/Control Tasks/TrackerControlTask.hpp | 5 ++-- teensy/src/MainControlLoop.hpp | 4 ++-- teensy/src/sfr.cpp | 1 + teensy/src/sfr.hpp | 1 + 9 files changed, 49 insertions(+), 22 deletions(-) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy.py b/src/sailboat_main/sailboat_main/teensy/teensy.py index 3c73868..3824ad2 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy.py @@ -73,7 +73,7 @@ def _parse_packet(self, packet): return wind_angle, sail_angle, rudder_angle, dropped_packets - def send_command(self, sail, rudder): + def send_command(self, sail, rudder, buoy_displacement): """ Send a properly formatted command packet to the servo. @@ -88,9 +88,10 @@ def send_command(self, sail, rudder): # convert sail and tail to signed 8-bit integers (bytes) sail_byte = sail & 0xFF if sail >= 0 else (sail + 256) & 0xFF rudder_byte = rudder & 0xFF if rudder >= 0 else (rudder + 256) & 0xFF + buoy_displacement_byte = buoy_displacement & 0xFF if buoy_displacement >= 0 else (buoy_displacement + 256) & 0xFF # create the packet: [start flag] [sail] [tail] [end flag] - command_packet = bytearray([self.START_BYTE, sail_byte, rudder_byte, self.END_BYTE]) + command_packet = bytearray([self.START_BYTE, sail_byte, rudder_byte, buoy_displacement_byte, self.END_BYTE]) # send the packet over serial self.serial.write(command_packet) diff --git a/src/sailboat_vision/sailboat_vision/buoy_detection.py b/src/sailboat_vision/sailboat_vision/buoy_detection.py index 86ee190..4cf4fa5 100644 --- a/src/sailboat_vision/sailboat_vision/buoy_detection.py +++ b/src/sailboat_vision/sailboat_vision/buoy_detection.py @@ -7,6 +7,8 @@ def __init__(self, hsv_lower=None, hsv_upper=None, detection_threshold=100): self.hsv_lower = np.array(hsv_lower if hsv_lower else [0, 120, 180]) self.hsv_upper = np.array(hsv_upper if hsv_upper else [10, 160, 255]) self.detection_threshold = detection_threshold + CENTER = 0 # TODO: Actually find the center and margin of error + MARGIN = 100 def update_parameters(self, hsv_lower=None, hsv_upper=None, detection_threshold=None): """Update parameters dynamically.""" @@ -51,6 +53,13 @@ def process_frame(self, frame): largest_contour = max(contours, key=cv2.contourArea) if cv2.contourArea(largest_contour) > self.detection_threshold: x, y, w, h = cv2.boundingRect(largest_contour) - buoy_center = (x + w // 2, y + h // 2) - return buoy_center, frame + buoy_center = x + w // 2 + direction = 0 + if buoy_center > self.CENTER + self.MARGIN: + direction = 1 + elif buoy_center < self.CENTER - self.MARGIN: + direction = -1 + return direction, frame + # buoy_center = (x + w // 2, y + h // 2) + # return buoy_center, frame return None, frame diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index 0e3a3c7..50ff36f 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -2,6 +2,7 @@ from rclpy.node import Node from geometry_msgs.msg import Point from buoy_detection import BuoyDetectorCV +from std_msgs.msg import Int32 import cv2 class BuoyDetectorNode(Node): @@ -22,7 +23,7 @@ def __init__(self): self.detector = BuoyDetectorCV(hsv_lower, hsv_upper, detection_threshold) - self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) + self.position_publisher = self.create_publisher(Int32, '/buoy_position', 10) # OpenCV video capture self.cap = cv2.VideoCapture(self.video_source) @@ -35,19 +36,22 @@ def __init__(self): self.get_logger().info('Buoy Detector Node Initialized') def process_frame(self): + """Process a single frame and publish the buoy position.""" ret, frame = self.cap.read() if not ret: self.get_logger().error('Failed to read frame from video source') return - buoy_center, _ = self.detector.process_frame(frame) + direction, _ = self.detector.process_frame(frame) - if buoy_center: - point_msg = Point() - point_msg.x, point_msg.y, point_msg.z = buoy_center[0], buoy_center[1], 0.0 - self.position_publisher.publish(point_msg) - self.get_logger().info(f'Detected buoy at: {buoy_center}') + if direction is not None: + self.position_publisher.publish(direction) + self.get_logger().info(f'Detected buoy at: {direction}') + # point_msg = Point() + # point_msg.x, point_msg.y, point_msg.z = buoy_center[0], buoy_center[1], 0.0 + # self.position_publisher.publish(point_msg) + # self.get_logger().info(f'Detected buoy at: {buoy_center}') def update_detector_parameters(self): """Update the CV detector parameters dynamically.""" diff --git a/teensy/src/Control Tasks/SerialControlTask.cpp b/teensy/src/Control Tasks/SerialControlTask.cpp index fc91434..00fcc85 100644 --- a/teensy/src/Control Tasks/SerialControlTask.cpp +++ b/teensy/src/Control Tasks/SerialControlTask.cpp @@ -20,6 +20,7 @@ void SerialControlTask::execute() sfr::anemometer::wind_angle, sfr::servo::sail_angle, sfr::servo::rudder_angle, + sfr::serial::buoy_angle, sfr::serial::dropped_packets, constants::serial::TX_END_FLAG}; diff --git a/teensy/src/Control Tasks/TrackerControlTask.cpp b/teensy/src/Control Tasks/TrackerControlTask.cpp index c9bca59..622777b 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.cpp +++ b/teensy/src/Control Tasks/TrackerControlTask.cpp @@ -9,24 +9,35 @@ void TrackerControlTask::execute() { if (sfr::serial::update_servos) { - if( sfr::buoy_displacement > 0) + if ( sfr::serial::buoy_displacement > 0) { - actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MAX_ANGLE)); + actuate_servo(tracker_servo, read_servo(tracker_servo) + 10); + // actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MAX_ANGLE)); } - else + else if (sfr::serial::buoy_displacement < 0) { - actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MIN_ANGLE)); + actuate_servo(tracker_servo, read_servo(tracker_servo) - 10); + // actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MIN_ANGLE)); + } + { + actuate_servo(tracker_servo, read_servo(tracker_servo)) + sfr::serial::buoy_angle = read_servo(tracker_servo) } } } -uint32_t ServoControlTask::angle_to_pwm(uint8_t angle) +uint32_t TrackerControlTask::angle_to_pwm(uint8_t angle) { return map(angle, 0, 90, 1050, 1300); // TODO: find out what these numbers mean } -void ServoControlTask::actuate_servo(Servo &servo, uint32_t pwm) +void TrackerControlTask::actuate_servo(Servo &servo, uint32_t pwm) { servo.write(pwm); +} + +uint8_t TrackerControlTask::read_servo(Servo &servo) +{ + return servo.read() } \ No newline at end of file diff --git a/teensy/src/Control Tasks/TrackerControlTask.hpp b/teensy/src/Control Tasks/TrackerControlTask.hpp index 09a72f2..a7f64be 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.hpp +++ b/teensy/src/Control Tasks/TrackerControlTask.hpp @@ -10,11 +10,10 @@ class TrackerControlTask TrackerControlTask(); void execute(); -} - private: uint32_t angle_to_pwm(uint8_t angle); - void actuate_servo(Servo &servo, uint32_t pwm); + uint32_t actuate_servo(Servo &servo, uint32_t pwm); + uint8_t read_servo(Servo &servo) Servo tracker_servo; }; \ No newline at end of file diff --git a/teensy/src/MainControlLoop.hpp b/teensy/src/MainControlLoop.hpp index 9bbdf27..3bb27fb 100644 --- a/teensy/src/MainControlLoop.hpp +++ b/teensy/src/MainControlLoop.hpp @@ -4,7 +4,7 @@ #include "Monitors/SerialMonitor.hpp" #include "Control Tasks/ServoControlTask.hpp" #include "Control Tasks/SerialControlTask.hpp" -//#include "Control Tasks/TrackerControlTask.hpp" +#include "Control Tasks/TrackerControlTask.hpp" #include "sfr.hpp" @@ -19,5 +19,5 @@ class MainControlLoop SerialMonitor serial_monitor; ServoControlTask servo_control_task; SerialControlTask serial_control_task; - // TrackerControlTask tracker_control_task; + TrackerControlTask tracker_control_task; }; \ No newline at end of file diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp index e3ba0e9..bdb380b 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -18,6 +18,7 @@ namespace sfr bool update_servos = false; int8_t buoy_displacement = 0; uint8_t dropped_packets = 0; + uint8_t buoy_angle = 0; uint8_t buffer[2] = {0}; } diff --git a/teensy/src/sfr.hpp b/teensy/src/sfr.hpp index c0db6a7..5f030b1 100644 --- a/teensy/src/sfr.hpp +++ b/teensy/src/sfr.hpp @@ -23,6 +23,7 @@ namespace sfr extern bool update_servos; extern bool send_telemetry; extern int8_t buoy_displacement; + extern int8_t buoy_angle extern uint8_t dropped_packets; From 62974af35691e04b48b487edbb8cbe31bc28c25e Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 22 Feb 2025 12:46:31 -0500 Subject: [PATCH 07/23] Fix bugs with teensy cpp --- teensy/src/Control Tasks/TrackerControlTask.cpp | 6 +++--- teensy/src/Control Tasks/TrackerControlTask.hpp | 4 ++-- teensy/src/sfr.cpp | 2 +- teensy/src/sfr.hpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/teensy/src/Control Tasks/TrackerControlTask.cpp b/teensy/src/Control Tasks/TrackerControlTask.cpp index 622777b..116df62 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.cpp +++ b/teensy/src/Control Tasks/TrackerControlTask.cpp @@ -20,8 +20,8 @@ void TrackerControlTask::execute() // actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MIN_ANGLE)); } { - actuate_servo(tracker_servo, read_servo(tracker_servo)) - sfr::serial::buoy_angle = read_servo(tracker_servo) + actuate_servo(tracker_servo, read_servo(tracker_servo)); + sfr::serial::buoy_angle = read_servo(tracker_servo); } } } @@ -39,5 +39,5 @@ void TrackerControlTask::actuate_servo(Servo &servo, uint32_t pwm) uint8_t TrackerControlTask::read_servo(Servo &servo) { - return servo.read() + return servo.read(); } \ No newline at end of file diff --git a/teensy/src/Control Tasks/TrackerControlTask.hpp b/teensy/src/Control Tasks/TrackerControlTask.hpp index a7f64be..deb5e45 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.hpp +++ b/teensy/src/Control Tasks/TrackerControlTask.hpp @@ -12,8 +12,8 @@ class TrackerControlTask private: uint32_t angle_to_pwm(uint8_t angle); - uint32_t actuate_servo(Servo &servo, uint32_t pwm); - uint8_t read_servo(Servo &servo) + void actuate_servo(Servo &servo, uint32_t pwm); + uint8_t read_servo(Servo &servo); Servo tracker_servo; }; \ No newline at end of file diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp index bdb380b..51bc1d0 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -18,7 +18,7 @@ namespace sfr bool update_servos = false; int8_t buoy_displacement = 0; uint8_t dropped_packets = 0; - uint8_t buoy_angle = 0; + int8_t buoy_angle = 0; uint8_t buffer[2] = {0}; } diff --git a/teensy/src/sfr.hpp b/teensy/src/sfr.hpp index 5f030b1..514a4c3 100644 --- a/teensy/src/sfr.hpp +++ b/teensy/src/sfr.hpp @@ -23,7 +23,7 @@ namespace sfr extern bool update_servos; extern bool send_telemetry; extern int8_t buoy_displacement; - extern int8_t buoy_angle + extern int8_t buoy_angle; extern uint8_t dropped_packets; From 15bca8394282b896f693da57ddaef1c56a20562b Mon Sep 17 00:00:00 2001 From: Nicole Luo Date: Sat, 22 Feb 2025 13:17:20 -0500 Subject: [PATCH 08/23] added buoy displacement info to telemetry data --- .../sailboat_main/teensy/teensy.py | 1 + .../sailboat_main/teensy/teensy_node.py | 39 ++++++++++++++++--- .../src/Control Tasks/SerialControlTask.cpp | 1 - .../src/Control Tasks/TrackerControlTask.cpp | 5 +-- 4 files changed, 35 insertions(+), 11 deletions(-) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy.py b/src/sailboat_main/sailboat_main/teensy/teensy.py index 3824ad2..6efe374 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy.py @@ -79,6 +79,7 @@ def send_command(self, sail, rudder, buoy_displacement): :param sail: Sail position (integer) :param tail: Tail position (integer) + :param buoy_displacement: Buoy displacement (integer) """ try: # check bounds diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_node.py b/src/sailboat_main/sailboat_main/teensy/teensy_node.py index 42489b9..9bf9758 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_node.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_node.py @@ -2,7 +2,8 @@ from rclpy.node import Node from sailboat_interface.msg import SailTail from std_msgs.msg import Int32 - +#import statement may or may not be necessary +from geometry_msgs.msg import Point from . import teensy from . import teensy_fake @@ -44,11 +45,13 @@ def __init__(self): # keep track of our desired sail and rudder angles self.desired_sail = 0 self.desired_rudder = 0 + self.desired_buoy_displacement = 0 # telemetry data publishers self.wind_angle_pub = self.create_publisher(Int32, 'wind', 10) self.actual_sail_angle_pub = self.create_publisher(Int32, 'actual_sail_angle', 10) self.actual_rudder_angle_pub = self.create_publisher(Int32, 'actual_rudder_angle', 10) + self.actual_buoy_displacement_pub = self.create_publisher(Int32, 'actual_buoy_displacement', 10) self.dropped_packets_pub = self.create_publisher(Int32, 'dropped_packets', 10) # callback to read teensy data @@ -66,6 +69,13 @@ def __init__(self): 'rudder_angle', self.rudder_callback, 10) + + #subscription for buoy displacement + self.subscription = self.create_subscription( + Int32, + '/buoy_displacement', + self.buoy_displacement_callback, + 10) def check_telemetry(self): """ @@ -87,6 +97,10 @@ def check_telemetry(self): rudder_angle_msg.data = data["rudder_angle"] self.actual_rudder_angle_pub.publish(rudder_angle_msg) + buoy_displacement_msg = Int32() + buoy_displacement_msg.data = data["buoy_displacement"] + self.actual_buoy_displacement_pub.publish(buoy_displacement_msg) + dropped_packets_msg = Int32() dropped_packets_msg.data = data["dropped_packets"] self.dropped_packets_pub.publish(dropped_packets_msg) @@ -104,22 +118,35 @@ def sail_callback(self, msg): previously set rudder. """ self.desired_sail = msg.data - self.get_logger().info(f"Sail callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder}") + self.get_logger().info(f"Sail position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.desired_buoy_displacement}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.desired_buoy_displacement) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") - + def rudder_callback(self, msg): """ Callback function for the 'rudder_angle' topic. Sends the updated rudder and the previously set sail. """ self.desired_rudder = msg.data - self.get_logger().info(f"Rudder callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder}") + self.get_logger().info(f"Rudder position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.desired_buoy_displacement}") + + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.desired_buoy_displacement) == 0: + self.get_logger().info(f"Message sent to servo") + else: + self.get_logger().warn(f"Message failed to send to servo") + + def buoy_displacement_callback(self, msg): + """ + Callback function for the 'buoy_displacement' topic. Sends the updated buoy position, previously set rudder, + and the previously set sail. + """ + self.desired_buoy_displacement = msg.data + self.get_logger().info(f"Buoy position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.desired_buoy_displacement}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.desired_buoy_displacement) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") diff --git a/teensy/src/Control Tasks/SerialControlTask.cpp b/teensy/src/Control Tasks/SerialControlTask.cpp index 00fcc85..577eb29 100644 --- a/teensy/src/Control Tasks/SerialControlTask.cpp +++ b/teensy/src/Control Tasks/SerialControlTask.cpp @@ -29,5 +29,4 @@ void SerialControlTask::execute() send_telemetry = false; } current_time = millis(); - } \ No newline at end of file diff --git a/teensy/src/Control Tasks/TrackerControlTask.cpp b/teensy/src/Control Tasks/TrackerControlTask.cpp index 116df62..0fe18d8 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.cpp +++ b/teensy/src/Control Tasks/TrackerControlTask.cpp @@ -9,15 +9,13 @@ void TrackerControlTask::execute() { if (sfr::serial::update_servos) { - if ( sfr::serial::buoy_displacement > 0) + if (sfr::serial::buoy_displacement > 0) { actuate_servo(tracker_servo, read_servo(tracker_servo) + 10); - // actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MAX_ANGLE)); } else if (sfr::serial::buoy_displacement < 0) { actuate_servo(tracker_servo, read_servo(tracker_servo) - 10); - // actuate_servo(tracker_servo, angle_to_pwm(constants::servo::TRACKER_MIN_ANGLE)); } { actuate_servo(tracker_servo, read_servo(tracker_servo)); @@ -26,7 +24,6 @@ void TrackerControlTask::execute() } } - uint32_t TrackerControlTask::angle_to_pwm(uint8_t angle) { return map(angle, 0, 90, 1050, 1300); // TODO: find out what these numbers mean From 7cec45ac20028fa4dd693d2cb02651c457762323 Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 1 Mar 2025 12:33:22 -0500 Subject: [PATCH 09/23] Add calculation for buoy z based on angle. --- .../sailboat_vision/buoy_detection.py | 13 +----- .../sailboat_vision/camera_node.py | 41 +++++++++++++++---- 2 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/sailboat_vision/sailboat_vision/buoy_detection.py b/src/sailboat_vision/sailboat_vision/buoy_detection.py index 4cf4fa5..86ee190 100644 --- a/src/sailboat_vision/sailboat_vision/buoy_detection.py +++ b/src/sailboat_vision/sailboat_vision/buoy_detection.py @@ -7,8 +7,6 @@ def __init__(self, hsv_lower=None, hsv_upper=None, detection_threshold=100): self.hsv_lower = np.array(hsv_lower if hsv_lower else [0, 120, 180]) self.hsv_upper = np.array(hsv_upper if hsv_upper else [10, 160, 255]) self.detection_threshold = detection_threshold - CENTER = 0 # TODO: Actually find the center and margin of error - MARGIN = 100 def update_parameters(self, hsv_lower=None, hsv_upper=None, detection_threshold=None): """Update parameters dynamically.""" @@ -53,13 +51,6 @@ def process_frame(self, frame): largest_contour = max(contours, key=cv2.contourArea) if cv2.contourArea(largest_contour) > self.detection_threshold: x, y, w, h = cv2.boundingRect(largest_contour) - buoy_center = x + w // 2 - direction = 0 - if buoy_center > self.CENTER + self.MARGIN: - direction = 1 - elif buoy_center < self.CENTER - self.MARGIN: - direction = -1 - return direction, frame - # buoy_center = (x + w // 2, y + h // 2) - # return buoy_center, frame + buoy_center = (x + w // 2, y + h // 2) + return buoy_center, frame return None, frame diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index 50ff36f..ac01da1 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -1,3 +1,4 @@ +import math import rclpy from rclpy.node import Node from geometry_msgs.msg import Point @@ -14,6 +15,11 @@ def __init__(self): self.declare_parameter('hsv_upper', [10, 160, 255]) self.declare_parameter('detection_threshold', 100) self.declare_parameter('timer_period', 0.1) # Timer period for frame processing + + + self.CENTER = 300 # TODO: Actually find the center and margin of error + self.MARGIN = 30 + self.angle = 0 self.video_source = self.get_parameter('video_source').value hsv_lower = self.get_parameter('hsv_lower').value @@ -23,6 +29,7 @@ def __init__(self): self.detector = BuoyDetectorCV(hsv_lower, hsv_upper, detection_threshold) + self.displacement_publisher = self.create_publisher(Int32, '/buoy_displacement', 10) self.position_publisher = self.create_publisher(Int32, '/buoy_position', 10) # OpenCV video capture @@ -33,6 +40,12 @@ def __init__(self): self.timer = self.create_timer(self.timer_period, self.process_frame) + self.subscription = self.create_subscription( + Int32, + 'buoy_angle', + self.buoy_angle_callback, + 10) + self.get_logger().info('Buoy Detector Node Initialized') def process_frame(self): @@ -43,15 +56,27 @@ def process_frame(self): self.get_logger().error('Failed to read frame from video source') return - direction, _ = self.detector.process_frame(frame) + buoy_center, _ = self.detector.process_frame(frame) - if direction is not None: - self.position_publisher.publish(direction) - self.get_logger().info(f'Detected buoy at: {direction}') - # point_msg = Point() - # point_msg.x, point_msg.y, point_msg.z = buoy_center[0], buoy_center[1], 0.0 - # self.position_publisher.publish(point_msg) - # self.get_logger().info(f'Detected buoy at: {buoy_center}') + displacement = 0 + if not buoy_center: + return + elif buoy_center[0] > self.CENTER + self.MARGIN: + displacement = 1 + elif buoy_center[0] < self.CENTER - self.MARGIN: + displacement = -1 + if displacement: + self.displacement_publisher.publish(displacement) + self.get_logger().info(f'Detected buoy displacement: {displacement}') + else: + point_msg = Point() + point_msg.x, point_msg.y = buoy_center[0], buoy_center[1] + point_msg.z = abs(point_msg.x - self.CENTER) / math. atan(self.angle) + self.position_publisher.publish(point_msg) + self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') + + def buoy_angle_callback(self, msg): + self.angle = abs(90 - msg.data) def update_detector_parameters(self): """Update the CV detector parameters dynamically.""" From c743bafb9931227890a6ee4f36fe6c1a4b7aec60 Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Tue, 11 Mar 2025 17:21:27 -0400 Subject: [PATCH 10/23] Fix camera node bugs --- .../sailboat_vision/camera_node.py | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index ac01da1..b72c25f 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -10,13 +10,18 @@ class BuoyDetectorNode(Node): def __init__(self): super().__init__('buoy_detector_node') - self.declare_parameter('video_source', 0) + # --network="host" + # ffmpeg -f avfoundation -list_devices true -i "" + # ffmpeg -f avfoundation -video_device_index 3 -framerate 30 -i "3" -f mpegts udp://localhost:8080 + # self.declare_parameter('video_source', "udp://localhost:8080?overrun_nonfatal=1fifo_size=50000000") + # self.declare_parameter('timer_period', 0.03) + + self.declare_parameter('video_source', 4) self.declare_parameter('hsv_lower', [0, 120, 180]) self.declare_parameter('hsv_upper', [10, 160, 255]) self.declare_parameter('detection_threshold', 100) self.declare_parameter('timer_period', 0.1) # Timer period for frame processing - self.CENTER = 300 # TODO: Actually find the center and margin of error self.MARGIN = 30 self.angle = 0 @@ -30,7 +35,7 @@ def __init__(self): self.detector = BuoyDetectorCV(hsv_lower, hsv_upper, detection_threshold) self.displacement_publisher = self.create_publisher(Int32, '/buoy_displacement', 10) - self.position_publisher = self.create_publisher(Int32, '/buoy_position', 10) + self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) # OpenCV video capture self.cap = cv2.VideoCapture(self.video_source) @@ -57,21 +62,26 @@ def process_frame(self): return buoy_center, _ = self.detector.process_frame(frame) + cv2.imwrite("frame.jpg", frame) + self.get_logger().info(f'Saved frame') displacement = 0 if not buoy_center: + self.get_logger().info(f'No buoy detected') return elif buoy_center[0] > self.CENTER + self.MARGIN: displacement = 1 elif buoy_center[0] < self.CENTER - self.MARGIN: displacement = -1 if displacement: - self.displacement_publisher.publish(displacement) + displacement_msg = Int32() + displacement_msg.data = displacement + self.displacement_publisher.publish(displacement_msg) self.get_logger().info(f'Detected buoy displacement: {displacement}') else: point_msg = Point() - point_msg.x, point_msg.y = buoy_center[0], buoy_center[1] - point_msg.z = abs(point_msg.x - self.CENTER) / math. atan(self.angle) + point_msg.x, point_msg.y, = map(float, buoy_center) + point_msg.z = 0.0 # TODO: Make this real based on self.angle self.position_publisher.publish(point_msg) self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') From aa12d314122b4c1d2c1fa9e8d03451c0e4ac81b0 Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 15 Mar 2025 16:32:27 -0400 Subject: [PATCH 11/23] Add function to align depth and color image --- .../sailboat_vision/camera_node.py | 112 +++++++++++++----- 1 file changed, 85 insertions(+), 27 deletions(-) diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index b72c25f..b5a63b1 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -1,10 +1,12 @@ +import cv2 import math +import pyrealsense2 as rs import rclpy from rclpy.node import Node from geometry_msgs.msg import Point -from buoy_detection import BuoyDetectorCV from std_msgs.msg import Int32 -import cv2 + +from buoy_detection import BuoyDetectorCV class BuoyDetectorNode(Node): def __init__(self): @@ -22,10 +24,6 @@ def __init__(self): self.declare_parameter('detection_threshold', 100) self.declare_parameter('timer_period', 0.1) # Timer period for frame processing - self.CENTER = 300 # TODO: Actually find the center and margin of error - self.MARGIN = 30 - self.angle = 0 - self.video_source = self.get_parameter('video_source').value hsv_lower = self.get_parameter('hsv_lower').value hsv_upper = self.get_parameter('hsv_upper').value @@ -38,10 +36,48 @@ def __init__(self): self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) # OpenCV video capture - self.cap = cv2.VideoCapture(self.video_source) - if not self.cap.isOpened(): - self.get_logger().error(f'Failed to open video source {self.video_source}') - raise RuntimeError(f'Cannot open video source {self.video_source}') + # self.cap = cv2.VideoCapture(self.video_source) + # if not self.cap.isOpened(): + # self.get_logger().error(f'Failed to open video source {self.video_source}') + # raise RuntimeError(f'Cannot open video source {self.video_source}') + + # ret, frame = self.cap.read() + # if not ret: + # self.get_logger().error('Failed to read frame from video source') + # raise RuntimeError(f'Failed to read frame from video source') + + # Create a pipeline + self.pipeline = rs.pipeline() + + # Create a config and configure the pipeline to stream + # different resolutions of color and depth streams + config = rs.config() + # config.enable_device_from_file("stairs.bag") + + + # Get device product line for setting a supporting resolution + pipeline_wrapper = rs.pipeline_wrapper(pipeline) + pipeline_profile = config.resolve(pipeline_wrapper) + device = pipeline_profile.get_device() + device_product_line = str(device.get_info(rs.camera_info.product_line)) + + config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) + config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) + + # Start streaming + profile = self.pipeline.start(config) + + # Getting the depth sensor's depth scale (see rs-align example for explanation) + depth_sensor = profile.get_device().first_depth_sensor() + depth_scale = depth_sensor.get_depth_scale() + print("Depth Scale is: " , depth_scale) + + align_to = rs.stream.color + self.align = rs.align(align_to) + + self.CENTER = 0 # frame.shape[1] / 2 + self.MARGIN = 30 # TODO: Find best value for margin of error + self.angle = 0 self.timer = self.create_timer(self.timer_period, self.process_frame) @@ -51,42 +87,64 @@ def __init__(self): self.buoy_angle_callback, 10) + + frames = self.pipeline.wait_for_frames() + aligned_frames = self.align.process(frames) + self.get_logger().info('Buoy Detector Node Initialized') def process_frame(self): - """Process a single frame and publish the buoy position.""" - ret, frame = self.cap.read() - if not ret: - self.get_logger().error('Failed to read frame from video source') + frames = self.pipeline.wait_for_frames() + # frames.get_depth_frame() is a 640x360 depth image + + # Align the depth frame to color frame + aligned_frames = self.align.process(frames) + + # Get aligned frames + aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image + color_frame = aligned_frames.get_color_frame() + + # Validate that both frames are valid + if not aligned_depth_frame or not color_frame: return - buoy_center, _ = self.detector.process_frame(frame) - cv2.imwrite("frame.jpg", frame) - self.get_logger().info(f'Saved frame') + depth_image = np.asanyarray(aligned_depth_frame.get_data()) + color_image = np.asanyarray(color_frame.get_data()) + + """Process a single frame and publish the buoy position.""" + # ret, frame = self.cap.read() + # if not ret: + # self.get_logger().error('Failed to read frame from video source') + # return + + buoy_center, _ = self.detector.process_frame(color_image) - displacement = 0 if not buoy_center: self.get_logger().info(f'No buoy detected') return - elif buoy_center[0] > self.CENTER + self.MARGIN: + + if buoy_center[0] > self.CENTER + self.MARGIN: displacement = 1 elif buoy_center[0] < self.CENTER - self.MARGIN: displacement = -1 - if displacement: - displacement_msg = Int32() - displacement_msg.data = displacement - self.displacement_publisher.publish(displacement_msg) - self.get_logger().info(f'Detected buoy displacement: {displacement}') else: + depth = 0.0 # TODO: Make this real based on self.angle point_msg = Point() - point_msg.x, point_msg.y, = map(float, buoy_center) - point_msg.z = 0.0 # TODO: Make this real based on self.angle + point_msg.x = self.CENTER + (depth * math.sin(self.angle)) + point_msg.y = float(buoy_center[1]) + point_msg.z = depth * math.cos(self.angle) self.position_publisher.publish(point_msg) self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') + return + + displacement_msg = Int32() + displacement_msg.data = displacement + self.displacement_publisher.publish(displacement_msg) + self.get_logger().info(f'Detected buoy displacement: {displacement}') def buoy_angle_callback(self, msg): - self.angle = abs(90 - msg.data) + self.angle = msg.data - 90 def update_detector_parameters(self): """Update the CV detector parameters dynamically.""" From ec7be3c005349b85b89f2bb075b8c5022e116a5b Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Tue, 18 Mar 2025 17:54:59 -0400 Subject: [PATCH 12/23] Finalize buoy tracking --- .../sailboat_main/teensy/teensy_node.py | 8 +- .../sailboat_vision/camera_node.py | 77 +++---------------- 2 files changed, 15 insertions(+), 70 deletions(-) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_node.py b/src/sailboat_main/sailboat_main/teensy/teensy_node.py index 9bf9758..819fab1 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_node.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_node.py @@ -51,7 +51,7 @@ def __init__(self): self.wind_angle_pub = self.create_publisher(Int32, 'wind', 10) self.actual_sail_angle_pub = self.create_publisher(Int32, 'actual_sail_angle', 10) self.actual_rudder_angle_pub = self.create_publisher(Int32, 'actual_rudder_angle', 10) - self.actual_buoy_displacement_pub = self.create_publisher(Int32, 'actual_buoy_displacement', 10) + self.buoy_angle = self.create_publisher(Int32, 'buoy_angle', 10) self.dropped_packets_pub = self.create_publisher(Int32, 'dropped_packets', 10) # callback to read teensy data @@ -97,9 +97,9 @@ def check_telemetry(self): rudder_angle_msg.data = data["rudder_angle"] self.actual_rudder_angle_pub.publish(rudder_angle_msg) - buoy_displacement_msg = Int32() - buoy_displacement_msg.data = data["buoy_displacement"] - self.actual_buoy_displacement_pub.publish(buoy_displacement_msg) + buoy_angle_msg = Int32() + buoy_angle_msg.data = data["buoy_angle"] + self.buoy_angle_pub.publish(buoy_angle_msg) dropped_packets_msg = Int32() dropped_packets_msg.data = data["dropped_packets"] diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index b5a63b1..91d98f4 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -12,13 +12,6 @@ class BuoyDetectorNode(Node): def __init__(self): super().__init__('buoy_detector_node') - # --network="host" - # ffmpeg -f avfoundation -list_devices true -i "" - # ffmpeg -f avfoundation -video_device_index 3 -framerate 30 -i "3" -f mpegts udp://localhost:8080 - # self.declare_parameter('video_source', "udp://localhost:8080?overrun_nonfatal=1fifo_size=50000000") - # self.declare_parameter('timer_period', 0.03) - - self.declare_parameter('video_source', 4) self.declare_parameter('hsv_lower', [0, 120, 180]) self.declare_parameter('hsv_upper', [10, 160, 255]) self.declare_parameter('detection_threshold', 100) @@ -30,54 +23,21 @@ def __init__(self): detection_threshold = self.get_parameter('detection_threshold').value self.timer_period = self.get_parameter('timer_period').value - self.detector = BuoyDetectorCV(hsv_lower, hsv_upper, detection_threshold) - - self.displacement_publisher = self.create_publisher(Int32, '/buoy_displacement', 10) - self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) - - # OpenCV video capture - # self.cap = cv2.VideoCapture(self.video_source) - # if not self.cap.isOpened(): - # self.get_logger().error(f'Failed to open video source {self.video_source}') - # raise RuntimeError(f'Cannot open video source {self.video_source}') - - # ret, frame = self.cap.read() - # if not ret: - # self.get_logger().error('Failed to read frame from video source') - # raise RuntimeError(f'Failed to read frame from video source') - - # Create a pipeline self.pipeline = rs.pipeline() - - # Create a config and configure the pipeline to stream - # different resolutions of color and depth streams config = rs.config() - # config.enable_device_from_file("stairs.bag") - - - # Get device product line for setting a supporting resolution - pipeline_wrapper = rs.pipeline_wrapper(pipeline) - pipeline_profile = config.resolve(pipeline_wrapper) - device = pipeline_profile.get_device() - device_product_line = str(device.get_info(rs.camera_info.product_line)) - config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) + self.pipeline.start(config) + self.align = rs.align(rs.stream.color) - # Start streaming - profile = self.pipeline.start(config) - - # Getting the depth sensor's depth scale (see rs-align example for explanation) - depth_sensor = profile.get_device().first_depth_sensor() - depth_scale = depth_sensor.get_depth_scale() - print("Depth Scale is: " , depth_scale) + self.CENTER = 320 + self.MARGIN = 30 # TODO: Find best value for margin of error + self.angle = 90 - align_to = rs.stream.color - self.align = rs.align(align_to) + self.detector = BuoyDetectorCV(hsv_lower, hsv_upper, detection_threshold) - self.CENTER = 0 # frame.shape[1] / 2 - self.MARGIN = 30 # TODO: Find best value for margin of error - self.angle = 0 + self.displacement_publisher = self.create_publisher(Int32, '/buoy_displacement', 10) + self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) self.timer = self.create_timer(self.timer_period, self.process_frame) @@ -87,37 +47,22 @@ def __init__(self): self.buoy_angle_callback, 10) - - frames = self.pipeline.wait_for_frames() - aligned_frames = self.align.process(frames) - self.get_logger().info('Buoy Detector Node Initialized') def process_frame(self): frames = self.pipeline.wait_for_frames() - # frames.get_depth_frame() is a 640x360 depth image # Align the depth frame to color frame aligned_frames = self.align.process(frames) - - # Get aligned frames - aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image + aligned_depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() - - # Validate that both frames are valid if not aligned_depth_frame or not color_frame: return depth_image = np.asanyarray(aligned_depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) - """Process a single frame and publish the buoy position.""" - # ret, frame = self.cap.read() - # if not ret: - # self.get_logger().error('Failed to read frame from video source') - # return - buoy_center, _ = self.detector.process_frame(color_image) if not buoy_center: @@ -129,7 +74,7 @@ def process_frame(self): elif buoy_center[0] < self.CENTER - self.MARGIN: displacement = -1 else: - depth = 0.0 # TODO: Make this real based on self.angle + depth = depth_image[buoy_center[0], buoy_center[1]] point_msg = Point() point_msg.x = self.CENTER + (depth * math.sin(self.angle)) point_msg.y = float(buoy_center[1]) @@ -157,7 +102,7 @@ def update_detector_parameters(self): def destroy_node(self): """Release the video capture and destroy the node.""" - self.cap.release() + self.pipeline.stop() super().destroy_node() def main(args=None): From 3fcd3bcbe6dd80ac63156de9e3522ebf1f26cb70 Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 22 Mar 2025 11:40:54 -0400 Subject: [PATCH 13/23] Reorder logic --- .../sailboat_vision/camera_node.py | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index 91d98f4..c4950d7 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -30,12 +30,12 @@ def __init__(self): self.pipeline.start(config) self.align = rs.align(rs.stream.color) + self.detector = BuoyDetectorCV(hsv_lower, hsv_upper, detection_threshold) + self.CENTER = 320 self.MARGIN = 30 # TODO: Find best value for margin of error self.angle = 90 - self.detector = BuoyDetectorCV(hsv_lower, hsv_upper, detection_threshold) - self.displacement_publisher = self.create_publisher(Int32, '/buoy_displacement', 10) self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) @@ -69,24 +69,26 @@ def process_frame(self): self.get_logger().info(f'No buoy detected') return + displacement = 0 if buoy_center[0] > self.CENTER + self.MARGIN: displacement = 1 elif buoy_center[0] < self.CENTER - self.MARGIN: displacement = -1 - else: - depth = depth_image[buoy_center[0], buoy_center[1]] - point_msg = Point() - point_msg.x = self.CENTER + (depth * math.sin(self.angle)) - point_msg.y = float(buoy_center[1]) - point_msg.z = depth * math.cos(self.angle) - self.position_publisher.publish(point_msg) - self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') + + if displacement: + displacement_msg = Int32() + displacement_msg.data = displacement + self.displacement_publisher.publish(displacement_msg) + self.get_logger().info(f'Detected buoy displacement: {displacement}') return - displacement_msg = Int32() - displacement_msg.data = displacement - self.displacement_publisher.publish(displacement_msg) - self.get_logger().info(f'Detected buoy displacement: {displacement}') + depth = depth_image[buoy_center[0], buoy_center[1]] + point_msg = Point() + point_msg.x = self.CENTER + (depth * math.sin(self.angle)) + point_msg.y = float(buoy_center[1]) + point_msg.z = depth * math.cos(self.angle) + self.position_publisher.publish(point_msg) + self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') def buoy_angle_callback(self, msg): self.angle = msg.data - 90 From eb8db717e4ae1e06872fc031ee76eb15da5de76f Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 22 Mar 2025 12:27:31 -0400 Subject: [PATCH 14/23] Include required dependencies/setup --- setup/requirements.txt | 1 + src/sailboat_vision/sailboat_vision/camera_node.py | 5 ++--- src/sailboat_vision/setup.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/setup/requirements.txt b/setup/requirements.txt index a482cb8..39b9e85 100644 --- a/setup/requirements.txt +++ b/setup/requirements.txt @@ -4,4 +4,5 @@ utm==0.7.0 geopy==2.4.1 rosdoc2 opencv-python==4.8.0.74 +pyrealsense2==2.55.1.6486 diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index c4950d7..2ceb2a6 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -6,7 +6,7 @@ from geometry_msgs.msg import Point from std_msgs.msg import Int32 -from buoy_detection import BuoyDetectorCV +from sailboat_vision.buoy_detection import BuoyDetectorCV class BuoyDetectorNode(Node): def __init__(self): @@ -17,7 +17,6 @@ def __init__(self): self.declare_parameter('detection_threshold', 100) self.declare_parameter('timer_period', 0.1) # Timer period for frame processing - self.video_source = self.get_parameter('video_source').value hsv_lower = self.get_parameter('hsv_lower').value hsv_upper = self.get_parameter('hsv_upper').value detection_threshold = self.get_parameter('detection_threshold').value @@ -82,7 +81,7 @@ def process_frame(self): self.get_logger().info(f'Detected buoy displacement: {displacement}') return - depth = depth_image[buoy_center[0], buoy_center[1]] + depth = depth_image[buoy_center[0], buoy_center[1]] / 1000 point_msg = Point() point_msg.x = self.CENTER + (depth * math.sin(self.angle)) point_msg.y = float(buoy_center[1]) diff --git a/src/sailboat_vision/setup.py b/src/sailboat_vision/setup.py index b1a86d1..ac928ff 100644 --- a/src/sailboat_vision/setup.py +++ b/src/sailboat_vision/setup.py @@ -20,7 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'buoy_detection = sailboat_vision.buoy_detection.camera_node:main' + 'buoy_detection = sailboat_vision.camera_node:main' ], }, ) From 535580345599efae0f65a54d24e46ff188dc599b Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 22 Mar 2025 12:54:47 -0400 Subject: [PATCH 15/23] Add launch file for testing and fix coordinate logic --- .../launch/sailboat.launch_cv_test.py | 37 +++++++++++++++++++ .../sailboat_vision/camera_node.py | 6 +-- 2 files changed, 40 insertions(+), 3 deletions(-) create mode 100644 src/sailboat_launch/launch/sailboat.launch_cv_test.py diff --git a/src/sailboat_launch/launch/sailboat.launch_cv_test.py b/src/sailboat_launch/launch/sailboat.launch_cv_test.py new file mode 100644 index 0000000..bbd8c53 --- /dev/null +++ b/src/sailboat_launch/launch/sailboat.launch_cv_test.py @@ -0,0 +1,37 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('sailboat_launch'), + 'config', + 'config_sim.yaml' + ) + + teensy_cmd = Node( + package='sailboat_main', + executable='teensy', + name='teensy', + namespace='sailbot', + parameters=[config] + ) + + cv_cmd = Node( + package='sailboat_vision', + executable='buoy_detection', + name='buoy_detection', + namespace='sailbot', + parameters=[config] + ) + + ld = LaunchDescription() + + # Sensors + ld.add_action(teensy_cmd) + ld.add_action(cv_cmd) + + return ld \ No newline at end of file diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index 2ceb2a6..06224b2 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -33,7 +33,7 @@ def __init__(self): self.CENTER = 320 self.MARGIN = 30 # TODO: Find best value for margin of error - self.angle = 90 + self.angle = 0 self.displacement_publisher = self.create_publisher(Int32, '/buoy_displacement', 10) self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) @@ -83,14 +83,14 @@ def process_frame(self): depth = depth_image[buoy_center[0], buoy_center[1]] / 1000 point_msg = Point() - point_msg.x = self.CENTER + (depth * math.sin(self.angle)) + point_msg.x = (depth * math.sin(self.angle)) point_msg.y = float(buoy_center[1]) point_msg.z = depth * math.cos(self.angle) self.position_publisher.publish(point_msg) self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') def buoy_angle_callback(self, msg): - self.angle = msg.data - 90 + self.angle = math.radians(msg.data - 90) def update_detector_parameters(self): """Update the CV detector parameters dynamically.""" From 2c61253d4f23f032e8060a511b6df09bfec56f76 Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 22 Mar 2025 13:20:25 -0400 Subject: [PATCH 16/23] Fix bugs in teensy node and fake teensy --- .../sailboat_main/teensy/teensy_fake.py | 6 ++++-- .../sailboat_main/teensy/teensy_node.py | 19 ++++++++++--------- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_fake.py b/src/sailboat_main/sailboat_main/teensy/teensy_fake.py index d3a95a6..f7d8b71 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_fake.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_fake.py @@ -14,6 +14,7 @@ def __init__(self, ): self.wind_step = random.uniform(-5, 5) self.last_sail = 0 self.last_rudder = 0 + self.buoy_angle = 90 self.dropped_packets = 0 print("Simulated TeensyFake initialized") @@ -26,6 +27,7 @@ def read_telemetry(self, data): data["wind_angle"] = self._generate_random_wind() data["sail_angle"] = self.last_sail data["rudder_angle"] = self.last_rudder + data["buoy_angle"] = self.buoy_angle data["dropped_packets"] = self.dropped_packets return random.randint(0,1) @@ -46,7 +48,7 @@ def _generate_random_wind(self): return int(self.wind_angle) - def send_command(self, sail, rudder): + def send_command(self, sail, rudder, buoy_displacement): """ Send a properly formatted command packet to the servo. @@ -55,6 +57,6 @@ def send_command(self, sail, rudder): """ self.last_sail = sail self.last_rudder = rudder - command_packet = bytearray([self.START_BYTE, sail, rudder, self.END_BYTE]) + command_packet = bytearray([self.START_BYTE, sail, rudder, buoy_displacement, self.END_BYTE]) print(f'Sent to FakeTeensy: {command_packet}') diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_node.py b/src/sailboat_main/sailboat_main/teensy/teensy_node.py index 819fab1..37bad4a 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_node.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_node.py @@ -45,13 +45,13 @@ def __init__(self): # keep track of our desired sail and rudder angles self.desired_sail = 0 self.desired_rudder = 0 - self.desired_buoy_displacement = 0 + self.buoy_displacement = 0 # telemetry data publishers self.wind_angle_pub = self.create_publisher(Int32, 'wind', 10) self.actual_sail_angle_pub = self.create_publisher(Int32, 'actual_sail_angle', 10) self.actual_rudder_angle_pub = self.create_publisher(Int32, 'actual_rudder_angle', 10) - self.buoy_angle = self.create_publisher(Int32, 'buoy_angle', 10) + self.buoy_angle_pub = self.create_publisher(Int32, 'buoy_angle', 10) self.dropped_packets_pub = self.create_publisher(Int32, 'dropped_packets', 10) # callback to read teensy data @@ -108,6 +108,7 @@ def check_telemetry(self): self.get_logger().info(f"{'Wind angle:':<20} {wind_angle_msg.data}") self.get_logger().info(f"{'Actual sail angle:':<20} {sail_angle_msg.data}") self.get_logger().info(f"{'Actual tail angle:':<20} {rudder_angle_msg.data}") + self.get_logger().info(f"{'Buoy angle:':<20} {buoy_angle_msg.data}") self.get_logger().info(f"{'Dropped packets:':<20} {dropped_packets_msg.data}") else: self.get_logger().info("No telemetry received") @@ -118,9 +119,9 @@ def sail_callback(self, msg): previously set rudder. """ self.desired_sail = msg.data - self.get_logger().info(f"Sail position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.desired_buoy_displacement}") + self.get_logger().info(f"Sail position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.buoy_displacement}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.desired_buoy_displacement) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.buoy_displacement) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") @@ -131,9 +132,9 @@ def rudder_callback(self, msg): and the previously set sail. """ self.desired_rudder = msg.data - self.get_logger().info(f"Rudder position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.desired_buoy_displacement}") + self.get_logger().info(f"Rudder position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.buoy_displacement}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.desired_buoy_displacement) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.buoy_displacement) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") @@ -143,10 +144,10 @@ def buoy_displacement_callback(self, msg): Callback function for the 'buoy_displacement' topic. Sends the updated buoy position, previously set rudder, and the previously set sail. """ - self.desired_buoy_displacement = msg.data - self.get_logger().info(f"Buoy position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.desired_buoy_displacement}") + self.buoy_displacement = msg.data + self.get_logger().info(f"Buoy position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.buoy_displacement}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.desired_buoy_displacement) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.buoy_displacement) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") From 3d3681559b85a6cc537ae4156f711ee9df033e64 Mon Sep 17 00:00:00 2001 From: nl545 Date: Sat, 22 Mar 2025 13:31:28 -0400 Subject: [PATCH 17/23] change teensy.py to include buoy angle info --- .python-version | 1 + src/sailboat_main/sailboat_main/teensy/teensy.py | 12 ++++++++++-- 2 files changed, 11 insertions(+), 2 deletions(-) create mode 100644 .python-version diff --git a/.python-version b/.python-version new file mode 100644 index 0000000..375f5ca --- /dev/null +++ b/.python-version @@ -0,0 +1 @@ +3.11.6 diff --git a/src/sailboat_main/sailboat_main/teensy/teensy.py b/src/sailboat_main/sailboat_main/teensy/teensy.py index 6efe374..ecb0545 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy.py @@ -38,6 +38,7 @@ def read_telemetry(self, data): data["wind_angle"], \ data["sail_angle"], \ data["rudder_angle"], \ + data["buoy_angle"], \ data["dropped_packets"] = self._parse_packet(self.buffer) # clear buffer @@ -68,9 +69,16 @@ def _parse_packet(self, packet): else: rudder_angle = packet[2] - dropped_packets = packet[3] - return wind_angle, sail_angle, rudder_angle, dropped_packets + if packet[3] >= 128: + buoy_angle = packet[3] - 256 + else: + buoy_angle = packet[3] + + dropped_packets = packet[4] + + return wind_angle, sail_angle, rudder_angle, buoy_angle, dropped_packets + def send_command(self, sail, rudder, buoy_displacement): From baeeb054d8e19627782a1c32b54c0aeb6bf0dce2 Mon Sep 17 00:00:00 2001 From: nl545 Date: Wed, 26 Mar 2025 17:13:48 -0400 Subject: [PATCH 18/23] added buoy info to teensy.py and fixed merge conflicts --- .../sailboat_main/teensy/teensy.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy.py b/src/sailboat_main/sailboat_main/teensy/teensy.py index e4d8b02..a65af5c 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy.py @@ -9,7 +9,7 @@ class TeensyHardware: START_BYTE = 0xff END_BYTE = 0xee - PACKET_LENGTH = 5 + PACKET_LENGTH = 6 def __init__(self, port): self.port = port @@ -66,20 +66,20 @@ def _parse_packet(self, packet): else: sail_angle = packet[2] # uint8_t to int8_t conversion - if packet[2] >= 128: - rudder_angle = packet[2] - 256 + if packet[3] >= 128: + rudder_angle = packet[3] - 256 else: - rudder_angle = packet[2] + rudder_angle = packet[3] - if packet[3] >= 128: - buoy_angle = packet[3] - 256 + if packet[4] >= 128: + buoy_angle = packet[4] - 256 else: - buoy_angle = packet[3] + buoy_angle = packet[4] - dropped_packets = packet[4] + dropped_packets = packet[5] - return wind_angle, sail_angle, rudder_angle, dropped_packets + return wind_angle, sail_angle, rudder_angle, buoy_angle, dropped_packets def send_command(self, sail, rudder, buoy_displacement): From 66911cbee078c818edca587e44734b96deb1777c Mon Sep 17 00:00:00 2001 From: nl545 Date: Sat, 12 Apr 2025 11:40:59 -0400 Subject: [PATCH 19/23] maintain servo angle instead of buoy displacement --- .vscode/settings.json | 3 ++ .../sailboat_main/teensy/teensy.py | 8 ++--- .../sailboat_main/teensy/teensy_fake.py | 4 +-- .../sailboat_main/teensy/teensy_node.py | 28 ++++++++--------- .../sailboat_vision/camera_node.py | 31 +++++++++++-------- .../src/Control Tasks/TrackerControlTask.cpp | 17 +++------- teensy/src/Monitors/SerialMonitor.cpp | 2 +- teensy/src/sfr.cpp | 5 +-- teensy/src/sfr.hpp | 5 +-- 9 files changed, 53 insertions(+), 50 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..70e34ec --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "C_Cpp.errorSquiggles": "disabled" +} \ No newline at end of file diff --git a/src/sailboat_main/sailboat_main/teensy/teensy.py b/src/sailboat_main/sailboat_main/teensy/teensy.py index a65af5c..e183be0 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy.py @@ -82,13 +82,13 @@ def _parse_packet(self, packet): return wind_angle, sail_angle, rudder_angle, buoy_angle, dropped_packets - def send_command(self, sail, rudder, buoy_displacement): + def send_command(self, sail, rudder, servo_angle): """ Send a properly formatted command packet to the servo. :param sail: Sail position (integer) :param tail: Tail position (integer) - :param buoy_displacement: Buoy displacement (integer) + :param servo_angle Servo angle (integer) """ try: # check bounds @@ -98,10 +98,10 @@ def send_command(self, sail, rudder, buoy_displacement): # convert sail and tail to signed 8-bit integers (bytes) sail_byte = sail & 0xFF if sail >= 0 else (sail + 256) & 0xFF rudder_byte = rudder & 0xFF if rudder >= 0 else (rudder + 256) & 0xFF - buoy_displacement_byte = buoy_displacement & 0xFF if buoy_displacement >= 0 else (buoy_displacement + 256) & 0xFF + servo_angle_byte = servo_angle & 0xFF if servo_angle >= 0 else (servo_angle + 256) & 0xFF # create the packet: [start flag] [sail] [tail] [end flag] - command_packet = bytearray([self.START_BYTE, sail_byte, rudder_byte, buoy_displacement_byte, self.END_BYTE]) + command_packet = bytearray([self.START_BYTE, sail_byte, rudder_byte, servo_angle_byte, self.END_BYTE]) # send the packet over serial self.serial.write(command_packet) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_fake.py b/src/sailboat_main/sailboat_main/teensy/teensy_fake.py index 0b404c1..7dfcacf 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_fake.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_fake.py @@ -48,7 +48,7 @@ def _generate_random_wind(self): return int(self.wind_angle) - def send_command(self, sail, rudder, buoy_displacement): + def send_command(self, sail, rudder, servo_angle): """ Send a properly formatted command packet to the servo. @@ -57,6 +57,6 @@ def send_command(self, sail, rudder, buoy_displacement): """ self.last_sail = sail self.last_rudder = rudder - command_packet = bytearray([self.START_BYTE, sail, rudder, buoy_displacement, self.END_BYTE]) + command_packet = bytearray([self.START_BYTE, sail, rudder, servo_angle, self.END_BYTE]) print(f'Sent to FakeTeensy: {command_packet}') diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_node.py b/src/sailboat_main/sailboat_main/teensy/teensy_node.py index 71f46d6..ff419d0 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_node.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_node.py @@ -45,7 +45,7 @@ def __init__(self): # keep track of our desired sail and rudder angles self.desired_sail = 0 self.desired_rudder = 0 - self.buoy_displacement = 0 + self.servo_angle = 0 # telemetry data publishers self.wind_angle_pub = self.create_publisher(Int32, 'wind', 10) @@ -70,11 +70,11 @@ def __init__(self): self.rudder_callback, 10) - #subscription for buoy displacement + #subscription for servo angle self.subscription = self.create_subscription( - Int32, - '/buoy_displacement', - self.buoy_displacement_callback, + Int32, + '/servo_angle', + self.servo_angle_callback, 10) def check_telemetry(self): @@ -121,7 +121,7 @@ def sail_callback(self, msg): self.desired_sail = msg.data #self.get_logger().info(f"Sail callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.buoy_displacement) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.servo_angle) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") @@ -132,22 +132,22 @@ def rudder_callback(self, msg): and the previously set sail. """ self.desired_rudder = msg.data - self.get_logger().info(f"Rudder position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.buoy_displacement}") + self.get_logger().info(f"Rudder position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} servo angle: {self.servo_angle}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.buoy_displacement) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.servo_angle) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") - - def buoy_displacement_callback(self, msg): + + def servo_angle_callback(self, msg): """ - Callback function for the 'buoy_displacement' topic. Sends the updated buoy position, previously set rudder, + Callback function for the 'servo_angle' topic. Sends the updated servo angle, previously set rudder, and the previously set sail. """ - self.buoy_displacement = msg.data - self.get_logger().info(f"Buoy position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} buoy position: {self.buoy_displacement}") + self.servo_angle = msg.data + self.get_logger().info(f"Buoy position callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder} servo angle: {self.servo_angle}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.buoy_displacement) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.servo_angle) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index 06224b2..603cec5 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -16,11 +16,13 @@ def __init__(self): self.declare_parameter('hsv_upper', [10, 160, 255]) self.declare_parameter('detection_threshold', 100) self.declare_parameter('timer_period', 0.1) # Timer period for frame processing + self.declare_parameter('servo_angle_step', 5) # How much to shift servo by each time hsv_lower = self.get_parameter('hsv_lower').value hsv_upper = self.get_parameter('hsv_upper').value detection_threshold = self.get_parameter('detection_threshold').value self.timer_period = self.get_parameter('timer_period').value + self.servo_angle_step = self.get_parameter('servo_angle_step').value self.pipeline = rs.pipeline() config = rs.config() @@ -34,9 +36,10 @@ def __init__(self): self.CENTER = 320 self.MARGIN = 30 # TODO: Find best value for margin of error self.angle = 0 + self.servo_angle = 90 #initialize to center position (check this) - self.displacement_publisher = self.create_publisher(Int32, '/buoy_displacement', 10) self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) + self.servo_angle_publisher = self.create_publisher(Int32, '/servo_angle', 10) self.timer = self.create_timer(self.timer_period, self.process_frame) @@ -67,30 +70,32 @@ def process_frame(self): if not buoy_center: self.get_logger().info(f'No buoy detected') return + + #default angle + servo_angle = 90 - displacement = 0 + #move servo right (min of 0) if buoy_center[0] > self.CENTER + self.MARGIN: - displacement = 1 + servo_angle = max(0, self.servo_angle - self.servo_angle_step) + #move servo left (max of 180) elif buoy_center[0] < self.CENTER - self.MARGIN: - displacement = -1 + servo_angle = min(180, self.servo_angle + self.servo_angle_step) - if displacement: - displacement_msg = Int32() - displacement_msg.data = displacement - self.displacement_publisher.publish(displacement_msg) - self.get_logger().info(f'Detected buoy displacement: {displacement}') - return + angle_msg = Int32() + angle_msg.data = servo_angle + self.servo_angle_publisher.publish(angle_msg) + self.get_logger().info(f'Servo angle: {servo_angle}') depth = depth_image[buoy_center[0], buoy_center[1]] / 1000 point_msg = Point() - point_msg.x = (depth * math.sin(self.angle)) + point_msg.x = (depth * math.sin(math.radians(self.angle) - 90)) point_msg.y = float(buoy_center[1]) - point_msg.z = depth * math.cos(self.angle) + point_msg.z = depth * math.cos(math.radians(self.angle) - 90) self.position_publisher.publish(point_msg) self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') def buoy_angle_callback(self, msg): - self.angle = math.radians(msg.data - 90) + self.angle = msg.data def update_detector_parameters(self): """Update the CV detector parameters dynamically.""" diff --git a/teensy/src/Control Tasks/TrackerControlTask.cpp b/teensy/src/Control Tasks/TrackerControlTask.cpp index 0fe18d8..4c6827d 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.cpp +++ b/teensy/src/Control Tasks/TrackerControlTask.cpp @@ -9,18 +9,11 @@ void TrackerControlTask::execute() { if (sfr::serial::update_servos) { - if (sfr::serial::buoy_displacement > 0) - { - actuate_servo(tracker_servo, read_servo(tracker_servo) + 10); - } - else if (sfr::serial::buoy_displacement < 0) - { - actuate_servo(tracker_servo, read_servo(tracker_servo) - 10); - } - { - actuate_servo(tracker_servo, read_servo(tracker_servo)); - sfr::serial::buoy_angle = read_servo(tracker_servo); - } + // Directly set servo to the commanded angle + actuate_servo(tracker_servo, angle_to_pwm(sfr::serial::servo_angle)); + + // Update the buoy angle to reflect the current servo position + sfr::serial::buoy_angle = sfr::serial::servo_angle; } } diff --git a/teensy/src/Monitors/SerialMonitor.cpp b/teensy/src/Monitors/SerialMonitor.cpp index 1f03165..c813575 100644 --- a/teensy/src/Monitors/SerialMonitor.cpp +++ b/teensy/src/Monitors/SerialMonitor.cpp @@ -27,7 +27,7 @@ void SerialMonitor::execute() sfr::serial::update_servos = true; sfr::servo::sail_angle = sfr::serial::buffer[0]; sfr::servo::rudder_angle = sfr::serial::buffer[1]; - sfr::serial::buoy_displacement = sfr::serial::buffer[2]; + sfr::serial::servo_angle = sfr::serial::buffer[2]; } else if (packet_started) { diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp index 881a07f..ef8e5d8 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -16,10 +16,11 @@ namespace sfr namespace serial { bool update_servos = false; - int8_t buoy_displacement = 0; + //int8_t buoy_displacement = 0; + int8_t servo_angle = 0; //what should the default value be uint8_t dropped_packets = 0; int8_t buoy_angle = 0; - uint8_t buffer[2] = {0}; + uint8_t buffer[3] = {0}; } } \ No newline at end of file diff --git a/teensy/src/sfr.hpp b/teensy/src/sfr.hpp index 122fbcb..25b7ef3 100644 --- a/teensy/src/sfr.hpp +++ b/teensy/src/sfr.hpp @@ -22,11 +22,12 @@ namespace sfr { extern bool update_servos; extern bool send_telemetry; - extern int8_t buoy_displacement; + //extern int8_t buoy_displacement; + extern int8_t servo_angle; extern int8_t buoy_angle; extern uint8_t dropped_packets; - extern uint8_t buffer[2]; + extern uint8_t buffer[3]; } } \ No newline at end of file From 50ef1697c2c34817dc17b791189cd7b582add9ba Mon Sep 17 00:00:00 2001 From: Eric Cai Date: Sat, 12 Apr 2025 12:06:50 -0400 Subject: [PATCH 20/23] Only update buoy position when centered --- .../sailboat_vision/camera_node.py | 46 +++++++++---------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index 603cec5..4260abb 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -1,5 +1,6 @@ import cv2 import math +import numpy as np import pyrealsense2 as rs import rclpy from rclpy.node import Node @@ -36,7 +37,7 @@ def __init__(self): self.CENTER = 320 self.MARGIN = 30 # TODO: Find best value for margin of error self.angle = 0 - self.servo_angle = 90 #initialize to center position (check this) + self.servo_angle = 90 self.position_publisher = self.create_publisher(Point, '/buoy_position', 10) self.servo_angle_publisher = self.create_publisher(Int32, '/servo_angle', 10) @@ -71,28 +72,27 @@ def process_frame(self): self.get_logger().info(f'No buoy detected') return - #default angle - servo_angle = 90 - - #move servo right (min of 0) - if buoy_center[0] > self.CENTER + self.MARGIN: - servo_angle = max(0, self.servo_angle - self.servo_angle_step) - #move servo left (max of 180) - elif buoy_center[0] < self.CENTER - self.MARGIN: - servo_angle = min(180, self.servo_angle + self.servo_angle_step) - - angle_msg = Int32() - angle_msg.data = servo_angle - self.servo_angle_publisher.publish(angle_msg) - self.get_logger().info(f'Servo angle: {servo_angle}') - - depth = depth_image[buoy_center[0], buoy_center[1]] / 1000 - point_msg = Point() - point_msg.x = (depth * math.sin(math.radians(self.angle) - 90)) - point_msg.y = float(buoy_center[1]) - point_msg.z = depth * math.cos(math.radians(self.angle) - 90) - self.position_publisher.publish(point_msg) - self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') + displacement = buoy_center[0] - self.CENTER + if displacement > self.MARGIN: + if buoy_center[0] > self.CENTER + self.MARGIN: + servo_angle = max(0, self.servo_angle - self.servo_angle_step) + #move servo left (max of 180) + elif buoy_center[0] < self.CENTER - self.MARGIN: + servo_angle = min(180, self.servo_angle + self.servo_angle_step) + + angle_msg = Int32() + angle_msg.data = servo_angle + self.servo_angle_publisher.publish(angle_msg) + self.get_logger().info(f'Detected buoy displacement: {displacement}') + self.get_logger().info(f'Servo angle: {servo_angle}') + else: + depth = depth_image[buoy_center[0], buoy_center[1]] / 1000 + point_msg = Point() + point_msg.x = depth * math.sin(math.radians(self.angle) - 90) + point_msg.y = depth * math.cos(math.radians(self.angle) - 90) + point_msg.z = float(buoy_center[1]) + self.position_publisher.publish(point_msg) + self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') def buoy_angle_callback(self, msg): self.angle = msg.data From c288156a018a52682ff9342b068b7ade46947694 Mon Sep 17 00:00:00 2001 From: nl545 Date: Sat, 19 Apr 2025 14:53:19 -0400 Subject: [PATCH 21/23] code cleanup and minor changes --- setup/Dockerfile | 296 +++++++++--------- .../sailboat_main/teensy/teensy_node.py | 2 - .../src/Control Tasks/SerialControlTask.cpp | 2 +- teensy/src/Control Tasks/ServoControlTask.cpp | 8 + teensy/src/Control Tasks/ServoControlTask.hpp | 1 + .../src/Control Tasks/TrackerControlTask.cpp | 3 + teensy/src/Monitors/SerialMonitor.cpp | 15 +- teensy/src/constants.hpp | 2 +- teensy/src/sfr.cpp | 3 +- 9 files changed, 177 insertions(+), 155 deletions(-) diff --git a/setup/Dockerfile b/setup/Dockerfile index 1e96a1c..c3f7d64 100644 --- a/setup/Dockerfile +++ b/setup/Dockerfile @@ -1,148 +1,148 @@ -############################################## -# Created from template ros2.dockerfile.jinja -############################################## - -########################################### -# Base image -########################################### -FROM ubuntu:22.04 AS base - -ENV DEBIAN_FRONTEND=noninteractive - -# Install language -RUN apt-get update && apt-get install -y \ - locales \ - && locale-gen en_US.UTF-8 \ - && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ - && rm -rf /var/lib/apt/lists/* -ENV LANG=en_US.UTF-8 - -# Install timezone -RUN ln -fs /usr/share/zoneinfo/UTC /etc/localtime \ - && export DEBIAN_FRONTEND=noninteractive \ - && apt-get update \ - && apt-get install -y tzdata \ - && dpkg-reconfigure --frontend noninteractive tzdata \ - && rm -rf /var/lib/apt/lists/* - -RUN apt-get update && apt-get -y upgrade \ - && rm -rf /var/lib/apt/lists/* - -# Install common programs -RUN apt-get update && apt-get install -y --no-install-recommends \ - curl \ - gnupg2 \ - lsb-release \ - sudo \ - software-properties-common \ - wget \ - && rm -rf /var/lib/apt/lists/* - -# Install ROS2 -RUN sudo add-apt-repository universe \ - && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ - && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ - && apt-get update && apt-get install -y --no-install-recommends \ - ros-humble-ros-base \ - python3-argcomplete \ - && rm -rf /var/lib/apt/lists/* - -ENV ROS_DISTRO=humble -ENV AMENT_PREFIX_PATH=/opt/ros/humble -ENV COLCON_PREFIX_PATH=/opt/ros/humble -ENV LD_LIBRARY_PATH=/opt/ros/humble/lib -ENV PATH=/opt/ros/humble/bin:$PATH -ENV PYTHONPATH=/opt/ros/humble/local/lib/python3.10/dist-packages:/opt/ros/humble/lib/python3.10/site-packages -ENV ROS_PYTHON_VERSION=3 -ENV ROS_VERSION=2 -ENV DEBIAN_FRONTEND= - -########################################### -# Develop image -########################################### -FROM base AS dev - -ENV DEBIAN_FRONTEND=noninteractive -RUN apt-get update && apt-get install -y --no-install-recommends \ - bash-completion \ - build-essential \ - cmake \ - gdb \ - git \ - openssh-client \ - python3-argcomplete \ - python3-pip \ - ros-dev-tools \ - ros-humble-ament-* \ - vim \ - && rm -rf /var/lib/apt/lists/* - -RUN rosdep init || echo "rosdep already initialized" - -ARG USERNAME=ros -ARG USER_UID=1000 -ARG USER_GID=$USER_UID - -# Check if "ubuntu" user exists, delete it if it does, then create the desired user -RUN if getent passwd ubuntu > /dev/null 2>&1; then \ - userdel -r ubuntu && \ - echo "Deleted existing ubuntu user"; \ - fi && \ - groupadd --gid $USER_GID $USERNAME && \ - useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME && \ - echo "Created new user $USERNAME" - -# Add sudo support for the non-root user -RUN apt-get update && apt-get install -y sudo \ - && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ - && chmod 0440 /etc/sudoers.d/$USERNAME \ - && rm -rf /var/lib/apt/lists/* - -# Set up autocompletion for user -RUN apt-get update && apt-get install -y git-core bash-completion \ - && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \ - && echo "if [ -f /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash ]; then source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash; fi" >> /home/$USERNAME/.bashrc \ - && rm -rf /var/lib/apt/lists/* - -ENV DEBIAN_FRONTEND= -ENV AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 - -########################################### -# Full image -########################################### -FROM dev AS full - -ENV DEBIAN_FRONTEND=noninteractive -# Install the full release -RUN apt-get update && apt-get install -y --no-install-recommends \ - ros-humble-desktop \ - && rm -rf /var/lib/apt/lists/* -ENV DEBIAN_FRONTEND= -ENV LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib - -########################################### -# Additional steps for requirements.txt and setup_ros.sh -########################################### - -# Copy the requirements.txt and src -COPY requirements.txt /home/ros2_user/ros2_ws/ - -# Copy the setup script into the container -COPY setup_ros.sh /home/ros2_user/setup_ros.sh - -# Make the script executable -RUN chmod +x /home/ros2_user/setup_ros.sh - -# Install Necessary Packages -RUN pip3 install -r /home/ros2_user/ros2_ws/requirements.txt - -# Install RosBridge and other WebDev Packages -RUN sudo apt-get -y update \ -&& sudo apt-get -y install curl \ -&& sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo tee /usr/share/keyrings/ros-archive-keyring.gpg > /dev/null \ -&& echo "deb [signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ -&& sudo apt-get -y install ros-humble-rosbridge-server - -# Set the entry point to run the setup script on container start -ENTRYPOINT ["/home/ros2_user/setup_ros.sh"] - +############################################## +# Created from template ros2.dockerfile.jinja +############################################## + +########################################### +# Base image +########################################### +FROM ubuntu:22.04 AS base + +ENV DEBIAN_FRONTEND=noninteractive + +# Install language +RUN apt-get update && apt-get install -y \ + locales \ + && locale-gen en_US.UTF-8 \ + && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ + && rm -rf /var/lib/apt/lists/* +ENV LANG=en_US.UTF-8 + +# Install timezone +RUN ln -fs /usr/share/zoneinfo/UTC /etc/localtime \ + && export DEBIAN_FRONTEND=noninteractive \ + && apt-get update \ + && apt-get install -y tzdata \ + && dpkg-reconfigure --frontend noninteractive tzdata \ + && rm -rf /var/lib/apt/lists/* + +RUN apt-get update && apt-get -y upgrade \ + && rm -rf /var/lib/apt/lists/* + +# Install common programs +RUN apt-get update && apt-get install -y --no-install-recommends \ + curl \ + gnupg2 \ + lsb-release \ + sudo \ + software-properties-common \ + wget \ + && rm -rf /var/lib/apt/lists/* + +# Install ROS2 +RUN sudo add-apt-repository universe \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ + && apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-ros-base \ + python3-argcomplete \ + && rm -rf /var/lib/apt/lists/* + +ENV ROS_DISTRO=humble +ENV AMENT_PREFIX_PATH=/opt/ros/humble +ENV COLCON_PREFIX_PATH=/opt/ros/humble +ENV LD_LIBRARY_PATH=/opt/ros/humble/lib +ENV PATH=/opt/ros/humble/bin:$PATH +ENV PYTHONPATH=/opt/ros/humble/local/lib/python3.10/dist-packages:/opt/ros/humble/lib/python3.10/site-packages +ENV ROS_PYTHON_VERSION=3 +ENV ROS_VERSION=2 +ENV DEBIAN_FRONTEND= + +########################################### +# Develop image +########################################### +FROM base AS dev + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y --no-install-recommends \ + bash-completion \ + build-essential \ + cmake \ + gdb \ + git \ + openssh-client \ + python3-argcomplete \ + python3-pip \ + ros-dev-tools \ + ros-humble-ament-* \ + vim \ + && rm -rf /var/lib/apt/lists/* + +RUN rosdep init || echo "rosdep already initialized" + +ARG USERNAME=ros +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +# Check if "ubuntu" user exists, delete it if it does, then create the desired user +RUN if getent passwd ubuntu > /dev/null 2>&1; then \ + userdel -r ubuntu && \ + echo "Deleted existing ubuntu user"; \ + fi && \ + groupadd --gid $USER_GID $USERNAME && \ + useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME && \ + echo "Created new user $USERNAME" + +# Add sudo support for the non-root user +RUN apt-get update && apt-get install -y sudo \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + && rm -rf /var/lib/apt/lists/* + +# Set up autocompletion for user +RUN apt-get update && apt-get install -y git-core bash-completion \ + && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \ + && echo "if [ -f /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash ]; then source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash; fi" >> /home/$USERNAME/.bashrc \ + && rm -rf /var/lib/apt/lists/* + +ENV DEBIAN_FRONTEND= +ENV AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 + +########################################### +# Full image +########################################### +FROM dev AS full + +ENV DEBIAN_FRONTEND=noninteractive +# Install the full release +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-desktop \ + && rm -rf /var/lib/apt/lists/* +ENV DEBIAN_FRONTEND= +ENV LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib + +########################################### +# Additional steps for requirements.txt and setup_ros.sh +########################################### + +# Copy the requirements.txt and src +COPY requirements.txt /home/ros2_user/ros2_ws/ + +# Copy the setup script into the container +COPY setup_ros.sh /home/ros2_user/setup_ros.sh + +# Make the script executable +RUN chmod +x /home/ros2_user/setup_ros.sh + +# Install Necessary Packages +RUN pip3 install -r /home/ros2_user/ros2_ws/requirements.txt + +# Install RosBridge and other WebDev Packages +RUN sudo apt-get -y update \ + && sudo apt-get -y install curl \ + && sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo tee /usr/share/keyrings/ros-archive-keyring.gpg > /dev/null \ + && echo "deb [signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ + && sudo apt-get -y install ros-humble-rosbridge-server + +# Set the entry point to run the setup script on container start +ENTRYPOINT ["/home/ros2_user/setup_ros.sh"] + diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_node.py b/src/sailboat_main/sailboat_main/teensy/teensy_node.py index ff419d0..49764fe 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_node.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_node.py @@ -2,8 +2,6 @@ from rclpy.node import Node from sailboat_interface.msg import SailTail from std_msgs.msg import Int32 -#import statement may or may not be necessary -from geometry_msgs.msg import Point from . import teensy from . import teensy_fake diff --git a/teensy/src/Control Tasks/SerialControlTask.cpp b/teensy/src/Control Tasks/SerialControlTask.cpp index 91555b5..7a5f378 100644 --- a/teensy/src/Control Tasks/SerialControlTask.cpp +++ b/teensy/src/Control Tasks/SerialControlTask.cpp @@ -26,7 +26,7 @@ void SerialControlTask::execute() constants::serial::TX_END_FLAG}; last_telemetry_send_time = millis(); - Serial.write(data, sizeof(data)); + // Serial.write(data, sizeof(data)); send_telemetry = false; } current_time = millis(); diff --git a/teensy/src/Control Tasks/ServoControlTask.cpp b/teensy/src/Control Tasks/ServoControlTask.cpp index 8fd361b..996d85a 100644 --- a/teensy/src/Control Tasks/ServoControlTask.cpp +++ b/teensy/src/Control Tasks/ServoControlTask.cpp @@ -4,6 +4,7 @@ ServoControlTask::ServoControlTask() { sail_servo.attach(constants::servo::SAIL_PIN); rudder_servo.attach(constants::servo::RUDDER_PIN); + tracker_servo.attach(constants::servo::TRACKER_PIN); // Set initial servo positions to 0-degrees actuate_servo(sail_servo, 1050); actuate_servo(rudder_servo, 1050); @@ -35,6 +36,13 @@ void ServoControlTask::execute() // actuate rudder servo actuate_servo(rudder_servo, sfr::servo::rudder_pwm); } + + // Directly set servo to the commanded angle + actuate_servo(tracker_servo, sfr::serial::servo_angle); + + // Update the buoy angle to reflect the current servo position + sfr::serial::buoy_angle = sfr::serial::servo_angle; + sfr::serial::update_servos = false; // reset flag for next update } } diff --git a/teensy/src/Control Tasks/ServoControlTask.hpp b/teensy/src/Control Tasks/ServoControlTask.hpp index 6a5e281..eb3edde 100644 --- a/teensy/src/Control Tasks/ServoControlTask.hpp +++ b/teensy/src/Control Tasks/ServoControlTask.hpp @@ -19,6 +19,7 @@ class ServoControlTask Servo sail_servo; Servo rudder_servo; + Servo tracker_servo; }; \ No newline at end of file diff --git a/teensy/src/Control Tasks/TrackerControlTask.cpp b/teensy/src/Control Tasks/TrackerControlTask.cpp index 4c6827d..982d173 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.cpp +++ b/teensy/src/Control Tasks/TrackerControlTask.cpp @@ -7,8 +7,11 @@ TrackerControlTask::TrackerControlTask() void TrackerControlTask::execute() { + // Serial.println(sfr::serial::update_servos); + if (sfr::serial::update_servos) { + Serial.println("SDJFOIWEJFOIWEJFOEWIJF"); // Directly set servo to the commanded angle actuate_servo(tracker_servo, angle_to_pwm(sfr::serial::servo_angle)); diff --git a/teensy/src/Monitors/SerialMonitor.cpp b/teensy/src/Monitors/SerialMonitor.cpp index c813575..18e3d1f 100644 --- a/teensy/src/Monitors/SerialMonitor.cpp +++ b/teensy/src/Monitors/SerialMonitor.cpp @@ -8,9 +8,16 @@ SerialMonitor::SerialMonitor() void SerialMonitor::execute() { + //Serial.println("is executing"); while (Serial.available()) { uint8_t incoming_byte = Serial.read(); + Serial.println(incoming_byte); + Serial.println("buffer conditionals"); + Serial.println(buffer_index); + Serial.println(sizeof(sfr::serial::buffer)); + Serial.println(packet_started); + Serial.println(incoming_byte == constants::serial::RX_END_FLAG); // only set flags if packet start byte is correct if (incoming_byte == constants::serial::RX_START_FLAG) { @@ -19,6 +26,8 @@ void SerialMonitor::execute() buffer_index = 0; } // check if packet end byte is correct and buffer is full + + else if (incoming_byte == constants::serial::RX_END_FLAG && buffer_index == sizeof(sfr::serial::buffer) && packet_started) { Serial.println("serial monitor 24"); @@ -28,12 +37,16 @@ void SerialMonitor::execute() sfr::servo::sail_angle = sfr::serial::buffer[0]; sfr::servo::rudder_angle = sfr::serial::buffer[1]; sfr::serial::servo_angle = sfr::serial::buffer[2]; + Serial.println(sfr::serial::update_servos); } else if (packet_started) { + Serial.println("packet started"); // store a data byte into the buffer - if (buffer_index < sizeof(sfr::serial::buffer)) + if (buffer_index < sizeof(sfr::serial::buffer)) { sfr::serial::buffer[buffer_index++] = incoming_byte; + Serial.println("added to buffer"); + } // if buffer is full, drop the packet else { diff --git a/teensy/src/constants.hpp b/teensy/src/constants.hpp index bdcd660..6fccc9a 100644 --- a/teensy/src/constants.hpp +++ b/teensy/src/constants.hpp @@ -18,7 +18,7 @@ namespace constants constexpr uint8_t RUDDER_MIN_ANGLE = 0; constexpr uint8_t RUDDER_MAX_ANGLE = 90; - constexpr uint8_t TRACKER_PIN = 5; //TODO: change to correct pin number + constexpr uint8_t TRACKER_PIN = 5; constexpr uint8_t TRACKER_MIN_ANGLE = 0; constexpr uint8_t TRACKER_MAX_ANGLE = 90; diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp index ef8e5d8..b1465ab 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -16,8 +16,7 @@ namespace sfr namespace serial { bool update_servos = false; - //int8_t buoy_displacement = 0; - int8_t servo_angle = 0; //what should the default value be + int8_t servo_angle = 90; uint8_t dropped_packets = 0; int8_t buoy_angle = 0; From 31e7fd5f757957a47a16db051e3468b22da8be39 Mon Sep 17 00:00:00 2001 From: nl545 Date: Sat, 19 Apr 2025 15:01:34 -0400 Subject: [PATCH 22/23] remove print statements --- teensy/src/Control Tasks/SerialControlTask.cpp | 2 +- teensy/src/Monitors/SerialMonitor.cpp | 10 ---------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/teensy/src/Control Tasks/SerialControlTask.cpp b/teensy/src/Control Tasks/SerialControlTask.cpp index 7a5f378..91555b5 100644 --- a/teensy/src/Control Tasks/SerialControlTask.cpp +++ b/teensy/src/Control Tasks/SerialControlTask.cpp @@ -26,7 +26,7 @@ void SerialControlTask::execute() constants::serial::TX_END_FLAG}; last_telemetry_send_time = millis(); - // Serial.write(data, sizeof(data)); + Serial.write(data, sizeof(data)); send_telemetry = false; } current_time = millis(); diff --git a/teensy/src/Monitors/SerialMonitor.cpp b/teensy/src/Monitors/SerialMonitor.cpp index 18e3d1f..ee8120c 100644 --- a/teensy/src/Monitors/SerialMonitor.cpp +++ b/teensy/src/Monitors/SerialMonitor.cpp @@ -8,16 +8,9 @@ SerialMonitor::SerialMonitor() void SerialMonitor::execute() { - //Serial.println("is executing"); while (Serial.available()) { uint8_t incoming_byte = Serial.read(); - Serial.println(incoming_byte); - Serial.println("buffer conditionals"); - Serial.println(buffer_index); - Serial.println(sizeof(sfr::serial::buffer)); - Serial.println(packet_started); - Serial.println(incoming_byte == constants::serial::RX_END_FLAG); // only set flags if packet start byte is correct if (incoming_byte == constants::serial::RX_START_FLAG) { @@ -37,15 +30,12 @@ void SerialMonitor::execute() sfr::servo::sail_angle = sfr::serial::buffer[0]; sfr::servo::rudder_angle = sfr::serial::buffer[1]; sfr::serial::servo_angle = sfr::serial::buffer[2]; - Serial.println(sfr::serial::update_servos); } else if (packet_started) { - Serial.println("packet started"); // store a data byte into the buffer if (buffer_index < sizeof(sfr::serial::buffer)) { sfr::serial::buffer[buffer_index++] = incoming_byte; - Serial.println("added to buffer"); } // if buffer is full, drop the packet else From 3ce231ffe5711132496a6bf2ae0a5be9350716a1 Mon Sep 17 00:00:00 2001 From: nl545 Date: Thu, 1 May 2025 16:14:16 -0400 Subject: [PATCH 23/23] cv debugging adjustments --- .../sailboat_vision/camera_node.py | 6 +- .../sailboat_vision/search_algo.py | 85 +++++++++++++++++++ teensy/src/Control Tasks/ServoControlTask.cpp | 8 +- teensy/src/Control Tasks/ServoControlTask.hpp | 2 + .../src/Control Tasks/TrackerControlTask.cpp | 56 ++++++------ .../src/Control Tasks/TrackerControlTask.hpp | 30 +++---- teensy/src/MainControlLoop.cpp | 2 +- teensy/src/MainControlLoop.hpp | 2 +- teensy/src/sfr.cpp | 2 +- teensy/src/sfr.hpp | 2 +- 10 files changed, 143 insertions(+), 52 deletions(-) create mode 100644 src/sailboat_vision/sailboat_vision/search_algo.py diff --git a/src/sailboat_vision/sailboat_vision/camera_node.py b/src/sailboat_vision/sailboat_vision/camera_node.py index 4260abb..ba2f3b3 100644 --- a/src/sailboat_vision/sailboat_vision/camera_node.py +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -72,7 +72,7 @@ def process_frame(self): self.get_logger().info(f'No buoy detected') return - displacement = buoy_center[0] - self.CENTER + displacement = abs(buoy_center[0] - self.CENTER) if displacement > self.MARGIN: if buoy_center[0] > self.CENTER + self.MARGIN: servo_angle = max(0, self.servo_angle - self.servo_angle_step) @@ -88,8 +88,8 @@ def process_frame(self): else: depth = depth_image[buoy_center[0], buoy_center[1]] / 1000 point_msg = Point() - point_msg.x = depth * math.sin(math.radians(self.angle) - 90) - point_msg.y = depth * math.cos(math.radians(self.angle) - 90) + point_msg.x = depth * math.sin(math.radians(self.angle - 90)) + point_msg.y = depth * math.cos(math.radians(self.angle - 90)) point_msg.z = float(buoy_center[1]) self.position_publisher.publish(point_msg) self.get_logger().info(f'Detected buoy at: ({point_msg.x, point_msg.y, point_msg.z})') diff --git a/src/sailboat_vision/sailboat_vision/search_algo.py b/src/sailboat_vision/sailboat_vision/search_algo.py new file mode 100644 index 0000000..8d414fa --- /dev/null +++ b/src/sailboat_vision/sailboat_vision/search_algo.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +import rospy +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import Float64 # Assuming buoy angle is a float +import math + +class BuoyWaypointPublisher: + def __init__(self): + rospy.init_node('buoy_waypoint_publisher', anonymous=True) + + # --- Publishers --- + self.waypoint_pub = rospy.Publisher(self.waypoint_topic, NavSatFix, queue_size=10) + + # --- Subscribers --- + rospy.Subscriber(self.buoy_angle_topic, Float64, self.buoy_angle_callback) + rospy.Subscriber(self.current_location_topic, NavSatFix, self.current_location_callback) + + # --- Internal State --- + self.latest_buoy_angle = None + self.current_location = None + + def buoy_angle_callback(self, msg): + """ + Callback function to receive buoy angle. + :param msg: Float64 message containing the buoy angle in radians. + """ + self.latest_buoy_angle = msg.data + self.calculate_and_publish_waypoint() + + def current_location_callback(self, msg): + """ + Callback function to receive the current GPS location. + :param msg: NavSatFix message containing the latitude and longitude. + """ + self.current_location = msg + self.calculate_and_publish_waypoint() + + def calculate_and_publish_waypoint(self): + """ + Calculates the new waypoint based on the buoy angle and current location, and publishes it. + """ + if self.latest_buoy_angle is None or self.current_location is None: + rospy.loginfo_throttle(60, "Waiting for buoy angle and current location to be available.") + return + + #Get current location (lat/lon) and buoy angle + lat = self.current_location.latitude + lon = self.current_location.longitude + angle_rad = self.latest_buoy_angle + + #Calculate the new waypoint coordinates + new_lat, new_lon = self.calculate_new_waypoint(lat, lon, angle_rad, self.waypoint_distance) + + # 3. Create and publish the new waypoint message + waypoint = NavSatFix() + + self.waypoint_pub.publish(waypoint) + rospy.loginfo(f"Published new waypoint: Lat={new_lat:.6f}, Lon={new_lon:.6f}") + + def calculate_new_waypoint(self, lat_deg, lon_deg, angle_rad, distance_m): + """ + Calculates a new waypoint (lat/lon) given a starting point, angle, and distance. + Uses the Haversine formula for more accurate distance calculations on a sphere. + """ + R = 6371e3 # Radius of Earth in meters + + lat_rad = math.radians(lat_deg) + lon_rad = math.radians(lon_deg) + + new_lat_rad = 0 + + new_lon_rad = 0 + + new_lat_deg = math.degrees(new_lat_rad) + new_lon_deg = math.degrees(new_lon_rad) + + return new_lat_deg, new_lon_deg + +if __name__ == '__main__': + try: + buoy_waypoint_publisher = BuoyWaypointPublisher() + rospy.spin() # Keep the node running + except rospy.ROSInterruptException: + pass diff --git a/teensy/src/Control Tasks/ServoControlTask.cpp b/teensy/src/Control Tasks/ServoControlTask.cpp index 996d85a..a239071 100644 --- a/teensy/src/Control Tasks/ServoControlTask.cpp +++ b/teensy/src/Control Tasks/ServoControlTask.cpp @@ -38,10 +38,11 @@ void ServoControlTask::execute() } // Directly set servo to the commanded angle - actuate_servo(tracker_servo, sfr::serial::servo_angle); + actuate_servo(tracker_servo, tracker_to_pwm(sfr::serial::servo_angle)); // Update the buoy angle to reflect the current servo position sfr::serial::buoy_angle = sfr::serial::servo_angle; + Serial.println(sfr::serial::servo_angle); sfr::serial::update_servos = false; // reset flag for next update } @@ -63,6 +64,11 @@ uint32_t ServoControlTask::sail_to_pwm(uint8_t angle) return map(angle, 0, 90, 1050, 1200); } +uint32_t ServoControlTask::tracker_to_pwm(uint8_t angle) +{ + return map(angle, 0, 180, 0, 127); +} + void ServoControlTask::actuate_servo(Servo &servo, uint32_t pwm) { servo.write(pwm); diff --git a/teensy/src/Control Tasks/ServoControlTask.hpp b/teensy/src/Control Tasks/ServoControlTask.hpp index eb3edde..ab027a5 100644 --- a/teensy/src/Control Tasks/ServoControlTask.hpp +++ b/teensy/src/Control Tasks/ServoControlTask.hpp @@ -14,6 +14,8 @@ class ServoControlTask // uint32_t angle_to_pwm(uint8_t angle); uint32_t sail_to_pwm(uint8_t angle); uint32_t tail_to_pwm(uint8_t angle); + uint32_t tracker_to_pwm(uint8_t angle); + void actuate_servo(Servo &servo, uint32_t pwm); diff --git a/teensy/src/Control Tasks/TrackerControlTask.cpp b/teensy/src/Control Tasks/TrackerControlTask.cpp index 982d173..d6ee4c3 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.cpp +++ b/teensy/src/Control Tasks/TrackerControlTask.cpp @@ -1,36 +1,34 @@ -#include "TrackerControlTask.hpp" +// #include "TrackerControlTask.hpp" -TrackerControlTask::TrackerControlTask() -{ - tracker_servo.attach(constants::servo::TRACKER_PIN); -} +// TrackerControlTask::TrackerControlTask() +// { +// tracker_servo.attach(constants::servo::TRACKER_PIN); +// } -void TrackerControlTask::execute() -{ - // Serial.println(sfr::serial::update_servos); +// void TrackerControlTask::execute() +// { - if (sfr::serial::update_servos) - { - Serial.println("SDJFOIWEJFOIWEJFOEWIJF"); - // Directly set servo to the commanded angle - actuate_servo(tracker_servo, angle_to_pwm(sfr::serial::servo_angle)); +// if (sfr::serial::update_servos) +// { +// // Directly set servo to the commanded angle +// actuate_servo(tracker_servo, angle_to_pwm(sfr::serial::servo_angle)); - // Update the buoy angle to reflect the current servo position - sfr::serial::buoy_angle = sfr::serial::servo_angle; - } -} +// // Update the buoy angle to reflect the current servo position +// sfr::serial::buoy_angle = sfr::serial::servo_angle; +// } +// } -uint32_t TrackerControlTask::angle_to_pwm(uint8_t angle) -{ - return map(angle, 0, 90, 1050, 1300); // TODO: find out what these numbers mean -} +// uint32_t TrackerControlTask::angle_to_pwm(uint8_t angle) +// { +// return map(angle, 0, 90, 1050, 1300); // TODO: find out what these numbers mean +// } -void TrackerControlTask::actuate_servo(Servo &servo, uint32_t pwm) -{ - servo.write(pwm); -} +// void TrackerControlTask::actuate_servo(Servo &servo, uint32_t pwm) +// { +// servo.write(pwm); +// } -uint8_t TrackerControlTask::read_servo(Servo &servo) -{ - return servo.read(); -} \ No newline at end of file +// uint8_t TrackerControlTask::read_servo(Servo &servo) +// { +// return servo.read(); +// } \ No newline at end of file diff --git a/teensy/src/Control Tasks/TrackerControlTask.hpp b/teensy/src/Control Tasks/TrackerControlTask.hpp index deb5e45..9ba89d7 100644 --- a/teensy/src/Control Tasks/TrackerControlTask.hpp +++ b/teensy/src/Control Tasks/TrackerControlTask.hpp @@ -1,19 +1,19 @@ -#pragma once -#include +// #pragma once +// #include -#include "constants.hpp" -#include "sfr.hpp" +// #include "constants.hpp" +// #include "sfr.hpp" -class TrackerControlTask -{ -public: - TrackerControlTask(); - void execute(); +// class TrackerControlTask +// { +// public: +// TrackerControlTask(); +// void execute(); -private: - uint32_t angle_to_pwm(uint8_t angle); - void actuate_servo(Servo &servo, uint32_t pwm); - uint8_t read_servo(Servo &servo); +// private: +// uint32_t angle_to_pwm(uint8_t angle); +// void actuate_servo(Servo &servo, uint32_t pwm); +// uint8_t read_servo(Servo &servo); - Servo tracker_servo; -}; \ No newline at end of file +// Servo tracker_servo; +// }; \ No newline at end of file diff --git a/teensy/src/MainControlLoop.cpp b/teensy/src/MainControlLoop.cpp index 09283a4..d33d1b4 100644 --- a/teensy/src/MainControlLoop.cpp +++ b/teensy/src/MainControlLoop.cpp @@ -17,5 +17,5 @@ void MainControlLoop::execute() serial_monitor.execute(); servo_control_task.execute(); serial_control_task.execute(); - tracker_control_task.execute(); + // tracker_control_task.execute(); } diff --git a/teensy/src/MainControlLoop.hpp b/teensy/src/MainControlLoop.hpp index 3bb27fb..d9bad4e 100644 --- a/teensy/src/MainControlLoop.hpp +++ b/teensy/src/MainControlLoop.hpp @@ -19,5 +19,5 @@ class MainControlLoop SerialMonitor serial_monitor; ServoControlTask servo_control_task; SerialControlTask serial_control_task; - TrackerControlTask tracker_control_task; + // TrackerControlTask tracker_control_task; }; \ No newline at end of file diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp index b1465ab..19ebc66 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -16,7 +16,7 @@ namespace sfr namespace serial { bool update_servos = false; - int8_t servo_angle = 90; + uint8_t servo_angle = 90; uint8_t dropped_packets = 0; int8_t buoy_angle = 0; diff --git a/teensy/src/sfr.hpp b/teensy/src/sfr.hpp index 25b7ef3..3809d2c 100644 --- a/teensy/src/sfr.hpp +++ b/teensy/src/sfr.hpp @@ -23,7 +23,7 @@ namespace sfr extern bool update_servos; extern bool send_telemetry; //extern int8_t buoy_displacement; - extern int8_t servo_angle; + extern uint8_t servo_angle; extern int8_t buoy_angle; extern uint8_t dropped_packets;