diff --git a/.github/workflows/code-coverage.yml b/.github/workflows/code-coverage.yml index 7b9b6db..d2c9091 100644 --- a/.github/workflows/code-coverage.yml +++ b/.github/workflows/code-coverage.yml @@ -8,5 +8,8 @@ on: jobs: code-coverage: uses: vortexntnu/vortex-ci/.github/workflows/reusable-code-coverage.yml@main + with: + vcs-repo-file-url: './dependencies.repos' + before_install_target_dependencies: './scripts/ci_install_dependencies.sh' secrets: CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} # Set in the repository secrets diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index 23b5750..3950afc 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -13,4 +13,6 @@ jobs: industrial-ci: uses: vortexntnu/vortex-ci/.github/workflows/reusable-industrial-ci.yml@main with: + upstream_workspace: './dependencies.repos' + before_install_upstream_dependencies: './scripts/ci_install_dependencies.sh' ros_repo: '["testing", "main"]' diff --git a/.gitignore b/.gitignore index 5d05913..2a027b3 100644 --- a/.gitignore +++ b/.gitignore @@ -25,7 +25,6 @@ dist/ downloads/ eggs/ .eggs/ -lib/ lib64/ parts/ sdist/ @@ -210,7 +209,6 @@ devel/ logs/ build/ bin/ -lib/ msg_gen/ srv_gen/ msg/*Action.msg diff --git a/dependencies.repos b/dependencies.repos new file mode 100644 index 0000000..116cec4 --- /dev/null +++ b/dependencies.repos @@ -0,0 +1,7 @@ +repositories: + vortex-msgs: + type: git + url: https://github.com/vortexntnu/vortex-msgs.git + vortex-utils: + type: git + url: https://github.com/vortexntnu/vortex-utils.git diff --git a/line_detection_houghp/CMakeLists.txt b/line_detection_houghp/CMakeLists.txt new file mode 100644 index 0000000..0bc663e --- /dev/null +++ b/line_detection_houghp/CMakeLists.txt @@ -0,0 +1,100 @@ +cmake_minimum_required(VERSION 3.8) +project(line_detection_houghp) + +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_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(OpenCV 4.5.4 REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +set(CORE_LIB_NAME "${PROJECT_NAME}") + +add_library(${CORE_LIB_NAME} SHARED + src/lib/houghp_line_detection.cpp + src/lib/utils.cpp +) + +target_include_directories(${CORE_LIB_NAME} + PUBLIC + $ + $ + ${OpenCV_INCLUDE_DIRS} +) + +ament_target_dependencies(${CORE_LIB_NAME} + vortex_utils +) + +target_link_libraries(${CORE_LIB_NAME} + ${OpenCV_LIBS}) + +set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") + +add_library(${COMPONENT_LIB_NAME} SHARED + src/ros/${PROJECT_NAME}_ros.cpp + src/ros/${PROJECT_NAME}_ros_utils.cpp +) + +ament_target_dependencies(${COMPONENT_LIB_NAME} + rclcpp + rclcpp_components + vortex_utils + vortex_utils_ros + vortex_msgs + cv_bridge + sensor_msgs + spdlog + fmt +) + +target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME}) + +rclcpp_components_register_node( + ${COMPONENT_LIB_NAME} + PLUGIN "LineDetectionHoughPNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +install( + TARGETS + ${CORE_LIB_NAME} + ${COMPONENT_LIB_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_export_include_directories(include) +ament_export_libraries(${CORE_LIB_NAME}) + +ament_package() diff --git a/line_detection_houghp/README.md b/line_detection_houghp/README.md new file mode 100644 index 0000000..b210a5a --- /dev/null +++ b/line_detection_houghp/README.md @@ -0,0 +1,108 @@ +# Line Detection HoughP + +## Overview + +The `line_detection_houghp` package provides line detection functionality using the Probabilistic Hough Transform (HoughP) for images. It is designed to detect linear features such as pipelines, walls, and other structural elements in underwater environments. + +## Architecture + +The package follows the standard Vortex separation of concerns: + +``` +line_detection_houghp/ +├── include/ +│ └── line_detection_houghp/ +│ ├── lib/ # Non-ROS library headers +│ │ └── houghp_line_detection.hpp +│ └── ros/ # ROS node headers +│ └── line_detection_ros.hpp +├── src/ +│ ├── lib/ # Non-ROS library implementations +│ │ └── houghp_line_detection.cpp +│ └── ros/ # ROS node implementations +│ └── line_detection_ros.cpp +├── launch/ +│ └── line_detection.launch.py +├── config/ +│ └── line_detection_params.yaml +├── test/ +│ └── test_houghp_line_detection.cpp +├── CMakeLists.txt +├── package.xml +└── README.md +``` + +**lib/** contains pure C++/OpenCV logic with no ROS dependencies. This makes it testable and reusable outside of ROS. + +**ros/** contains the ROS 2 node that wraps the library, handling subscriptions, publishers, parameters, and message conversions. + +## Topics + +### Subscriptions + +| Topic | Type | Description | +|-------|------|-------------| +| Input image | `sensor_msgs/msg/Image` | Input image for line detection (8-bit, single or multi-channel) | + +### Publications + +| Topic | Type | Modes | Description | +|-------|------|-------|-------------| +| Detected lines | `vortex_msgs/msg/LineSegment2DArray` | `standard` `visualize` `debug` | Detected line segments (Always published). | +| Color overlay | `sensor_msgs/msg/Image` (BGR, 8UC3) | `visualize` `debug` | Input color image with detected line segments drawn on top | +| Canny edge map | `sensor_msgs/msg/Image` (mono, 8UC1) | `debug` | Raw Canny edge map | +| Canny overlay | `sensor_msgs/msg/Image` (BGR, 8UC3) | `debug` | Edge map converted to BGR with detected line segments drawn on top | + + +## Parameters + +### Canny Edge Detection + +The Canny edge detector is used as a pre-processing step to extract edges before the Hough transform. It uses hysteresis thresholding with two thresholds to identify strong and weak edges. + +| Parameter | Type | Description | +|-----------|------|-------------| +| `canny_low_threshold` | `int` | Low threshold for the hysteresis procedure. Edges with gradient magnitude below this are discarded. | +| `canny_high_threshold` | `int` | High threshold for the hysteresis procedure. Edges with gradient magnitude above this are always accepted. Edges between low and high are accepted only if connected to a strong edge. | +| `canny_aperture_size` | `int` | Aperture size for the Sobel operator used internally by Canny. Must be odd (3, 5, or 7). Larger values detect smoother edges. | +| `canny_L2_gradient` | `bool` | If `true`, uses the more accurate L2 norm (√(Gx² + Gy²)) for gradient magnitude instead of the default L1 norm (\|Gx\| + \|Gy\|). | + +### Probabilistic Hough Transform + +| Parameter | Type | Description | +|-----------|------|-------------| +| `hough_rho` | `double` | Distance resolution of the accumulator in pixels | +| `hough_theta` | `double` | Angle resolution of the accumulator in radians | +| `hough_threshold` | `int` | Accumulator threshold — minimum number of votes to accept a line | +| `min_line_length` | `double` | Minimum line length to accept (pixels). Lines shorter than this are rejected. | +| `max_line_gap` | `double` | Maximum allowed gap between points on the same line to link them into a single segment (pixels) | + + + +> **Note:** Adjust parameters in the config YAML or via `ros2 param set` at runtime. + +## Build + +```bash +# Build this package and its dependencies +colcon build --packages-up-to line_detection_houghp --symlink-install +source install/setup.bash +``` + +## Run + +```bash +# Launch the node with default parameters +ros2 launch line_detection_houghp line_detection.launch.py +``` + + +## Theory + +The [Probabilistic Hough Transform](https://docs.opencv.org/4.x/dd/d1a/group__imgproc__feature.html#ga8618180a5948286384e3b7ca02f6feeb) (`cv::HoughLinesP`) is a variant of the Standard Hough Transform that returns line segments rather than infinite lines. It works by: + +1. Randomly sampling edge points from a binary image. +2. Accumulating votes in (ρ, θ) parameter space. +3. Extracting line segments that exceed the vote threshold and meet minimum length / maximum gap constraints. + +Detected line segments are converted to polar (Hesse normal) form using `vortex::utils::types::LineSegment2D::polar_parametrization()` for downstream consumers. diff --git a/line_detection_houghp/config/line_detection_houghp.yaml b/line_detection_houghp/config/line_detection_houghp.yaml new file mode 100644 index 0000000..5e6273d --- /dev/null +++ b/line_detection_houghp/config/line_detection_houghp.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + topic: + image_sub_topic: /fls_publisher/display_mono + line_segments_pub_topic: line_detection/line_segments + color_overlay_pub_topic: line_detection/visualization + canny_debug_pub_topic: line_detection/canny_image + canny_overlay_pub_topic: line_detection/canny_overlay + + canny: + low_threshold: 100 + high_threshold: 200 + aperture_size: 3 + L2_gradient: false + + houghp: + rho: 1.0 + theta: 0.01745329251 # pi / 180 + threshold: 100 + min_line_length: 20.0 # pixels + max_line_gap: 10.0 # pixels + + mode: "debug" # {"standard", "visualize", "debug"} diff --git a/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp b/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp new file mode 100644 index 0000000..53c125b --- /dev/null +++ b/line_detection_houghp/include/line_detection_houghp/lib/line_detection_houghp.hpp @@ -0,0 +1,35 @@ +#ifndef LINE_DETECTION_HOUGHP__LIB__LINE_DETECTION_HOUGHP_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__LINE_DETECTION_HOUGHP_HPP_ + +#include +#include +#include +#include +#include "typedefs.hpp" + +namespace vortex::line_detection { + +class LineDetectorHoughP { + public: + explicit LineDetectorHoughP(const CannyConfig& edge_config, + const HoughPConfig& line_config); + + Result detect(const cv::Mat& input_image, + DetectorMode mode = DetectorMode::standard) const; + + private: + CannyConfig canny_config_; + HoughPConfig houghp_config_; + + void detect_edges(const cv::Mat& input_image, cv::Mat& edge_image) const; + + void detect_line_segments(const cv::Mat& edge_image, + std::vector& cv_lines) const; + + static std::vector to_line_segments( + const std::vector& cv_lines); +}; + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_HOUGHP__LIB__LINE_DETECTION_HOUGHP_HPP_ diff --git a/line_detection_houghp/include/line_detection_houghp/lib/mode_utils.hpp b/line_detection_houghp/include/line_detection_houghp/lib/mode_utils.hpp new file mode 100644 index 0000000..6473666 --- /dev/null +++ b/line_detection_houghp/include/line_detection_houghp/lib/mode_utils.hpp @@ -0,0 +1,38 @@ +#ifndef LINE_DETECTION_HOUGHP__LIB__MODE_UTILS_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__MODE_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include "line_detection_houghp/lib/typedefs.hpp" + +namespace vortex::line_detection { + +inline constexpr std::array, 3> + kDetectorModeMap{{ + {"standard", DetectorMode::standard}, + {"visualize", DetectorMode::visualize}, + {"debug", DetectorMode::debug}, + }}; + +inline DetectorMode parse_mode(const std::string& s) { + for (const auto& [name, mode] : kDetectorModeMap) { + if (s == name) + return mode; + } + throw std::runtime_error("Invalid DetectorMode: " + s); +} + +inline std::string_view to_string(DetectorMode mode) { + for (const auto& [name, m] : kDetectorModeMap) { + if (m == mode) + return name; + } + throw std::runtime_error("Unknown DetectorMode"); +} + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_HOUGHP__LIB__MODE_UTILS_HPP_ diff --git a/line_detection_houghp/include/line_detection_houghp/lib/typedefs.hpp b/line_detection_houghp/include/line_detection_houghp/lib/typedefs.hpp new file mode 100644 index 0000000..0af584a --- /dev/null +++ b/line_detection_houghp/include/line_detection_houghp/lib/typedefs.hpp @@ -0,0 +1,107 @@ +#ifndef LINE_DETECTION_HOUGHP__LIB__TYPEDEFS_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__TYPEDEFS_HPP_ + +#include +#include +#include +#include + +namespace vortex::line_detection { + +/** + * @brief Config for the OpenCV Canny edge algorithm + * used to detect edges in an 8-bit single channel image. + */ +struct CannyConfig { + int low_threshold{50}; // Low threshold for the hysteresis procedure. + int high_threshold{150}; // High threshold for the hysteresis procedure. + int aperture_size{3}; // Aperture size for the Sobel operator. + bool L2_gradient{false}; // L2_gradient flag, indicating whether to use a + // more accurate edge equation algorithm. +}; + +/** + * @brief Config for the OpenCV HoughLinesP algorithm + * used to detect lines segments in an 8-bit single channel image. + */ +struct HoughPConfig { + double rho{1.0}; // Distance resolution of the accumulator in pixels. + double theta{CV_PI / 180.0}; // Theta angle resolution + // of the accumulator in radians. + int threshold{50}; // Accumulator threshold used for accepting lines. + double min_line_length{0.0}; // Minimum allowed length for returned lines. + double max_line_gap{0.0}; // Allowed gap between points + // on the same line to link them. +}; + +/** + * @brief Operating mode of the detector. + * + * - standard: compute and return line segments only. + * - visualize: return line segments + + * an overlay image on the input color image. + * - debug: return line segments + edge map + + * overlay color and edge image for debugging. + */ +enum class DetectorMode { standard, visualize, debug }; + +/** + * @brief Empty output payload for Mode::standard. + * + * Used as the "no extra outputs" alternative in the Output variant. + */ +struct NoOutput {}; + +/** + * @brief Visualization payload for Mode::visualize. + * + * - overlay_color: a BGR visualization derived from the input color image with + * drawings applied. + */ +struct VisualizeOutput { + cv::Mat overlay_color; // Input image with line segments overlaid + // (typically CV_8UC3). +}; + +/** + * @brief Debug payload for Mode::debug. + * + * - canny: the edge map produced by Canny (8-bit, single-channel). + * - overlay_canny: a BGR visualization derived from the edge map with drawings + * applied. + * - overlay_color: a BGR visualization derived from the input color image with + * drawings applied. + */ +struct DebugOutput { + cv::Mat canny; // Canny edge map (CV_8UC1). + cv::Mat overlay_canny; // Edge map with line segments overlaid (typically + // CV_8UC3). + cv::Mat overlay_color; // Input image with line segments overlaid + // (typically CV_8UC3). +}; + +/** + * @brief Variant holding the mode-dependent output payload. + * + * Exactly one alternative is active: + * - NoOutput for Mode::standard + * - VisualizeOutput for Mode::visualize + * - DebugOutput for Mode::debug + */ +using Output = std::variant; + +/** + * @brief Result returned from a detection call. + * + * line_segments is always populated. output contains additional products + * depending on the requested Mode. + */ +struct Result { + std::vector + line_segments; // Detected line segments. + Output output; // Mode-dependent extra outputs. +}; + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_HOUGHP__LIB__TYPEDEFS_HPP_ diff --git a/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp b/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp new file mode 100644 index 0000000..76b3bd0 --- /dev/null +++ b/line_detection_houghp/include/line_detection_houghp/lib/utils.hpp @@ -0,0 +1,112 @@ +#ifndef LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ + +#include +#include + +namespace vortex::line_detection { + +/** + * @brief Convert an image to 8-bit, single-channel grayscale (CV_8UC1). + * + * This helper is intended to produce a suitable input for edge detection (e.g., + * Canny) from a variety of common OpenCV image formats. + * + * Conversion rules: + * - If @p input is already CV_8UC1, the function returns a **shallow copy** (no + * pixel data copied). + * - If @p input has 3 channels, it is converted using @c cv::COLOR_BGR2GRAY + * (assumes BGR ordering). + * - If @p input has 4 channels, it is converted using @c cv::COLOR_BGRA2GRAY + * (assumes BGRA ordering). + * + * - If the resulting grayscale image is 16-bit unsigned (CV_16U), it is scaled + * to 8-bit using + * @c convertTo with @c alpha = 1/256 (i.e., approximately a right shift by 8 + * bits). + * - For other depths (signed integers or floating point), the image is min-max + * normalized to [0,255] using @c cv::normalize(..., cv::NORM_MINMAX) and + * written as CV_8U. + * + * @param input Input image. Supported channel counts: 1, 3 (BGR), or 4 (BGRA). + * Supported depths: any OpenCV depth; non-8U depths are converted + * as described above. + * + * @return A grayscale image of type CV_8UC1. May share data with @p input if no + * conversion is needed. + * + * @throws std::runtime_error If @p input is empty or has an unsupported channel + * count. + */ +cv::Mat to_gray8(const cv::Mat& input); + +/** + * @brief Convert an image to 8-bit, 3-channel BGR (CV_8UC3). + * + * Conversion rules: + * - If @p input is already CV_8UC3, the function returns a **shallow copy** (no + * pixel data copied). + * - If @p input is single-channel, it is expanded to BGR using @c + * cv::COLOR_GRAY2BGR. In this case, the already-computed @p gray8 (CV_8UC1) + * must be provided to avoid recomputing grayscale conversion / scaling. + * - If @p input has 4 channels, it is converted using @c cv::COLOR_BGRA2BGR + * (assumes BGRA ordering). + * - If the resulting BGR image is 16-bit unsigned (CV_16U), it is scaled to + * 8-bit using + * @c convertTo with @c alpha = 1/256 (i.e., approximately a right shift by 8 + * bits). + * - For other depths (signed integers or floating point), the image is min-max + * normalized to [0,255] using @c cv::normalize(..., cv::NORM_MINMAX) and + * written as CV_8U. + * + * @note The normalization step computes min/max per call. This is usually fine + * for visualization, but it may produce different brightness/contrast per frame + * when viewing a stream. + * + * @param input Input image. Supported channel counts: 1, 3 (assumed BGR), or 4 + * (assumed BGRA). + * @param gray8 A grayscale image of type CV_8UC1 corresponding to @p input (or + * derived from it). Required when @p input is single-channel; ignored for 3- or + * 4-channel inputs. + * + * @return A BGR image of type CV_8UC3. May share data with @p input if no + * conversion is needed. + * + * @throws std::runtime_error If @p input is empty, has an unsupported channel + * count, or if @p input is single-channel and @p gray8 is empty or not CV_8UC1. + */ +cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/); + +/** + * @brief Create a BGR visualization by overlaying detected lines on the input + * color image. + * @param input_bgr Input color image (typically CV_8UC3). If not already in + * BGR format, it should be converted to BGR8 using @c to_bgr8 before + * calling this function. + * @param gray8 Grayscale version of the input image (CV_8UC1). This is used as + * an intermediate step to ensure proper handling of different input formats and + * depths. + * @param lines Detected line segments, where each line is represented as a + * cv::Vec4i (x1, y1, x2, y2). + * @return A BGR image with line segments overlaid on the input image. This is + * typically CV_8UC3. + */ +cv::Mat make_overlay_color(const cv::Mat& input_bgr, + const cv::Mat& gray8, + const std::vector& lines); + +/** + * @brief Create a BGR visualization by overlaying detected lines on the Canny + * edge map. + * @param edges Canny edge map (CV_8UC1). + * @param lines Detected line segments, where each line is represented as a + * cv::Vec4i (x1, y1, x2, y2). + * @return A BGR image with line segments overlaid on the edge map. This is + * typically CV_8UC3. + */ +cv::Mat make_overlay_canny(const cv::Mat& edges, + const std::vector& lines); + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ diff --git a/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp b/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp new file mode 100644 index 0000000..dd63986 --- /dev/null +++ b/line_detection_houghp/include/line_detection_houghp/ros/line_detection_houghp_ros.hpp @@ -0,0 +1,89 @@ +#ifndef LINE_DETECTION_HOUGHP__ROS__LINE_DETECTION_HOUGHP_ROS_HPP_ +#define LINE_DETECTION_HOUGHP__ROS__LINE_DETECTION_HOUGHP_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "line_detection_houghp/lib/line_detection_houghp.hpp" + +namespace vortex::line_detection { + +class LineDetectionHoughPNode : public rclcpp::Node { + public: + explicit LineDetectionHoughPNode(const rclcpp::NodeOptions& options); + + ~LineDetectionHoughPNode() override = default; + + private: + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Publisher::SharedPtr + line_segments_pub_; + rclcpp::Publisher::SharedPtr color_overlay_pub_; + rclcpp::Publisher::SharedPtr canny_debug_pub_; + rclcpp::Publisher::SharedPtr canny_overlay_pub_; + + void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg); + + void declare_parameters(); + + void setup_publishers_and_subscribers(); + + void set_detector(); + + void set_mode(); + + void mode_conditional_publishing(const Result& result, + const std_msgs::msg::Header& header); + + /** + * @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 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_; + + std::unique_ptr detector_{}; + + DetectorMode mode_{DetectorMode::standard}; +}; + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_HOUGHP__ROS__LINE_DETECTION_HOUGHP_ROS_HPP_ diff --git a/line_detection_houghp/launch/line_detection_houghp.launch.py b/line_detection_houghp/launch/line_detection_houghp.launch.py new file mode 100644 index 0000000..c003968 --- /dev/null +++ b/line_detection_houghp/launch/line_detection_houghp.launch.py @@ -0,0 +1,30 @@ +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('line_detection_houghp'), + 'config', + 'line_detection_houghp.yaml', + ) + + return LaunchDescription( + [ + Node( + package='line_detection_houghp', + executable='line_detection_houghp_node', + name='line_detection_houghp', + output='screen', + parameters=[ + config, + { + 'use_sim_time': False + }, # If testing with rosbags sim_time might be preferred if bag is looped + ], + ), + ] + ) diff --git a/line_detection_houghp/package.xml b/line_detection_houghp/package.xml new file mode 100644 index 0000000..4f00beb --- /dev/null +++ b/line_detection_houghp/package.xml @@ -0,0 +1,24 @@ + + + + line_detection_houghp + 0.0.0 + Package for detecting lines using the HoughP OpenCV function. + + jorgen + MIT + + ament_cmake + + rclcpp + rclcpp_components + vortex_utils + vortex_utils_ros + vortex_msgs + libopencv-dev + cv_bridge + + + ament_cmake + + diff --git a/line_detection_houghp/src/lib/houghp_line_detection.cpp b/line_detection_houghp/src/lib/houghp_line_detection.cpp new file mode 100644 index 0000000..7fd07cd --- /dev/null +++ b/line_detection_houghp/src/lib/houghp_line_detection.cpp @@ -0,0 +1,85 @@ +#include +#include +#include "line_detection_houghp/lib/line_detection_houghp.hpp" +#include "line_detection_houghp/lib/typedefs.hpp" +#include "line_detection_houghp/lib/utils.hpp" + +namespace vortex::line_detection { + +LineDetectorHoughP::LineDetectorHoughP(const CannyConfig& edge_config, + const HoughPConfig& line_config) + : canny_config_(edge_config), houghp_config_(line_config) {} + +Result LineDetectorHoughP::detect(const cv::Mat& input_image, + DetectorMode mode) const { + if (input_image.empty()) { + throw std::runtime_error( + "LineDetectorHoughP::detect: input_image is empty"); + } + cv::Mat gray8 = vortex::line_detection::to_gray8(input_image); + cv::Mat edge_image; + + detect_edges(gray8, edge_image); + + std::vector cv_lines; + detect_line_segments(edge_image, cv_lines); + + Result r; + r.line_segments = to_line_segments(cv_lines); + + switch (mode) { + case DetectorMode::standard: { + r.output = NoOutput{}; + break; + } + case DetectorMode::visualize: { + VisualizeOutput v; + v.overlay_color = vortex::line_detection::make_overlay_color( + input_image, gray8, cv_lines); + r.output = std::move(v); + break; + } + case DetectorMode::debug: { + DebugOutput d; + d.canny = edge_image; + d.overlay_canny = vortex::line_detection::make_overlay_canny( + edge_image, cv_lines); + d.overlay_color = vortex::line_detection::make_overlay_color( + input_image, gray8, cv_lines); + r.output = std::move(d); + break; + } + } + return r; +} + +void LineDetectorHoughP::detect_edges(const cv::Mat& input_image, + cv::Mat& edge_image) const { + cv::Canny(input_image, edge_image, canny_config_.low_threshold, + canny_config_.high_threshold, canny_config_.aperture_size, + canny_config_.L2_gradient); +} + +void LineDetectorHoughP::detect_line_segments( + const cv::Mat& edge_image, + std::vector& cv_lines) const { + cv::HoughLinesP(edge_image, cv_lines, houghp_config_.rho, + houghp_config_.theta, houghp_config_.threshold, + houghp_config_.min_line_length, + houghp_config_.max_line_gap); +} + +std::vector +LineDetectorHoughP::to_line_segments(const std::vector& cv_lines) { + std::vector line_segments; + line_segments.reserve(cv_lines.size()); + for (const auto& cv_line : cv_lines) { + line_segments.push_back(vortex::utils::types::LineSegment2D{ + {static_cast(cv_line[0]), static_cast(cv_line[1])}, + {static_cast(cv_line[2]), + static_cast(cv_line[3])}}); + } + return line_segments; +} + +} // namespace vortex::line_detection diff --git a/line_detection_houghp/src/lib/utils.cpp b/line_detection_houghp/src/lib/utils.cpp new file mode 100644 index 0000000..9be7e63 --- /dev/null +++ b/line_detection_houghp/src/lib/utils.cpp @@ -0,0 +1,146 @@ +#ifndef LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ +#define LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ + +#include "line_detection_houghp/lib/utils.hpp" +#include +#include + +namespace vortex::line_detection { + +cv::Mat to_gray8(const cv::Mat& input) { + if (input.empty()) { + throw std::runtime_error("to_gray8: input is empty"); + } + + if (input.type() == CV_8UC1) { + return input; + } + + cv::Mat gray; + const int ch = input.channels(); + + // Convert to grayscale + if (ch == 1) { + gray = input; + } else if (ch == 3) { + cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY); // assumes BGR + } else if (ch == 4) { + cv::cvtColor(input, gray, cv::COLOR_BGRA2GRAY); // assumes BGRA + } else { + throw std::runtime_error("to_gray8: unsupported channel count"); + } + + // Now ensure 8-bit depth. + if (gray.depth() == CV_8U) { + return gray; + } + + if (gray.depth() == CV_16U) { + cv::Mat out; + gray.convertTo(out, CV_8U, 1.0 / 256.0); // quick/common 16U->8U + return out; + } + + // Generic fallback for signed ints / floats etc. + cv::Mat out; + cv::normalize(gray, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; +} + +cv::Mat to_bgr8(const cv::Mat& input, const cv::Mat& gray8 /*CV_8UC1*/) { + if (input.empty()) { + throw std::runtime_error("to_bgr8_for_overlay: input is empty"); + } + + if (input.type() == CV_8UC3) { + return input; + } + + const int ch = input.channels(); + + if (ch == 1) { + if (gray8.empty() || gray8.type() != CV_8UC1) { + throw std::runtime_error( + "to_bgr8_for_overlay: gray8 must be CV_8UC1"); + } + cv::Mat bgr; + cv::cvtColor(gray8, bgr, cv::COLOR_GRAY2BGR); + return bgr; + } + + if (ch == 4) { + cv::Mat bgr; + cv::cvtColor(input, bgr, cv::COLOR_BGRA2BGR); // assumes BGRA + + if (bgr.depth() == CV_8U) { + return bgr; + } + if (bgr.depth() == CV_16U) { + cv::Mat out; + bgr.convertTo(out, CV_8U, 1.0 / 256.0); + return out; + } + cv::Mat out; + cv::normalize(bgr, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; + } + + if (ch == 3) { + if (input.depth() == CV_8U) { + return input; + } + if (input.depth() == CV_16U) { + cv::Mat out; + input.convertTo(out, CV_8U, 1.0 / 256.0); + return out; + } + cv::Mat out; + cv::normalize(input, out, 0, 255, cv::NORM_MINMAX, CV_8U); + return out; + } + + throw std::runtime_error("to_bgr8_for_overlay: unsupported channel count"); +} + +constexpr int k_line_thickness = 2; +constexpr int k_line_type = cv::LINE_AA; +const cv::Scalar k_line_color(0, 0, 255); // BGR red + +cv::Mat make_overlay_color(const cv::Mat& input_bgr, + const cv::Mat& gray8, + const std::vector& lines) { + if (input_bgr.empty()) { + throw std::runtime_error("make_overlay_color: input_bgr is empty"); + } + cv::Mat out_bgr = vortex::line_detection::to_bgr8(input_bgr, gray8); + for (const auto& l : lines) { + const cv::Point p0(l[0], l[1]); + const cv::Point p1(l[2], l[3]); + cv::line(out_bgr, p0, p1, k_line_color, k_line_thickness, k_line_type); + } + return out_bgr; +} + +cv::Mat make_overlay_canny(const cv::Mat& edges, + const std::vector& lines) { + if (edges.empty()) { + throw std::runtime_error("make_overlay_canny: edges is empty"); + } + if (edges.type() != CV_8UC1) { + throw std::runtime_error("make_overlay_canny: edges must be CV_8UC1"); + } + + cv::Mat out_bgr; + cv::cvtColor(edges, out_bgr, cv::COLOR_GRAY2BGR); + + for (const auto& l : lines) { + const cv::Point p0(l[0], l[1]); + const cv::Point p1(l[2], l[3]); + cv::line(out_bgr, p0, p1, k_line_color, k_line_thickness, k_line_type); + } + return out_bgr; +} + +} // namespace vortex::line_detection + +#endif // LINE_DETECTION_HOUGHP__LIB__UTILS_HPP_ diff --git a/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp b/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp new file mode 100644 index 0000000..936d734 --- /dev/null +++ b/line_detection_houghp/src/ros/line_detection_houghp_ros.cpp @@ -0,0 +1,138 @@ +#include "line_detection_houghp/ros/line_detection_houghp_ros.hpp" +#include +#include +#include +#include +#include "line_detection_houghp/lib/line_detection_houghp.hpp" + +namespace vortex::line_detection { + +LineDetectionHoughPNode::LineDetectionHoughPNode( + const rclcpp::NodeOptions& options) + : rclcpp::Node("line_detection_houghp_node", options) { + declare_parameters(); + set_detector(); + set_mode(); + setup_publishers_and_subscribers(); + initialize_parameter_handler(); +} + +void LineDetectionHoughPNode::declare_parameters() { + this->declare_parameter("topic.image_sub_topic"); + this->declare_parameter("topic.line_segments_pub_topic"); + this->declare_parameter("topic.color_overlay_pub_topic"); + this->declare_parameter("topic.canny_debug_pub_topic"); + this->declare_parameter("topic.canny_overlay_pub_topic"); + + this->declare_parameter("canny.low_threshold"); + this->declare_parameter("canny.high_threshold"); + this->declare_parameter("canny.aperture_size"); + this->declare_parameter("canny.L2_gradient"); + + this->declare_parameter("houghp.rho"); + this->declare_parameter("houghp.theta"); + this->declare_parameter("houghp.threshold"); + this->declare_parameter("houghp.min_line_length"); + this->declare_parameter("houghp.max_line_gap"); + + this->declare_parameter("mode"); +} + +void LineDetectionHoughPNode::setup_publishers_and_subscribers() { + const std::string image_sub_topic = + this->get_parameter("topic.image_sub_topic").as_string(); + const std::string line_segments_pub_topic = + this->get_parameter("topic.line_segments_pub_topic").as_string(); + const std::string color_overlay_pub_topic = + this->get_parameter("topic.color_overlay_pub_topic").as_string(); + const std::string canny_debug_pub_topic = + this->get_parameter("topic.canny_debug_pub_topic").as_string(); + const std::string canny_overlay_pub_topic = + this->get_parameter("topic.canny_overlay_pub_topic").as_string(); + + const auto qos_profile = + vortex::utils::qos_profiles::sensor_data_profile(1); + + image_sub_ = this->create_subscription( + image_sub_topic, qos_profile, + [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + this->image_callback(msg); + }); + + line_segments_pub_ = + this->create_publisher( + line_segments_pub_topic, qos_profile); + + color_overlay_pub_ = this->create_publisher( + color_overlay_pub_topic, qos_profile); + + canny_debug_pub_ = this->create_publisher( + canny_debug_pub_topic, qos_profile); + + canny_overlay_pub_ = this->create_publisher( + canny_overlay_pub_topic, qos_profile); +} + +void LineDetectionHoughPNode::set_detector() { + CannyConfig edge_config; + edge_config.low_threshold = + this->get_parameter("canny.low_threshold").as_int(); + edge_config.high_threshold = + this->get_parameter("canny.high_threshold").as_int(); + edge_config.aperture_size = + this->get_parameter("canny.aperture_size").as_int(); + edge_config.L2_gradient = + this->get_parameter("canny.L2_gradient").as_bool(); + + HoughPConfig line_config; + line_config.rho = this->get_parameter("houghp.rho").as_double(); + line_config.theta = this->get_parameter("houghp.theta").as_double(); + line_config.threshold = this->get_parameter("houghp.threshold").as_int(); + line_config.min_line_length = + this->get_parameter("houghp.min_line_length").as_double(); + line_config.max_line_gap = + this->get_parameter("houghp.max_line_gap").as_double(); + + detector_ = std::make_unique(edge_config, line_config); +} + +void LineDetectionHoughPNode::image_callback( + const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); + } catch (const cv_bridge::Exception& e) { + spdlog::error("cv_bridge exception: {}", e.what()); + return; + } + + const cv::Mat& input_image = cv_ptr->image; + + Result result; + try { + result = detector_->detect(input_image, mode_); + } catch (const std::exception& e) { + spdlog::error("Line detection failed: {}", e.what()); + return; + } + + vortex_msgs::msg::LineSegment2DArray line_segments_msg; + line_segments_msg.header = msg->header; + line_segments_msg.lines.reserve(result.line_segments.size()); + + for (const auto& segment : result.line_segments) { + vortex_msgs::msg::LineSegment2D seg_msg; + seg_msg.p0.x = segment.p0.x; + seg_msg.p0.y = segment.p0.y; + seg_msg.p1.x = segment.p1.x; + seg_msg.p1.y = segment.p1.y; + line_segments_msg.lines.push_back(seg_msg); + } + line_segments_pub_->publish(line_segments_msg); + + mode_conditional_publishing(result, msg->header); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(LineDetectionHoughPNode) + +} // namespace vortex::line_detection diff --git a/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp b/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp new file mode 100644 index 0000000..9a0d88c --- /dev/null +++ b/line_detection_houghp/src/ros/line_detection_houghp_ros_utils.cpp @@ -0,0 +1,137 @@ +#include +#include +#include "line_detection_houghp/lib/mode_utils.hpp" +#include "line_detection_houghp/ros/line_detection_houghp_ros.hpp" + +namespace vortex::line_detection { + +static bool starts_with(const std::string& s, const std::string& prefix) { + return s.size() >= prefix.size() && + s.compare(0, prefix.size(), prefix) == 0; +} + +void LineDetectionHoughPNode::initialize_parameter_handler() { + param_handler_ = std::make_shared(this); + + auto parameter_event_callback = + [this](const rcl_interfaces::msg::ParameterEvent& event) -> void { + this->on_parameter_event(event); + }; + + param_cb_handle_ = + param_handler_->add_parameter_event_callback(parameter_event_callback); +} + +void LineDetectionHoughPNode::on_parameter_event( + const rcl_interfaces::msg::ParameterEvent& event) { + const auto node_name = this->get_fully_qualified_name(); + if (event.node != node_name) + return; + + bool detector_params_changed = false; + bool mode_changed = false; + + for (const auto& p : event.changed_parameters) { + const auto& name = p.name; + + if (starts_with(name, "canny.") || starts_with(name, "houghp.")) { + detector_params_changed = true; + } else if (name == "mode") { + mode_changed = true; + } else { + continue; + } + + spdlog::info("Parameter changed: {}", name); + } + + if (mode_changed) { + set_mode(); + } + + if (detector_params_changed) { + set_detector(); + spdlog::info("Recreated HoughPLineDetector with updated params"); + } +} + +void LineDetectionHoughPNode::mode_conditional_publishing( + const Result& result, + const std_msgs::msg::Header& header) { + switch (mode_) { + case DetectorMode::standard: + return; + + case DetectorMode::visualize: { + const auto* vis = std::get_if(&result.output); + if (!vis) { + spdlog::warn( + "Mode is visualize but Result::output is not " + "VisualizeOutput"); + return; + } + + cv_bridge::CvImage overlay_msg; + overlay_msg.header = header; + overlay_msg.encoding = sensor_msgs::image_encodings::BGR8; + overlay_msg.image = vis->overlay_color; // expected CV_8UC3 + color_overlay_pub_->publish(*overlay_msg.toImageMsg()); + return; + } + + case DetectorMode::debug: { + const auto* dbg = std::get_if(&result.output); + if (!dbg) { + spdlog::warn( + "Mode is debug but Result::output is not DebugOutput"); + return; + } + + // overlay_canny: CV_8UC3 -> bgr8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::BGR8; + m.image = dbg->overlay_canny; + canny_overlay_pub_->publish(*m.toImageMsg()); + } + + // canny: CV_8UC1 -> mono8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::MONO8; + m.image = dbg->canny; + canny_debug_pub_->publish(*m.toImageMsg()); + } + + // overlay_color: CV_8UC3 -> bgr8 + { + cv_bridge::CvImage m; + m.header = header; + m.encoding = sensor_msgs::image_encodings::BGR8; + m.image = dbg->overlay_color; + color_overlay_pub_->publish(*m.toImageMsg()); + } + + return; + } + + default: + return; + } +} + +void LineDetectionHoughPNode::set_mode() { + const auto mode_str = this->get_parameter("mode").as_string(); + + try { + mode_ = parse_mode(mode_str); + spdlog::info("Set detector mode to '{}'", mode_str); + } catch (const std::exception& e) { + spdlog::warn("Invalid mode '{}': {}. Keeping previous mode.", mode_str, + e.what()); + } +} + +} // namespace vortex::line_detection diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh new file mode 100644 index 0000000..e37c97d --- /dev/null +++ b/scripts/ci_install_dependencies.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +# Installs dependencies not handled by rosdep. + +set -e # Exit on any error + +# Note: These are required by vortex-vkf +# Add Ubuntu Toolchain PPA to get gcc-13/g++-13 +sudo apt-get update -qq +sudo apt-get install -y --no-install-recommends software-properties-common +sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y +sudo apt-get update -qq + +# Install and switch to GCC 13 +sudo apt-get install -y --no-install-recommends gcc-13 g++-13 lcov +sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100 +sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 100 +sudo update-alternatives --install /usr/bin/gcov gcov /usr/bin/gcov-13 100 + +echo "Done installing additional dependencies."