feat: Final openCV assignment

.fish/activate.fish

@@ -1,9 +1,9 @@
-# Self-contained environment: ros2-lectures
-# Exported on: Thu Dec  4 09:45:42 AM CET 2025
-# Original environment from: /home/wessel/.config/fish/environments/configs/ros2-lectures
+# Self-contained environment: ros2-pass-2
+# Exported on: Thu Nov 13 06:58:18 PM CET 2025
+# Original environment from: /home/wessel/.config/fish/environments/configs/ros2-pass-2
 
 # ROS2 development environment (requires distrobox)
-# Environment: ros2-lectures
+# Environment: ros2-pass-2
 
 # First check if running inside distrobox
 if not test -f /run/.containerenv; and test -z "$CONTAINER_ID"
@@ -19,7 +19,7 @@ end
 
 # Mark as initialized (only after distrobox check passes)
 set -gx __ENV_INITIALIZED "1"
-set -gx CURRENT_ENV "ros2-lectures"
+set -gx CURRENT_ENV "ros2-pass-2"
 # Source ROS2 setup files using bass
 if type -q bass
     bass source /opt/ros/jazzy/setup.bash
@@ -51,7 +51,7 @@ end
 
 # Define new prompt with ROS2 prefix
 function fish_prompt
-    echo -n (set_color magenta)'(ros2-lectures)'(set_color normal)' '
+    echo -n (set_color magenta)'(ros2-pass-2)'(set_color normal)' '
     __env_orig_prompt
 end
 
@@ -63,11 +63,7 @@ alias cbt="colcon build --packages-select"
 alias ct="colcon test"
 alias ctr="colcon test-result"
 
-# For communication between nodes on other devices
-export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
-export ROS_DOMAIN_ID=5
-
-echo (set_color green)"Activated ROS2 environment: ros2-lectures"(set_color normal)
+echo (set_color green)"Activated ROS2 environment: ros2-pass-2"(set_color normal)
 
 # Custom deactivation function
 function __env_custom_deactivate

.fish/readme.norg

@@ -1,4 +1,4 @@
-* Exported Fish Environment: ros2-lectures
+* Exported Fish Environment: ros2-pass-2
 This directory contains a self-contained fish environment.
 
 ** Files Structure

