feat: Final openCV assignment

.fish/activate.fish

@@ -1,9 +1,9 @@
-# Self-contained environment: ros2-lectures
-# Exported on: Sat Dec 13 05:03:27 PM 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,7 +63,7 @@ alias cbt="colcon build --packages-select"
 alias ct="colcon test"
 alias ctr="colcon test-result"
 
-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,17 +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>pose_calculator</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>tf2_ros</depend>
-    <depend>geometry_msgs</depend>
-    <depend>sensor_msgs</depend>
+
+  <depend>rclcpp</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

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

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/pose_calculator/CMakeLists.txt

@@ -1,36 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(pose_calculator)
-
-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(tf2_ros REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(sensor_msgs REQUIRED)
-
-# set dependencies
-set(dependencies
-rclcpp
-tf2_ros
-geometry_msgs
-sensor_msgs
-)
-
-# add executables for scripts
-add_executable(ik_node src/InverseKinematicNode.cpp)
-ament_target_dependencies(ik_node ${dependencies})
-
-# install the scripts
-install(TARGETS
-ik_node
-DESTINATION lib/${PROJECT_NAME})
-install(
-DIRECTORY launch
-DESTINATION share/${PROJECT_NAME}
-)
-
-ament_package()

src/pose_calculator/launch/inverse_kinematic.launch.xml

@@ -1,20 +0,0 @@
-<launch>
-    <!-- Launch arguments for target position -->
-    <arg name="target_x" default="0.29" description="Target X position"/>
-    <arg name="target_y" default="0.29" description="Target Y position"/>
-
-    <let name="urdf_path" value="$(find-pkg-share robot_description)/urdf/skyentific_robot.urdf.xacro"/>
-    <let name="rviz_config_path" value="$(find-pkg-share robot_description)/rviz/display.rviz"/>
-
-    <node pkg="robot_state_publisher" exec="robot_state_publisher">
-        <param name="robot_description" value="$(command 'xacro $(var urdf_path)')" />
-    </node>
-
-    <node pkg="pose_calculator" exec="ik_node">
-        <param name="target_x" value="$(var target_x)"/>
-        <param name="target_y" value="$(var target_y)"/>
-    </node>
-
-    <node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config_path)" />
-    <node pkg="rqt_reconfigure" exec="rqt_reconfigure" output="screen" />
-</launch>

src/pose_calculator/src/InverseKinematicNode.cpp

