Pipeline filtering

.gitignore

@@ -25,6 +25,9 @@ devel_isolated/
 *.dox
 *.wikidoc
 
+# vscode stuff
+.vscode
+
 # eclipse stuff
 .project
 .cproject

pipeline-filtering/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.8)
+project(pipeline_filtering)
+
+# === C++ standard ===
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+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(rclcpp_components REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(OpenCV 4.5.4 REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(vortex_msgs REQUIRED)
+
+include_directories(include)
+
+set(LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${LIB_NAME} SHARED
+  src/pipeline_processing.cpp
+  src/pipeline_filtering_ros.cpp
+)
+
+target_link_libraries(${LIB_NAME} PUBLIC
+  ${OpenCV_LIBS}
+)
+
+target_include_directories(${LIB_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  rclcpp_components
+  rclcpp_action
+  cv_bridge
+  sensor_msgs
+  vortex_msgs
+)
+
+rclcpp_components_register_node(
+    ${LIB_NAME}
+    PLUGIN "vortex::pipeline_processing::PipelineFilteringNode"
+    EXECUTABLE ${PROJECT_NAME}_node
+)
+
+# Export the target for other packages to use
+ament_export_targets(export_${LIB_NAME})
+
+install(TARGETS ${LIB_NAME}
+  EXPORT export_${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  params
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+    add_subdirectory(test)
+endif()
+
+ament_package()

pipeline-filtering/include/pipeline_filters/pipeline_filtering_ros.hpp

@@ -0,0 +1,128 @@
+#ifndef PIPELINE_FILTERING_ROS_HPP
+#define PIPELINE_FILTERING_ROS_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/msg/image.hpp>
+#include "std_msgs/msg/int32.hpp"
+#include "std_msgs/msg/float32.hpp"
+#include <rclcpp/qos.hpp>
+#include "pipeline_processing.hpp"
+#include <rclcpp/parameter_event_handler.hpp>
+
+namespace vortex::pipeline_processing
+{
+class PipelineFilteringNode : public rclcpp::Node{
+
+public:
+    explicit PipelineFilteringNode(const rclcpp::NodeOptions & options);
+    ~PipelineFilteringNode(){};
+
+
+private:
+    /**
+     * @brief Subscribes to image topic
+    */
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+    /**
+     * @brief Publishes the filtered image
+    */
+    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
+
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr optimal_threshold_publisher_;
+
+    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr auto_gamma_publisher_;
+
+    /**
+     * @brief Check and subscribe to image if not yet subscribed. Allows for dynaminc reconfiguration of image topic.
+     * If a new topic is set, the old subscription is cancelled and a new one is bound to the callback function.
+     * 
+    */
+    void check_and_subscribe_to_image_topic();
+
+    /**
+     * @brief Create publisher for filtered image
+     * 
+    */
+    void create_image_publisher();
+
+    /**
+     * @brief Set the filter parameters for the FilterParams struct.
+     * 
+    */
+    void set_filter_params();
+
+    /**
+     * @brief Initialize the parameter handler and a parameter event callback.
+     * 
+    */
+    void initialize_parameter_handler();
+    /**
+     * @brief Callback function for parameter events.
+     * Checks for parameter changes that matches the nodes' namespace and invokes the relevant initializer functions to update member variables.
+     * 
+     * @param event The parameter event.
+    */  
+    void on_parameter_event(const rcl_interfaces::msg::ParameterEvent &event);
+
+    /**
+     * @brief Manages parameter events for the node.
+     *
+     * This handle is used to set up a mechanism to listen for and react to changes in parameters. 
+     * Parameters can be used to configure the node's operational behavior dynamically, 
+     * allowing adjustments without altering the code. The `param_handler_` is responsible for 
+     * registering callbacks that are triggered on parameter changes, providing a centralized 
+     * management system within the node for such events.
+     */
+    std::shared_ptr<rclcpp::ParameterEventHandler> param_handler_;
+
+    /**
+     * @brief Handle to the registration of the parameter event callback.
+     *
+     * Represents a token or reference to the specific callback registration made with 
+     * the parameter event handler (`param_handler_`). This handle allows for management 
+     * of the lifecycle of the callback, such as removing the callback if it's no longer needed. 
+     * It ensures that the node can respond to parameter changes with the registered callback 
+     * in an efficient and controlled manner.
+     */
+    rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_;
+
+    /**
+     * @brief Callback function for image topic
+     * 
+     * @param msg The image message
+    */
+    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+
+    /**
+     * @brief The image topic to subscribe to
+     * 
+    */
+    std::string image_topic_;
+
+    /**
+     * @brief The image topic to publish to
+     * 
+    */
+    std::string image_pub_topic_;
+
+    /**
+     * @brief The filter parameters
+     * 
+    */
+    FilterParams filter_params_;
+
+    /**
+     * @brief filter to apply
+     * 
+    */
+    std::string filter_;
+
+    bool confirmed_;
+    int confirmed_counter_;
+};
+
+} // namespace vortex::pipeline_processing
+
+
+#endif // PIPELINE_FILTERING_ROS_HPP

pipeline-filtering/include/pipeline_filters/pipeline_processing.hpp

@@ -0,0 +1,53 @@
+#ifndef PIPELINE_PROCESSING_HPP
+#define PIPELINE_PROCESSING_HPP
+
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/xphoto.hpp>
+#include <iostream>
+#include <map>
+
+namespace vortex::pipeline_processing
+{
+
+
+struct OtsuParams {
+    bool gamma_auto_correction;
+    double gamma_auto_correction_weight;
+    bool otsu_segmentation;
+    double gsc_weight_r;
+    double gsc_weight_g;
+    double gsc_weight_b;
+};
+
+struct FilterParams {
+    OtsuParams otsu;
+};
+
+typedef void (*FilterFunction)(const FilterParams&, const cv::Mat&, cv::Mat&);
+
+/**
+ * Reads the filter_type from the FilterParams struct 
+ * and calls the appropriate filter function from the filter_functions map.
+ */
+void apply_filter(const std::string& filter, const FilterParams& params, const cv::Mat &original, cv::Mat &output);
+
+/**
+ * No filter, just copy the image
+ */
+void no_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &output);
+
+/**
+ * A filter based on Otsu's method
+ */
+void otsu_segmentation_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &output);
+
+const static std::map<std::string, FilterFunction> filter_functions ={
+	{"no_filter", no_filter},
+    {"otsu", otsu_segmentation_filter}};
+
+
+} // namespace pipeline_processing
+#endif // PIPELINE_PROCESSING_HPP

pipeline-filtering/launch/pipeline_filtering.launch.py

@@ -0,0 +1,16 @@
+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():
+    pipeline_filtering_node = Node(
+            package='pipeline_filtering',
+            executable='pipeline_filtering_node',
+            name='pipeline_filtering_node',
+            parameters=[os.path.join(get_package_share_directory('pipeline_filtering'),'params','pipeline_filtering_params.yaml')],
+            output='screen',
+        )
+    return LaunchDescription([
+        pipeline_filtering_node
+    ])

pipeline-filtering/package.xml

@@ -0,0 +1,30 @@
+<?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>pipeline_filtering</name>
+  <version>0.0.1</version>
+  <description>The pipeline filtering package</description>
+  <maintainer email="[email protected]">Simon Edna</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>sensor_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
+  <depend>action_msgs</depend>
+  <depend>vortex_msgs</depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

pipeline-filtering/params/pipeline_filtering_params.yaml

@@ -0,0 +1,16 @@
+pipeline_filtering_node:
+  ros__parameters:
+    sub_topic: "/gripper_camera/image_raw"
+    pub_topic: "/filtered_image"
+    filter_params:
+      filter_type: "otsu"
+      otsu:
+        gsc_weight_r: 1.0  # Grayscale red weight
+        gsc_weight_g: 1.0  # Grayscale green weight
+        gsc_weight_b: 1.0    # Grayscale blue weight
+        gamma_auto_correction: true
+        gamma_auto_correction_weight: 2.0 
+        otsu_segmentation: true
+
+# Filter params should reflect the FilterParams struct 
+# defined in /include/pipeline_filters/pipeline_processing.hpp