src/image_reader_wessel_pkg/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(image_reader_wessel_pkg)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(OpenCV REQUIRED)
+
+# Add executable
+add_executable(image_reader_node
+  src/Main.cpp
+  src/nodes/ImageReader.cpp
+)
+
+# Link dependencies
+ament_target_dependencies(image_reader_node
+  rclcpp
+  sensor_msgs
+  cv_bridge
+  OpenCV
+)
+
+# Install targets
+install(TARGETS
+  image_reader_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

src/image_reader_wessel_pkg/package.xml

@@ -1,16 +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>soup_relay_pkg</name>
+  <name>image_reader_wessel_pkg</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Image reader package with OpenCV processing</description>
   <maintainer email="contact@wessel.gg">wessel</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>libopencv-dev</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/image_reader_wessel_pkg/src/Main.cpp

@@ -0,0 +1,15 @@
+#include <cstdlib>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nodes/ImageReader.hpp"
+
+int main(int argc,char *argv[]) {
+    rclcpp::init(argc,argv);
+
+    auto node = std::make_shared<assignments::two::image_reader::ImageReaderNode>();
+
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+
+    return 0;
+}

src/image_reader_wessel_pkg/src/nodes/ImageReader.cpp

@@ -0,0 +1,145 @@
+#include "ImageReader.hpp"
+
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.hpp>
+
+namespace assignments::two::image_reader {
+
+ImageReaderNode::ImageReaderNode() : Node("image_reader_node") {
+    circle_color_.r = this->declare_parameter<int>("circle_color_r", 255);
+    circle_color_.g = this->declare_parameter<int>("circle_color_g", 0);
+    circle_color_.b = this->declare_parameter<int>("circle_color_b", 0);
+
+    RCLCPP_INFO(this->get_logger(), "circle_color_=(r%d, g%d, b%d)",
+        circle_color_.r, circle_color_.g, circle_color_.b);
+
+    // Subscribe to image topic
+    image_subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
+        "/image_raw",
+        10,
+        std::bind(
+            &ImageReaderNode::image_callback,
+            this,
+            std::placeholders::_1
+        )
+    );
+
+    // Publisher for processed image
+    image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("/image_processed", 10);
+
+    RCLCPP_INFO(this->get_logger(), "initialized");
+}
+
+void ImageReaderNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
+    RCLCPP_INFO_ONCE(this->get_logger(), "'/image_raw': received image, processing");
+
+    try {
+        // handle both RGB and BGR
+        cv_bridge::CvImagePtr cv_ptr;
+        if (msg->encoding == sensor_msgs::image_encodings::RGB8) {
+            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
+            cv::cvtColor(cv_ptr->image, cv_ptr->image, cv::COLOR_RGB2BGR);
+        } else {
+            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+        }
+
+        ImageMetadata img;
+        img.canvas = cv_ptr->image;
+        img.width = img.canvas.cols;
+        img.height = img.canvas.rows;
+
+        process_top_left_quadrant_white(img);
+        process_top_right_quadrant_circle(img);
+        process_bottom_left_quadrant_remove_blue(img);
+        process_bottom_right_quadrant_edge_detection(img);
+
+        draw_division_lines(img);
+
+        cv_ptr->image = img.canvas;
+        image_publisher_->publish(*cv_ptr->toImageMsg());
+    } catch (cv_bridge::Exception& e) {
+        RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
+    }
+}
+
+void ImageReaderNode::draw_division_lines(ImageMetadata& img) {
+    int half_width = img.width / 2;
+    int half_height = img.height / 2;
+
+    cv::line(
+        img.canvas,
+        cv::Point(half_width, 0),
+        cv::Point(half_width, img.height),
+        cv::Scalar(255, 255, 255),
+        2
+    );
+
+    cv::line(
+        img.canvas,
+        cv::Point(0, half_height),
+        cv::Point(img.width, half_height),
+        cv::Scalar(255, 255, 255),
+        2
+    );
+}
+
+void ImageReaderNode::process_top_left_quadrant_white(ImageMetadata& img) {
+    int half_width = img.width / 2;
+    int half_height = img.height / 2;
+
+    cv::Mat top_left = img.canvas(cv::Rect(0, 0, half_width, half_height));
+    top_left.setTo(cv::Scalar(255, 255, 255));
+}
+
+void ImageReaderNode::process_top_right_quadrant_circle(ImageMetadata& img) {
+    int half_width = img.width / 2;
+    int half_height = img.height / 2;
+
+    cv::Mat top_right = img.canvas(cv::Rect(half_width, 0, img.width - half_width, half_height));
+    int radius = std::min(top_right.cols, top_right.rows) / 4;
+    cv::Point center(top_right.cols / 2, top_right.rows / 2);
+
+    cv::circle(
+        top_right,
+        center,
+        radius,
+        cv::Scalar(circle_color_.b, circle_color_.g, circle_color_.r),
+        -1
+    );
+}
+
+void ImageReaderNode::process_bottom_left_quadrant_remove_blue(ImageMetadata& img) {
+    int half_width = img.width / 2;
+    int half_height = img.height / 2;
+    cv::Mat bottom_left =
+        img.canvas(cv::Rect(0, half_height, half_width, img.height - half_height));
+
+    for (int y = 0; y < bottom_left.rows; y++) {
+        for (int x = 0; x < bottom_left.cols; x++) {
+            bottom_left.at<cv::Vec3b>(y, x)[0] = 0; // Blue channel
+        }
+    }
+}
+
+void ImageReaderNode::process_bottom_right_quadrant_edge_detection(ImageMetadata& img) {
+    int half_width = img.width / 2;
+    int half_height = img.height / 2;
+
+    cv::Mat bottom_right =
+        img.canvas(
+            cv::Rect(
+                half_width,
+                half_height,
+                img.width - half_width,
+                img.height - half_height
+            )
+        );
+
+    cv::Mat gray, edges;
+
+    cv::cvtColor(bottom_right, gray, cv::COLOR_BGR2GRAY);
+    cv::Canny(gray, edges, 50, 150);
+    cv::cvtColor(edges, bottom_right, cv::COLOR_GRAY2BGR);
+}
+
+} // namespace assignments::two::image_reader

src/image_reader_wessel_pkg/src/nodes/ImageReader.hpp

@@ -0,0 +1,40 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include <opencv2/opencv.hpp>
+
+namespace assignments::two::image_reader {
+
+struct RGBColors {
+    int r;
+    int g;
+    int b;
+};
+
+struct ImageMetadata {
+    cv::Mat canvas;
+    int width;
+    int height;
+};
+
+class ImageReaderNode : public rclcpp::Node {
+public:
+    ImageReaderNode();
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscriber_;
+    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
+
+    RGBColors circle_color_;
+
+    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+
+    void draw_division_lines(ImageMetadata& img);
+    void process_top_left_quadrant_white(ImageMetadata& img);
+    void process_top_right_quadrant_circle(ImageMetadata& img);
+    void process_bottom_left_quadrant_remove_blue(ImageMetadata& img);
+    void process_bottom_right_quadrant_edge_detection(ImageMetadata& img);
+};
+
+} // namespace assignments::two::image_reader

src/launch_wessel_pkg/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(launch_wessel_pkg)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY launch rviz
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/launch_wessel_pkg/launch/image_reader.launch.xml