@@ -1,193 +0,0 @@
-#include <cmath>
-
-#include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/msg/joint_state.hpp>
-
-class InverseKinematicNode : public rclcpp::Node {
-public:
-    InverseKinematicNode() : Node("skyentific_pose_calculator_node") {
-        auto target_x_desc = rcl_interfaces::msg::ParameterDescriptor();
-        target_x_desc.description = "Target X position";
-        target_x_desc.read_only = false;
-        target_x_desc.floating_point_range.resize(1);
-        target_x_desc.floating_point_range[0].from_value = -0.5;
-        target_x_desc.floating_point_range[0].to_value = 0.5;
-        target_x_desc.floating_point_range[0].step = 0.01;
-
-        auto target_y_desc = rcl_interfaces::msg::ParameterDescriptor();
-        target_y_desc.description = "Target Y position";
-        target_y_desc.read_only = false;
-        target_y_desc.floating_point_range.resize(1);
-        target_y_desc.floating_point_range[0].from_value = -0.5;
-        target_y_desc.floating_point_range[0].to_value = 0.5;
-        target_y_desc.floating_point_range[0].step = 0.01;
-
-        target_x_ = this->declare_parameter<double>("target_x", 0.29, target_x_desc);
-        target_y_ = this->declare_parameter<double>("target_y", 0.29, target_y_desc);
-
-        link1_ = this->declare_parameter<double>("L1", 0.225);
-        link2_ = this->declare_parameter<double>("L2", 0.383);
-
-        // Set up parameter callback for dynamic reconfigure
-        param_callback_handle_ = this->add_on_set_parameters_callback(
-            std::bind(&InverseKinematicNode::callback_parameters, this, std::placeholders::_1)
-        );
-
-        if (!calculate_2dpose(target_x_, target_y_)) {
-            RCLCPP_ERROR(this->get_logger(), "failed calculating pose (x=%f, y=%f)", target_x_, target_y_);
-        } else {
-            RCLCPP_INFO(this->get_logger(),
-                "target position (x=%f, y=%f) [j1=%f, j2=%f, j3=%f, j4=%f, j5=%f, j6=%f]",
-                target_x_, target_y_, joint_1_, joint_2_, joint_3_, joint_4_, joint_5_, joint_6_
-            );
-        }
-
-        joint_state_publisher_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
-
-        timer_ = this->create_wall_timer(
-            std::chrono::milliseconds(100),
-            std::bind(&InverseKinematicNode::publish_joint_state, this));
-    }
-
-private:
-    rcl_interfaces::msg::SetParametersResult callback_parameters(
-        const std::vector<rclcpp::Parameter> &parameters
-    ) {
-
-        rcl_interfaces::msg::SetParametersResult result;
-        result.successful = true;
-
-        bool recalculate = false;
-        double new_x = target_x_;
-        double new_y = target_y_;
-
-        for (const auto &param : parameters) {
-            if (param.get_name() == "target_x") {
-                new_x = param.as_double();
-                recalculate = true;
-                RCLCPP_INFO(this->get_logger(), "changed 'target_x' -> %.4f", new_x);
-            } else if (param.get_name() == "target_y") {
-                new_y = param.as_double();
-                recalculate = true;
-                RCLCPP_INFO(this->get_logger(), "changed 'target_y' -> %.4f", new_y);
-            }
-        }
-
-        if (recalculate) {
-            if (calculate_2dpose(new_x, new_y)) {
-                target_x_ = new_x;
-                target_y_ = new_y;
-                result.successful = true;
-                result.reason = "pose calculation successful";
-            } else {
-                result.successful = false;
-                result.reason = "target position unreachable";
-                RCLCPP_WARN(this->get_logger(), "failed to calculate pose, keeping previous values");
-            }
-        }
-
-        return result;
-    }
-
-private:
-    OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
-    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
-    rclcpp::TimerBase::SharedPtr timer_;
-
-    double target_x_, target_y_;
-    double link1_, link2_;
-    double joint_1_, joint_2_, joint_3_, joint_4_, joint_5_, joint_6_;
-
-    bool calculate_2dpose(double x, double y) {
-        RCLCPP_INFO(this->get_logger(), "0. x=%.4f, y=%.4f, l1=%.4f, l2=%.4f", x, y, link1_, link2_);
-
-        double r = std::sqrt(x * x + y * y);
-        RCLCPP_INFO(this->get_logger(), "1. r=%.6f", r);
-
-        // joint 1 = base rotation
-        joint_1_ = 0.0;
-        // joint_1_ = std::atan2(y, x);
-
-        // Check if target is reachable
-        double max_reach = link1_ + link2_;
-        double min_reach = std::abs(link1_ - link2_);
-        RCLCPP_INFO(this->get_logger(), "3. reach_min=%.4f, reach_max=%.4f", min_reach, max_reach);
-
-        if (r > max_reach || r < min_reach) {
-            RCLCPP_ERROR(this->get_logger(), "e. target out of reach, aborting");
-            return false;
-        }
-
-        // Joint 2: cos(Joint2) = (r^2 - Link1^2 - Link2^2) / (2 * Link1 * Link2)
-        double r_squared = r * r;
-        double link1_squared = link1_ * link1_;
-        double link2_squared = link2_ * link2_;
-
-        double cos_joint2 = (r_squared - link1_squared - link2_squared) / (2.0 * link1_ * link2_);
-
-        RCLCPP_INFO(this->get_logger(),
-            "4. cos_joint2=(%.6f - %.6f - %.6f) / (2*%.3f*%.3f) = %.6f",
-            r_squared, link1_squared, link2_squared, link1_, link2_, cos_joint2
-        );
-
-        joint_2_ = std::acos(cos_joint2);
-        RCLCPP_INFO(this->get_logger(), "4. joint_2=%.6f rad", joint_2_);
-
-        // Joint 3: Joint3 = atan2(y, x) - atan2(Link2*sin(Joint2), Link1 + Link2*cos(Joint2))
-        double atan2_yx = std::atan2(y, x);
-        double link2_sin_j2 = link2_ * std::sin(joint_2_);
-        double link1_plus_link2_cos_joint2 = link1_ + link2_ * std::cos(joint_2_);
-        double atan2_term = std::atan2(link2_sin_j2, link1_plus_link2_cos_joint2);
-
-        RCLCPP_INFO(this->get_logger(), "5. atan2(%.2f, %.2f) = %.6f", y, x, atan2_yx);
-        RCLCPP_INFO(this->get_logger(), "5. atan2(%.3f*sin(%.6f), %.3f + %.3f*cos(%.6f))",
-            link2_, joint_2_, link1_, link2_, joint_2_
-        );
-        RCLCPP_INFO(this->get_logger(), "5. atan2(%.6f, %.6f) = %.6f",
-            link2_sin_j2, link1_plus_link2_cos_joint2, atan2_term
-        );
-
-        joint_3_ = atan2_yx - atan2_term;
-        RCLCPP_INFO(this->get_logger(), "5. joint_3=%.6f - %.6f=%.6f rad", atan2_yx, atan2_term, joint_3_);
-
-        // joints 4-6 related to gripper
-        joint_4_ = 0.0;
-        joint_5_ = 0.0;
-        joint_6_ = 0.0;
-
-        return true;
-    }
-
-    void publish_joint_state() {
-        auto joint_state = sensor_msgs::msg::JointState();
-        joint_state.header.stamp = this->get_clock()->now();
-
-        joint_state.name = {
-            "joint_1",
-            "joint_2",
-            "joint_3",
-            "joint_4",
-            "joint_5",
-            "joint_6"
-        };
-
-        joint_state.position = {
-            joint_1_,
-            joint_2_,
-            joint_3_,
-            joint_4_,
-            joint_5_,
-            joint_6_
-        };
-
-        joint_state_publisher_->publish(joint_state);
-    }
-};
-
-int main(int argc, char** argv) {
-    rclcpp::init(argc, argv);
-    auto node = std::make_shared<InverseKinematicNode>();
-    rclcpp::spin(node);
-    rclcpp::shutdown();
-    return 0;
-}

