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/.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/setup/Dockerfile b/setup/Dockerfile index f3fd31a..c3f7d64 100644 --- a/setup/Dockerfile +++ b/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"] diff --git a/setup/requirements.txt b/setup/requirements.txt index 458d33d..39b9e85 100644 --- a/setup/requirements.txt +++ b/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 + diff --git a/src/sailboat_launch/config/config.yaml b/src/sailboat_launch/config/config.yaml index 48ee176..c192d8f 100644 --- a/src/sailboat_launch/config/config.yaml +++ b/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 \ No newline at end of file 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_main/sailboat_main/teensy/teensy.py b/src/sailboat_main/sailboat_main/teensy/teensy.py index 577b01d..e183be0 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 @@ -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) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_fake.py b/src/sailboat_main/sailboat_main/teensy/teensy_fake.py index 2025789..7dfcacf 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,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}') diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_node.py b/src/sailboat_main/sailboat_main/teensy/teensy_node.py index 91f8021..49764fe 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_node.py +++ b/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,6 +95,10 @@ 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) @@ -94,6 +106,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") @@ -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") 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/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..ba2f3b3 --- /dev/null +++ b/src/sailboat_vision/sailboat_vision/camera_node.py @@ -0,0 +1,122 @@ +import cv2 +import math +import numpy as np +import pyrealsense2 as rs +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Point +from std_msgs.msg import Int32 + +from sailboat_vision.buoy_detection import BuoyDetectorCV + +class BuoyDetectorNode(Node): + def __init__(self): + super().__init__('buoy_detector_node') + + 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.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() + 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) + + 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 = 0 + 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) + + 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): + + frames = self.pipeline.wait_for_frames() + + # Align the depth frame to color frame + aligned_frames = self.align.process(frames) + aligned_depth_frame = aligned_frames.get_depth_frame() + color_frame = aligned_frames.get_color_frame() + 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()) + + buoy_center, _ = self.detector.process_frame(color_image) + + if not buoy_center: + self.get_logger().info(f'No buoy detected') + return + + 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) + #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 + + 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.pipeline.stop() + 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/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/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..ac928ff --- /dev/null +++ b/src/sailboat_vision/setup.py @@ -0,0 +1,26 @@ +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': [ + 'buoy_detection = sailboat_vision.camera_node:main' + ], + }, +) 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' diff --git a/teensy/src/Control Tasks/SerialControlTask.cpp b/teensy/src/Control Tasks/SerialControlTask.cpp index ce54097..91555b5 100644 --- a/teensy/src/Control Tasks/SerialControlTask.cpp +++ b/teensy/src/Control Tasks/SerialControlTask.cpp @@ -21,6 +21,7 @@ void SerialControlTask::execute() sfr::anemometer::wind_angle & 0xFF, 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/ServoControlTask.cpp b/teensy/src/Control Tasks/ServoControlTask.cpp index 8fd361b..a239071 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,14 @@ 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, 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 } } @@ -55,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 6a5e281..ab027a5 100644 --- a/teensy/src/Control Tasks/ServoControlTask.hpp +++ b/teensy/src/Control Tasks/ServoControlTask.hpp @@ -14,11 +14,14 @@ 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); 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 new file mode 100644 index 0000000..d6ee4c3 --- /dev/null +++ b/teensy/src/Control Tasks/TrackerControlTask.cpp @@ -0,0 +1,34 @@ +// #include "TrackerControlTask.hpp" + +// TrackerControlTask::TrackerControlTask() +// { +// tracker_servo.attach(constants::servo::TRACKER_PIN); +// } + +// void TrackerControlTask::execute() +// { + +// 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; +// } +// } + +// 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); +// } + +// 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 new file mode 100644 index 0000000..9ba89d7 --- /dev/null +++ b/teensy/src/Control Tasks/TrackerControlTask.hpp @@ -0,0 +1,19 @@ +// #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); +// uint8_t read_servo(Servo &servo); + +// Servo tracker_servo; +// }; \ No newline at end of file diff --git a/teensy/src/MainControlLoop.cpp b/teensy/src/MainControlLoop.cpp index 7fbf197..d33d1b4 100644 --- a/teensy/src/MainControlLoop.cpp +++ b/teensy/src/MainControlLoop.cpp @@ -5,10 +5,6 @@ MainControlLoop::MainControlLoop() serial_monitor(), servo_control_task(), serial_control_task() -// MainControlLoop::MainControlLoop() -// : -// serial_monitor(), -// serial_control_task() { @@ -21,4 +17,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..d9bad4e 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 cdbdb4b..ee8120c 100644 --- a/teensy/src/Monitors/SerialMonitor.cpp +++ b/teensy/src/Monitors/SerialMonitor.cpp @@ -19,6 +19,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"); @@ -27,12 +29,14 @@ 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::servo_angle = sfr::serial::buffer[2]; } else if (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; + } // if buffer is full, drop the packet else { diff --git a/teensy/src/constants.hpp b/teensy/src/constants.hpp index e73dca7..6fccc9a 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; + 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 66fd0e7..19ebc66 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -16,9 +16,10 @@ namespace sfr namespace serial { bool update_servos = false; - + uint8_t servo_angle = 90; 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 d274335..3809d2c 100644 --- a/teensy/src/sfr.hpp +++ b/teensy/src/sfr.hpp @@ -22,9 +22,12 @@ namespace sfr { extern bool update_servos; extern bool send_telemetry; + //extern int8_t buoy_displacement; + extern uint8_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