@@ -0,0 +1,19 @@
+<launch>
+    <node pkg="usb_cam" exec="usb_cam_node_exe" name="usb_cam">
+        <param from="$(find-pkg-share usb_cam_wessel_pkg)/config/params.yaml"/>
+    </node>
+
+    <node pkg="image_reader_wessel_pkg" exec="image_reader_node" name="image_reader_node">
+        <param name="circle_color_b" value="255"/>
+        <param name="circle_color_g" value="0"/>
+        <param name="circle_color_r" value="0"/>
+    </node>
+
+    <node
+        pkg="rviz2"
+        exec="rviz2"
+        name="rviz2"
+        output="screen"
+        args="-d $(find-pkg-share launch_wessel_pkg)/rviz/image_processor_config.rviz"
+    />
+</launch>

src/launch_wessel_pkg/package.xml

@@ -0,0 +1,15 @@
+<?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>launch_wessel_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="contact@wessel.gg">Wessel T</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/launch_wessel_pkg/rviz/image_processor_config.rviz

@@ -0,0 +1,131 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 1255
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /image_processed
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1436
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000056ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000130000056f000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000032c0000056ffc0200000004fb0000000a0049006d00610067006501000000130000056f0000001600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003a000005040000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ae0000003efc0100000002fb0000000800540069006d00650000000000000004ae000002e800fffffffb0000000800540069006d00650100000000000004500000000000000000000000200000056f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1198
+  X: 30
+  Y: 54

src/soup_relay_pkg/CMakeLists.txt

@@ -1,39 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(soup_relay_pkg)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-
-add_executable(soup_relay src/soup_relay.cpp)
-target_include_directories(soup_relay PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
-target_compile_features(soup_relay PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-ament_target_dependencies(
-  soup_relay
-  "rclcpp"
-  "std_msgs"
-)
-
-install(TARGETS soup_relay
-  DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()

src/soup_relay_pkg/src/soup_relay.cpp

@@ -1,54 +0,0 @@
-#include <string>
-#include <iostream>
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/string.hpp>
-
-class SoupRelayNode : public rclcpp::Node {
-public:
-    SoupRelayNode() : Node("soup_relay_node") {
-        auto topic_publisher = this->declare_parameter<std::string>("topic_publisher", "vin7");
-        auto topic_subscriber = this->declare_parameter<std::string>("topic_subscriber", "vin6");
-
-        publisher_ = this->create_publisher<std_msgs::msg::String>(topic_publisher, 10);
-        subscriber_ = this->create_subscription<std_msgs::msg::String>(
-            topic_subscriber, 10,
-            std::bind(&SoupRelayNode::message_callback, this, std::placeholders::_1)
-        );
-
-        RCLCPP_INFO(this->get_logger(),
-            "listener='%s', publisher='%s'", topic_subscriber.c_str(), topic_publisher.c_str()
-        );
-    }
-
-private:
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
-
-    void message_callback(const std_msgs::msg::String::SharedPtr msg) {
-        auto new_msg = std_msgs::msg::String();
-        new_msg.data = trim(msg->data, " ") + " makkelijk";
-
-        RCLCPP_INFO(this->get_logger(),
-            "incoming='%s', outgoing='%s'", msg->data.c_str(), new_msg.data.c_str()
-        );
-
-        publisher_->publish(new_msg);
-    }
-
-    std::string trim(const std::string& str, const std::string& whitespace = " \t") {
-        const auto strBegin = str.find_first_not_of(whitespace);
-        if (strBegin == std::string::npos) return "";
-
-        const auto strEnd = str.find_last_not_of(whitespace);
-        const auto strRange = strEnd - strBegin + 1;
-
-        return str.substr(strBegin, strRange);
-    }
-};
-
-int main(int argc, char *argv[]) {
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<SoupRelayNode>());
-    rclcpp::shutdown();
-    return 0;
-}

src/usb_cam_wessel_pkg/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(usb_cam_wessel_pkg)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+    DIRECTORY config/
+    DESTINATION share/${PROJECT_NAME}/config
+)
+
+ament_package()

src/usb_cam_wessel_pkg/config/params.yaml

@@ -0,0 +1,23 @@
+/usb_cam:
+  ros__parameters:
+    video_device: "/dev/video0"
+    framerate: 30.0
+    io_method: "mmap"
+    frame_id: "camera"
+    pixel_format: "mjpeg2rgb"
+    color_format: "yuv422p"
+    image_width: 640
+    image_height: 480
+    camera_name: "usb_cam"
+    camera_info_url: ""
+    brightness: -1
+    contrast: -1
+    saturation: -1
+    sharpness: -1
+    gain: -1
+    auto_white_balance: true
+    white_balance: 4000
+    autoexposure: true
+    exposure: 100
+    autofocus: false
+    focus: -1

src/usb_cam_wessel_pkg/package.xml

@@ -0,0 +1,15 @@
+<?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>usb_cam_wessel_pkg</name>
+  <version>0.0.0</version>
+  <description>USB camera package for ROS2</description>
+  <maintainer email="contact@wessel.gg">wessel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>