src/robot_description/CMakeLists.txt

@@ -1,17 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(robot_description)
-
-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)
-
-# Install directories
-install(
-    DIRECTORY launch meshes urdf rviz
-    DESTINATION share/${PROJECT_NAME}
-)
-
-ament_package()

src/robot_description/launch/display.launch.xml

@@ -1,26 +0,0 @@
-<launch>
-    <let name="urdf_path"
-        value="$(find-pkg-share robot_description)/urdf/skyentific_robot.urdf.xacro"
-    />
-
-    <let name="rviz_config_path"
-        value="$(find-pkg-share robot_description)/rviz/display.rviz"
-    />
-
-    <node pkg="robot_state_publisher"
-        exec="robot_state_publisher"
-    >
-        <param name="robot_description"
-        value="$(command 'xacro $(var urdf_path)')"/>
-    </node>
-
-    <node pkg="joint_state_publisher_gui"
-        exec="joint_state_publisher_gui"
-    />
-
-    <node pkg="rviz2"
-        exec="rviz2"
-        output="screen"
-        args="-d $(var rviz_config_path)"
-    />
-</launch>

src/robot_description/meshes/readme.md

@@ -1 +0,0 @@
-meshes to be used with lesson 1

src/robot_description/rviz/display.rviz

@@ -1,239 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 85
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /RobotModel1/Description Topic1
-      Splitter Ratio: 0.5
-    Tree Height: 517
-  - 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:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_description
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        arm_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        elbow_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        forearm_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        hand_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        shoulder_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        tool_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        wrist_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Mass Properties:
-        Inertia: false
-        Mass: false
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Filter (blacklist): ""
-      Filter (whitelist): ""
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-        arm_link:
-          Value: true
-        base_link:
-          Value: true
-        elbow_link:
-          Value: true
-        forearm_link:
-          Value: true
-        hand_link:
-          Value: true
-        shoulder_link:
-          Value: true
-        tool_link:
-          Value: true
-        wrist_link:
-          Value: true
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: false
-      Tree:
-        base_link:
-          shoulder_link:
-            arm_link:
-              elbow_link:
-                forearm_link:
-                  wrist_link:
-                    hand_link:
-                      tool_link:
-                        {}
-      Update Interval: 0
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: base_link
-    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: 2.5
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0.0
-        Y: 0.0
-        Z: 0.5
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.785
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 854
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000160000002a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000002a3000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000048000002a3000000c500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bd00000042fc0100000002fb0000000800540069006d00650100000000000004bd000002dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000356000002a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1213
-  X: 400
-  Y: 84

