-add ros2 support to face mesh

Hoog-V · Hoog-V · commit 0a99fe0d7b1e · 2025-01-31T23:44:21.000+01:00
diff --git a/.github/workflows/code_compiles.yml b/.github/workflows/code_compiles.yml
@@ -0,0 +1,31 @@
+name: code_compiles
+
+on:
+  push:
+    branches: ["main"]
+  pull_request:
+    branches: ["main"]
+  workflow_dispatch: {}
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac.
+    # You can convert this to a matrix build if you need cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    runs-on: ubuntu-24.04
+
+    steps:
+      - uses: actions/checkout@v3
+
+      - name: Install prerequired packages
+        # To compile and run the code we require cmake, ninja and opencv
+        run: sudo apt-get update && sudo apt-get install build-essential cmake ninja-build libopencv-dev
+
+      - name: Configure CMake
+        # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
+        # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
+        run: cmake -B ${{github.workspace}}/build -G Ninja
+
+      - name: Build
+        # Build your program with the given configuration
+        run: cmake --build ${{github.workspace}}/build
diff --git a/.github/workflows/ros2_code_compiles.yml b/.github/workflows/ros2_code_compiles.yml
@@ -0,0 +1,38 @@
+name: ros_code_compiles
+
+on:
+  push:
+    branches: ["main"]
+  pull_request:
+    branches: ["main"]
+  workflow_dispatch: {}
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac.
+    # You can convert this to a matrix build if you need cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    runs-on: ubuntu-24.04
+
+    steps:
+      - uses: actions/checkout@v3
+      - uses: ros-tooling/setup-ros@v0.7
+        with:
+          required-ros-distributions: jazzy
+
+      - name: Install prerequired packages
+        # To compile and run the code we require cmake, ninja and opencv
+        run: sudo apt-get update && sudo apt-get install build-essential cmake ninja-build libopencv-dev
+
+      - name: Install dependencies
+        working-directory: ${{ github.workspace }}
+        run: |
+          sudo rosdep init || true
+          rosdep update
+          rosdep install --from-paths src -y --ignore-src
+
+      - name: Colcon build
+        working-directory: ${{ github.workspace }}
+        run: |
+          source /opt/ros/jazzy/setup.bash
+          colcon build --packages-select face_detector
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -8,7 +8,7 @@ set(CLFML_FACE_MESH_MAIN_PROJECT OFF)
 option(CLFML_FACE_MESH_BUILD_EXAMPLE_PROJECTS "Build example projects" ON)
 
 # Optionally enable ROS2 package build (Requires ROS2 jazzy to be installed!)
-option(CLFML_ROS2_PACKAGE_BUILD "Build a ROS2 package" OFF)
+option(CLFML_ROS2_PACKAGE_BUILD "Build a ROS2 package" ON)
 
 if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR)
   set(CMAKE_CXX_STANDARD 17)
@@ -111,18 +111,6 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC -DCLFML_FACE_MESH_CPU_MODEL_PA
 
 set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17)
 
-if(CLFML_FACE_MESH_BUILD_EXAMPLE_PROJECTS)
-  FetchContent_Declare(
-        face_detector.cpp
-        GIT_REPOSITORY https://github.com/CLFML/Face_Detector.Cpp.git
-        GIT_TAG main
-    )
-  FetchContent_MakeAvailable(face_detector.cpp)
-  add_executable(face_mesh_demo ${CMAKE_CURRENT_LIST_DIR}/example/face_mesh_demo/demo.cpp)
-  target_link_libraries(face_mesh_demo PUBLIC CLFML::${PROJECT_NAME} CLFML::face_detector)
-  
-endif()
-
 if (CLFML_ROS2_PACKAGE_BUILD)
   find_package(ament_cmake REQUIRED)
   find_package(rclcpp REQUIRED)
@@ -148,6 +136,35 @@ if (CLFML_ROS2_PACKAGE_BUILD)
     DESTINATION lib/${PROJECT_NAME})
 endif()
 
