Buoy tracking with computer vision

.python-version

@@ -0,0 +1 @@
+3.11.6

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "C_Cpp.errorSquiggles": "disabled"
+}

setup/Dockerfile

@@ -15,7 +15,7 @@ RUN apt-get update && apt-get install -y \
   && 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
+ENV LANG=en_US.UTF-8
 
 # Install timezone
 RUN ln -fs /usr/share/zoneinfo/UTC /etc/localtime \
@@ -26,25 +26,25 @@ RUN ln -fs /usr/share/zoneinfo/UTC /etc/localtime \
   && rm -rf /var/lib/apt/lists/*
 
 RUN apt-get update && apt-get -y upgrade \
-    && rm -rf /var/lib/apt/lists/*
+  && 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/*
+  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 \
+  ros-humble-ros-base \
+  python3-argcomplete \
   && rm -rf /var/lib/apt/lists/*
 
 ENV ROS_DISTRO=humble
@@ -85,12 +85,12 @@ 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"
+  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 \
@@ -137,11 +137,11 @@ RUN chmod +x /home/ros2_user/setup_ros.sh
 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
+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"]

setup/requirements.txt

@@ -3,3 +3,6 @@ pyserial==3.5
 utm==0.7.0
 geopy==2.4.1
 rosdoc2
+opencv-python==4.8.0.74
+pyrealsense2==2.55.1.6486
+

src/sailboat_launch/config/config.yaml

@@ -17,3 +17,9 @@ sailbot: #Namespace
       ros__parameters:
          vectornav_port: "/dev/serial/by-id/usb-FTDI_USB-RS232-WE_AV0LF3XT-if00-port0"
          simulated: false
+   computer_vision: 
+      ros__parameters:
+         video_source: 0 
+         hsv_lower: [0, 120, 180]
+         hsv_upper: [10, 160, 255] 
+         detection_threshold: 100

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

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
@@ -39,6 +39,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
@@ -70,17 +71,24 @@ def _parse_packet(self, packet):
         else:
             rudder_angle = packet[3]
 
-        dropped_packets = packet[4]
 
-        return wind_angle, sail_angle, rudder_angle, dropped_packets
+        if packet[4] >= 128:
+            buoy_angle = packet[4] - 256
+        else:
+            buoy_angle = packet[4]
+
+        dropped_packets = packet[5]
+
+        return wind_angle, sail_angle, rudder_angle, buoy_angle, dropped_packets
 
 
-    def send_command(self, sail, rudder):
+    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 servo_angle Servo angle (integer)
         """
         try:
             # check bounds
@@ -90,9 +98,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
+            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, 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)

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,28 +48,15 @@ def _generate_random_wind(self):
         return int(self.wind_angle)
 
 
-    def send_command(self, sail, rudder):
+    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)
         """
-        try:
-            # check bounds
-            sail = max(min(sail, 127), -128)
-            rudder = max(min(rudder, 127), -128)
-
-            # 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
-
-            # create the packet: [start flag] [sail] [tail] [end flag]
-            command_packet = bytearray([self.START_BYTE, sail_byte, rudder_byte, self.END_BYTE])
-
-            # send the packet over serial
-            return 0   
-        except:
-            return 1
+        self.last_sail = sail
+        self.last_rudder = rudder
+        command_packet = bytearray([self.START_BYTE, sail, rudder, servo_angle, self.END_BYTE])
 
         print(f'Sent to FakeTeensy: {command_packet}')

src/sailboat_main/sailboat_main/teensy/teensy_node.py

@@ -2,7 +2,6 @@
 from rclpy.node import Node
 from sailboat_interface.msg import SailTail
 from std_msgs.msg import Int32
-
 from . import teensy
 from . import teensy_fake
 
@@ -44,11 +43,13 @@ def __init__(self):
         # keep track of our desired sail and rudder angles
         self.desired_sail = 0
         self.desired_rudder = 0
+        self.servo_angle = 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_pub = self.create_publisher(Int32, 'buoy_angle', 10)
         self.dropped_packets_pub = self.create_publisher(Int32, 'dropped_packets', 10)
 
         # callback to read teensy data
@@ -66,6 +67,13 @@ def __init__(self):
             'rudder_angle',
             self.rudder_callback,
             10)
+
+        #subscription for servo angle
+        self.subscription = self.create_subscription(
+            Int32,
+            '/servo_angle',
+            self.servo_angle_callback,
+            10)
 
     def check_telemetry(self):
         """
@@ -87,13 +95,18 @@ def check_telemetry(self):
             rudder_angle_msg.data = data["rudder_angle"]
             self.actual_rudder_angle_pub.publish(rudder_angle_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"]
             self.dropped_packets_pub.publish(dropped_packets_msg)
 
             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")
@@ -106,20 +119,33 @@ 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) == 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 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} servo angle: {self.servo_angle}")
+
+        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 servo_angle_callback(self, msg):
+        """
+        Callback function for the 'servo_angle' topic. Sends the updated servo angle, previously set rudder,
+        and the previously set sail.
+        """
+        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) == 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")

src/sailboat_vision/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>sailboat_vision</name>
+  <version>0.0.0</version>
+  <description>Computer Vision</description>
+  <maintainer email="fz246@cornell.edu">Fiona Zheng</maintainer>
+  <license>Apache License 2.0</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>