src/robot_description/rviz/readme.md

@@ -1 +0,0 @@
-rviz2 config file

src/robot_description/urdf/skyentific_robot.urdf.xacro

@@ -1,263 +0,0 @@
-<?xml version="1.0"?>
-<robot
-    name="skyentific_robot"
-    xmlns:xacro="http://www.ros.org/wiki/xacro"
->
-    <!-- Include ros2_control Plugins -->
-    <xacro:include
-        filename="$(find robot_description)/urdf/skyentific_robot_ros2_control.xacro"
-    />
-    <!-- Useful XACRO Variables (Properties) -->
-    <xacro:property name="PI" value="3.14159265359" />
-    <material name="grey">
-        <color rgba="0.5 0.5 0.5 1.0"/>
-    </material>
-    <material name="red">
-        <color rgba="0.5 0.0 0.0 1.0"/>
-    </material>
-
-    <link name="base_link">
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-            <geometry>
-                <mesh
-                    filename="file://$(find robot_description)/meshes/base_z_ob.stl"
-                    scale="0.001 0.001 0.001"
-                />
-            </geometry>
-            <material name="grey" />
-        </visual>
-        <collision> <!-- cyclinder equivalent -->
-            <origin xyz="0 0 0.075" rpy="0 0 0"/>
-            <geometry>
-                <cylinder radius="0.1" length="0.15"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <link name="link_1">
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-            <geometry>
-                <mesh
-                    filename="file://$(find robot_description)/meshes/link_1.stl"
-                    scale="0.001 0.001 0.001"
-                />
-            </geometry>
-            <material name="grey" />
-        </visual>
-        <collision>
-            <origin xyz="0 0 0.150" rpy="1.57 0 0"/>
-            <geometry>
-                <cylinder radius="0.095" length="0.1"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="joint_1" type="revolute">
-        <parent link="base_link"/>
-        <child link="link_1"/>
-        <origin xyz="0 0 0.0965" rpy="0 0 0"/>
-        <axis xyz="0 0 1"/>
-        <limit
-            effort="1000.0"
-            velocity="100.0"
-            lower="-3.14"
-            upper="3.14"
-        />
-    </joint>
-
-    <link name="link_2">
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-            <geometry>
-                <mesh
-                    filename="file://$(find robot_description)/meshes/link_2.stl"
-                    scale="0.001 0.001 0.001"
-                />
-            </geometry>
-            <material name="grey"/>
-        </visual>
-        <collision>
-            <origin xyz="0.225 0 0" rpy="0 0 0"/>
-            <geometry>
-                <cylinder radius="0.07" length="0.1"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="joint_2" type="revolute">
-        <parent link="link_1"/>
-        <child link="link_2"/>
-        <origin xyz="0 0 0.150" rpy="0 -1.57 1.57"/>
-        <axis xyz="0 0 1"/>
-        <limit
-            effort="1000.0"
-            velocity="100.0"
-            lower="-2.51"
-            upper="2.51"
-        />
-    </joint>
-
-    <link name="link_3">
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-            <geometry>
-                <mesh
-                    filename="file://$(find robot_description)/meshes/link_3.stl"
-                    scale="0.001 0.001 0.001"
-                />
-            </geometry>
-            <material name="grey" />
-        </visual>
-        <collision>
-            <origin xyz="0.2 0 0" rpy="0 0 0"/>
-            <geometry>
-                <cylinder radius="0.055" length="0.1"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="joint_3" type="revolute">
-        <parent link="link_2"/>
-        <child link="link_3"/>
-        <origin xyz="0.225 0 0" rpy="0 0 0"/>
-        <axis xyz="0 0 1"/>
-        <limit
-            effort="1000.0"
-            velocity="100.0"
-            lower="-2.51"
-            upper="2.51"
-        />
-    </joint>
-
-    <link name="link_4">
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-            <geometry>
-                <mesh
-                    filename="file://$(find robot_description)/meshes/link_4.stl"
-                    scale="0.001 0.001 0.001"
-                />
-            </geometry>
-            <material name="grey" />
-        </visual>
-        <collision>
-            <origin xyz="0.1 0 0" rpy="0 1.57 0"/>
-            <geometry>
-                <cylinder radius="0.055" length="0.06"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="joint_4" type="revolute">
-        <parent link="link_3"/>
-        <child link="link_4"/>
-        <origin xyz="0.2 0 0" rpy="0 0 0"/>
-        <axis xyz="0 0 1"/>
-        <limit
-            effort="1000.0"
-            velocity="100.0"
-            lower="-2.51"
-            upper="2.51"
-        />
-    </joint>
-
-    <link name="link_5">
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-            <geometry>
-                <mesh
-                    filename="file://$(find robot_description)/meshes/link_5_gripper.stl"
-                    scale="0.001 0.001 0.001"
-                />
-            </geometry>
-            <material name="grey" />
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="1.57 0 0"/>
-            <geometry>
-                <cylinder radius="0.055" length="0.01"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="joint_5" type="revolute">
-        <parent link="link_4"/>
-        <child link="link_5"/>
-        <origin xyz="0.0915 0 0" rpy="0 1.57 0"/>
-        <axis xyz="0 0 1"/>
-        <limit
-            effort="1000.0"
-            velocity="100.0"
-            lower="-2.51"
-            upper="2.51"
-        />
-    </joint>
-
-    <link name="gripper_right">
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-            <geometry>
-                <mesh
-                    filename="file://$(find robot_description)/meshes/rightjaw.stl"
-                    scale="0.001 0.001 0.001"
-                />
-            </geometry>
-            <material name="red"/>
-        </visual>
-        <collision>
-            <origin xyz="0.01 0.025 0.045" rpy="0 0 0"/>
-            <geometry>
-                <cylinder radius="0.0125" length="0.04"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="joint_6" type="prismatic">
-        <parent link="link_5"/>
-        <child link="gripper_right"/>
-        <origin xyz="-0.01 0 0.1015" rpy="0 0 0"/>
-        <axis xyz="0 1 0"/>
-        <limit
-            effort="1000.0"
-            velocity="100.0"
-            lower="-0.018"
-            upper="0.02"
-        />
-    </joint>
-
-    <link name="gripper_left">
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-            <geometry>
-                <mesh
-                    filename="file://$(find robot_description)/meshes/leftjaw.stl"
-                    scale="0.001 0.001 0.001"
-                />
-            </geometry>
-            <material name="red"/>
-        </visual>
-        <collision>
-            <origin xyz="0.01 -0.025 0.045" rpy="0 0 0"/>
-            <geometry>
-                <cylinder radius="0.0125" length="0.04"/>
-            </geometry>
-        </collision>
-    </link>
-
-    <joint name="joint_7" type="prismatic">
-        <parent link="link_5"/>
-        <child link="gripper_left"/>
-        <origin xyz="-0.01 0 0.1015" rpy="0 0 0"/>
-        <axis xyz="0 1 0"/>
-        <mimic joint="joint_6" multiplier="-1"/>
-        <limit
-            effort="1000.0"
-            velocity="100.0"
-            lower="-0.018"
-            upper="0.02"
-        />
-    </joint>
-
-</robot>