+
+if(CLFML_FACE_MESH_BUILD_EXAMPLE_PROJECTS)
+FetchContent_Declare(
+  face_detector.cpp
+  GIT_REPOSITORY https://github.com/CLFML/Face_Detector.Cpp.git
+  GIT_TAG main
+)
+FetchContent_MakeAvailable(face_detector.cpp)
+if (CLFML_ROS2_PACKAGE_BUILD)
+    add_executable(face_mesh_viewer ${CMAKE_CURRENT_LIST_DIR}/example/ros2/face_mesh_viewer.cpp)
+    ament_target_dependencies(face_mesh_viewer
+        rclcpp
+        sensor_msgs
+        cv_bridge
+        std_msgs
+        geometry_msgs
+        std_srvs
+        OpenCV
+      )
+    install(TARGETS
+      face_mesh_viewer
+      DESTINATION lib/${PROJECT_NAME})
+  else ()
+  add_executable(face_mesh_demo ${CMAKE_CURRENT_LIST_DIR}/example/face_mesh_demo/demo.cpp)
+  target_link_libraries(face_mesh_demo PUBLIC CLFML::${PROJECT_NAME} CLFML::face_detector)
+  endif()
+endif()
+
+
 if (CLFML_ROS2_PACKAGE_BUILD)
   install(FILES bindings/ros2/set_ld_path.sh DESTINATION lib/${PROJECT_NAME})
   ament_package()