src/robot_description/urdf/skyentific_robot_ros2_control.xacro

@@ -1,50 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <ros2_control name="Arm" type="system">
-        <hardware>
-            <plugin>mock_components/GenericSystem</plugin>
-        </hardware>
-        <joint name="joint_1">
-            <command_interface name="position"/>
-            <state_interface name="position">
-                <param name="initial_value">0.0</param>
-            </state_interface>
-        </joint>
-        <joint name="joint_2">
-            <command_interface name="position"/>
-            <state_interface name="position">
-                <param name="initial_value">0.0</param>
-            </state_interface>
-        </joint>
-        <joint name="joint_3">
-            <command_interface name="position"/>
-            <state_interface name="position">
-                <param name="initial_value">0.0</param>
-            </state_interface>
-        </joint>
-        <joint name="joint_4">
-            <command_interface name="position"/>
-            <state_interface name="position">
-                <param name="initial_value">0.0</param>
-            </state_interface>
-        </joint>
-        <joint name="joint_5">
-            <command_interface name="position"/>
-            <state_interface name="position">
-                <param name="initial_value">0.0</param>
-            </state_interface>
-        </joint>
-    </ros2_control>
-
-    <ros2_control name="Gripper" type="system">
-        <hardware>
-            <plugin>mock_components/GenericSystem</plugin>
-        </hardware>
-        <joint name="joint_6">
-            <command_interface name="position"/>
-            <state_interface name="position">
-                <param name="initial_value">0.0</param>
-            </state_interface>
-        </joint>
-    </ros2_control>
-</robot>

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>