diff --git a/README.md b/README.md
@@ -77,14 +77,65 @@ add_subdirectory(Face_Mesh.Cpp)
 ...
 target_link_libraries(YOUR_EXECUTABLE CLFML::face_mesh)
 ```
+
+## Building a ROS2 package with Colcon
+Before using this library you will need the following packages installed:
+- OpenCV
+- ROS2
+- ROS CV bridge
+- Working C++ compiler (GCC or Clang)
+- CMake
+
+### Running the examples (Ubuntu, CPU)
+
+1. Clone this repo:
+```
+git clone https://github.com/CLFML/Face_Mesh.Cpp.git
+```
+
+2. Source your ROS2 installation:
+
+```bash
+source /opt/ros/jazzy/setup.bash
+```
+
+3. Install the dependencies:
+```bash
+rosdep install --from-paths src -y --ignore-src
+``` 
+
+4. Build the package:
+
+```bash
+colcon build --packages-select face_mesh
+```
+
+5. Set up the environment:
+
+```bash
+source install/setup.bash
+```
+
+6. Run the camera node:
+
+```bash
+ros2 run v4l2_camera v4l2_camera_node
+```
+
+7. In another terminal, run the nodes
+
+```bash
+ros2 launch example/ros2/launch.py
+```
+
+
 ## Aditional documentation
 See our [wiki](https://clfml.github.io/Face_Mesh.Cpp/)...
 
 ## Todo
 - Add language bindings for Python, C# and Java
 - Add support for MakeFiles and Bazel
 - Add Unit-tests 
-- Add ROS2 package support
 - Add support for the [Face Mesh V2 model](https://storage.googleapis.com/mediapipe-assets/Model%20Card%20MediaPipe%20Face%20Mesh%20V2.pdf)
 
 ## License
diff --git a/bindings/ros2/face_mesh_node.cpp b/bindings/ros2/face_mesh_node.cpp
@@ -20,12 +20,12 @@
  *
  */
 #include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/image.hpp"
-#include "sensor_msgs/msg/region_of_interest.hpp"
 #include "cv_bridge/cv_bridge.hpp"
 #include "std_msgs/msg/int32.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/region_of_interest.hpp"
 #include "std_srvs/srv/set_bool.hpp"
 #include "face_mesh.hpp"
 
@@ -43,38 +43,27 @@ class FaceMeshNode : public rclcpp::Node
 private:
     void declare_and_get_parameters()
     {
-        declare_parameter("run_on_demand", false);
-        declare_parameter("trigger_topic", "/face_detected");
+        declare_parameter("roi_topic", "/face_roi");
         declare_parameter("camera_topic", "/image_raw");
         declare_parameter("face_mesh_landmarks_topic", "/face_mesh_landmarks");
 
         camera_topic_ = get_parameter("camera_topic").as_string();
         face_landmarks_topic_ = get_parameter("face_mesh_landmarks_topic").as_string();
-        on_demand_topic_ = get_parameter("trigger_topic").as_string();
-        run_continuous_ = !get_parameter("run_on_demand").as_bool();
+        roi_topic_ = get_parameter("roi_topic").as_string();
     }
 
     void setup_communication()
     {
-        if (run_continuous_)
-        {
-            // Link process image straight to the incoming image callback
-            image_sub_ = create_subscription<sensor_msgs::msg::Image>(
-                camera_topic_, rclcpp::SensorDataQoS(),
-                std::bind(&FaceMeshNode::image_callback, this, std::placeholders::_1));
-        }
-        else
-        {
-            // Subscribe to the on-demand trigger topic
-            on_demand_sub_ = create_subscription<std_msgs::msg::Int32>(
-                on_demand_topic_, 10,
-                std::bind(&FaceMeshNode::on_demand_callback, this, std::placeholders::_1));
-
-            // Subscribe to the image topic but only store the latest image
-            image_sub_ = create_subscription<sensor_msgs::msg::Image>(
-                camera_topic_, rclcpp::SensorDataQoS(),
-                std::bind(&FaceMeshNode::store_latest_image, this, std::placeholders::_1));
-        }
+        // Subscribe to the on-demand trigger topic
+        on_demand_sub_ = create_subscription<sensor_msgs::msg::RegionOfInterest>(
+            roi_topic_, 10,
+            std::bind(&FaceMeshNode::on_demand_callback, this, std::placeholders::_1));
+
+        // Subscribe to the image topic but only store the latest image
+        image_sub_ = create_subscription<sensor_msgs::msg::Image>(
+            camera_topic_, rclcpp::SensorDataQoS(),
+            std::bind(&FaceMeshNode::store_latest_image, this, std::placeholders::_1));
+
         face_mesh_landmarks_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(face_landmarks_topic_, 10);
 
         toggle_service_ = create_service<std_srvs::srv::SetBool>(
@@ -97,7 +86,9 @@ class FaceMeshNode : public rclcpp::Node
             RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what());
             return;
         }
-        det_.load_image(cv_ptr->image);
+        
+        cv::Mat cropped_image_to_roi = cv_ptr->image(roi_);
+        det_.load_image(cropped_image_to_roi, roi_);
 
         publish_face_mesh_landmarks();
     }
@@ -106,16 +97,20 @@ class FaceMeshNode : public rclcpp::Node
         last_image_msg_ = msg; // Store the latest image in memory
     }
 
-    void on_demand_callback(const std_msgs::msg::Int32::SharedPtr msg)
+    void on_demand_callback(const sensor_msgs::msg::RegionOfInterest::SharedPtr msg)
     {
-        if (!detection_enabled_ || msg->data == 0)
+        if (!detection_enabled_ || msg->width == 0 || msg->height == 0)
             return;
 
-        RCLCPP_INFO(get_logger(), "Trigger received! Processing latest image...");
-
         if (last_image_msg_)
         {
-            image_callback(last_image_msg_); // Process the last stored image
+            roi_ = cv::Rect(msg->x_offset, msg->y_offset, msg->width, msg->height);
+            if (roi_.x >= 0 && roi_.y >= 0 &&
+                roi_.x + roi_.width <= last_image_msg_->width &&
+                roi_.y + roi_.height <= last_image_msg_->height)
+            {
+                image_callback(last_image_msg_); // Process the last stored image
+            }
         }
         else
         {
@@ -179,7 +174,7 @@ class FaceMeshNode : public rclcpp::Node
     }
 
     rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
-    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr on_demand_sub_;
+    rclcpp::Subscription<sensor_msgs::msg::RegionOfInterest>::SharedPtr on_demand_sub_;
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr face_mesh_landmarks_pub_;
     rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_service_;
 
@@ -188,9 +183,9 @@ class FaceMeshNode : public rclcpp::Node
     CLFML::FaceMesh::FaceMesh det_;
     std::string camera_topic_;
     std::string face_landmarks_topic_;
-    std::string on_demand_topic_;
+    std::string roi_topic_;
+    cv::Rect roi_ = cv::Rect(0, 0, 0, 0);
     bool detection_enabled_;
-    bool run_continuous_;
 };
 
 int main(int argc, char **argv)
diff --git a/example/ros2/face_mesh_viewer.cpp b/example/ros2/face_mesh_viewer.cpp
diff --git a/example/ros2/launch.py b/example/ros2/launch.py
