From 6c19efb4c16aacd7a74284ed4c08165b27b2a45a Mon Sep 17 00:00:00 2001 From: Victor Hogeweij Date: Thu, 30 Jan 2025 22:43:00 +0100 Subject: [PATCH 1/4] add ros support --- CMakeLists.txt | 93 +++++++++++--- bindings/ros2/face_mesh_node.cpp | 202 +++++++++++++++++++++++++++++++ bindings/ros2/set_ld_path.sh | 20 +++ colcon.pkg | 8 ++ package.xml | 33 +++++ 5 files changed, 342 insertions(+), 14 deletions(-) create mode 100644 bindings/ros2/face_mesh_node.cpp create mode 100644 bindings/ros2/set_ld_path.sh create mode 100644 colcon.pkg create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index d866508..2da8369 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -48,24 +48,59 @@ else() SOURCE_SUBDIR tensorflow/lite ) FetchContent_MakeAvailable(tensorflow_rel_package) - else() + else() + # Check if we can use any of the precompiled tensorflow lite packages + # https://github.com/CLFML/TensorFlow_Lite_Compiled include(FetchContent) if(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "arm64") FetchContent_Declare( - tensorflow_compiled_rel_package - URL https://github.com/CLFML/TensorFlow_Lite_Compiled/releases/download/v2.16.1/tensorflow_linux_generic_aarch64.zip - SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/external/tensorflow -) + tensorflow_compiled_rel_package + URL https://github.com/CLFML/TensorFlow_Lite_Compiled/releases/download/v2.16.1/tensorflow_linux_generic_aarch64.zip + SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/external/tensorflow + ) else() - FetchContent_Declare( - tensorflow_compiled_rel_package - URL https://github.com/CLFML/TensorFlow_Lite_Compiled/releases/download/v2.16.1/tensorflow_linux_generic_x86_64.zip - SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/external/tensorflow - ) - endif() - FetchContent_MakeAvailable(tensorflow_compiled_rel_package) - if(CLFML_ROS2_PACKAGE_BUILD) - install(FILES external/tensorflow/lib/libtensorflowlite.so DESTINATION lib/${PROJECT_NAME}) + # Get the CPU compatible instructionset(s) + execute_process( + COMMAND lscpu + OUTPUT_VARIABLE CLFML_FACE_MESH_CPU_INFO + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + if(CLFML_FACE_MESH_CPU_INFO MATCHES "avx2") + message(STATUS "CLFML Face_Mesh.Cpp: CPU supports AVX2") + # Download the AVX2 compatible precompiled tensorflow lite package + FetchContent_Declare( + tensorflow_compiled_rel_package + URL https://github.com/CLFML/TensorFlow_Lite_Compiled/releases/download/v2.16.1/tensorflow_linux_avx2_x86_64.zip + SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/external/tensorflow + ) + FetchContent_MakeAvailable(tensorflow_compiled_rel_package) + if(CLFML_ROS2_PACKAGE_BUILD) + install(FILES external/tensorflow/lib/libtensorflowlite.so DESTINATION lib/${PROJECT_NAME}) + endif() + + elseif(CLFML_FACE_MESH_CPU_INFO MATCHES "avx") + message(STATUS "CLFML Face_Mesh.Cpp: CPU supports AVX") + # Download the AVX compatible precompiled tensorflow lite package + FetchContent_Declare( + tensorflow_compiled_rel_package + URL https://github.com/CLFML/TensorFlow_Lite_Compiled/releases/download/v2.16.1/tensorflow_linux_generic_x86_64.zip + SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/external/tensorflow + ) + FetchContent_MakeAvailable(tensorflow_compiled_rel_package) + if(CLFML_ROS2_PACKAGE_BUILD) + install(FILES external/tensorflow/lib/libtensorflowlite.so DESTINATION lib/${PROJECT_NAME}) + endif() + else() + message(STATUS "CLFML Face_Mesh.Cpp: CPU doesn't support AVX or AVX2; Compiling from source...") + # Download and build from source + FetchContent_Declare( + tensorflow_rel_package + URL https://github.com/tensorflow/tensorflow/archive/refs/tags/v2.16.1.zip + SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/external/tensorflow + SOURCE_SUBDIR tensorflow/lite + ) + FetchContent_MakeAvailable(tensorflow_rel_package) + endif() endif() endif() target_link_libraries(${PROJECT_NAME} tensorflow-lite) @@ -87,3 +122,33 @@ if(CLFML_FACE_MESH_BUILD_EXAMPLE_PROJECTS) 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) + find_package(sensor_msgs REQUIRED) + find_package(std_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(std_srvs REQUIRED) + find_package(cv_bridge REQUIRED) + target_link_libraries(${PROJECT_NAME} ${cv_bridge_LIBRARIES}) + add_executable(face_mesh_node ${CMAKE_CURRENT_LIST_DIR}/bindings/ros2/face_mesh_node.cpp) + ament_target_dependencies(face_mesh_node + rclcpp + sensor_msgs + cv_bridge + std_msgs + geometry_msgs + std_srvs + OpenCV + ) + target_link_libraries(face_mesh_node face_mesh) + install(TARGETS + face_mesh_node + DESTINATION lib/${PROJECT_NAME}) +endif() + +if (CLFML_ROS2_PACKAGE_BUILD) + install(FILES bindings/ros2/set_ld_path.sh DESTINATION lib/${PROJECT_NAME}) + ament_package() +endif() \ No newline at end of file diff --git a/bindings/ros2/face_mesh_node.cpp b/bindings/ros2/face_mesh_node.cpp new file mode 100644 index 0000000..867fcae --- /dev/null +++ b/bindings/ros2/face_mesh_node.cpp @@ -0,0 +1,202 @@ +/* + * Copyright 2024 (C) Jeroen Veen & Victor Hogeweij + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * This file is part of the Face_Mesh.Cpp library + * + * Author: Jeroen Veen + * Victor Hogeweij + * + */ +#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 "std_srvs/srv/set_bool.hpp" +#include "face_mesh.hpp" + +class FaceMeshNode : public rclcpp::Node +{ +public: + FaceMeshNode() : Node("face_mesh_node") + { + declare_and_get_parameters(); + det_.load_model(CLFML_FACE_MESH_CPU_MODEL_PATH); + setup_communication(); + detection_enabled_ = true; + } + +private: + void declare_and_get_parameters() + { + declare_parameter("run_on_demand", false); + declare_parameter("trigger_topic", "/face_detected"); + 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(); + } + + void setup_communication() + { + if (run_continuous_) + { + // Link process image straight to the incoming image callback + image_sub_ = create_subscription( + 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( + 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( + camera_topic_, rclcpp::SensorDataQoS(), + std::bind(&FaceMeshNode::store_latest_image, this, std::placeholders::_1)); + } + face_mesh_landmarks_pub_ = create_publisher(face_landmarks_topic_, 10); + + toggle_service_ = create_service( + "toggle_face_mesh", + std::bind(&FaceMeshNode::toggle_detection, this, std::placeholders::_1, std::placeholders::_2)); + } + + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + if (!detection_enabled_) + return; + + cv_bridge::CvImagePtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + } + catch (cv_bridge::Exception &e) + { + RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + det_.load_image(cv_ptr->image); + + publish_face_mesh_landmarks(); + } + void store_latest_image(const sensor_msgs::msg::Image::SharedPtr msg) + { + last_image_msg_ = msg; // Store the latest image in memory + } + + void on_demand_callback(const std_msgs::msg::Int32::SharedPtr msg) + { + if (!detection_enabled_ || msg->data == 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 + } + else + { + RCLCPP_WARN(get_logger(), "No image available when trigger received."); + } + } + + void publish_face_mesh_landmarks() + { + std::array face_landmarks = det_.get_face_mesh_points(); + if (face_landmarks.empty()) + { + RCLCPP_WARN(get_logger(), "No face landmarks detected."); + return; + } + + // Create PointCloud2 message + sensor_msgs::msg::PointCloud2 cloud_msg; + cloud_msg.header.stamp = now(); + cloud_msg.header.frame_id = "camera_frame"; // Set the reference frame + cloud_msg.height = 1; // Single row + cloud_msg.width = face_landmarks.size(); // Number of points + cloud_msg.is_dense = true; + cloud_msg.is_bigendian = false; + + // Define the fields for x, y, z + sensor_msgs::PointCloud2Modifier modifier(cloud_msg); + modifier.setPointCloud2Fields(3, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + + modifier.resize(face_landmarks.size()); // Resize for the number of points + + // Create iterators for point data + sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); + + // Fill PointCloud2 data + for (const auto &landmark : face_landmarks) + { + *iter_x = landmark.x; + *iter_y = landmark.y; + *iter_z = landmark.z; + ++iter_x; + ++iter_y; + ++iter_z; + } + + // Publish the PointCloud2 message + face_mesh_landmarks_pub_->publish(cloud_msg); + } + + void toggle_detection(const std::shared_ptr request, + std::shared_ptr response) + { + detection_enabled_ = request->data; + response->success = true; + response->message = detection_enabled_ ? "Face mesh enabled" : "Face mesh disabled"; + } + + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Subscription::SharedPtr on_demand_sub_; + rclcpp::Publisher::SharedPtr face_mesh_landmarks_pub_; + rclcpp::Service::SharedPtr toggle_service_; + + sensor_msgs::msg::Image::SharedPtr last_image_msg_; + + CLFML::FaceMesh::FaceMesh det_; + std::string camera_topic_; + std::string face_landmarks_topic_; + std::string on_demand_topic_; + bool detection_enabled_; + bool run_continuous_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/bindings/ros2/set_ld_path.sh b/bindings/ros2/set_ld_path.sh new file mode 100644 index 0000000..4bb4bdb --- /dev/null +++ b/bindings/ros2/set_ld_path.sh @@ -0,0 +1,20 @@ +#!/bin/sh + +# Check if the script is run from the root of the project by checking if the "install" directory exists +if [ ! -d "install" ]; then + echo "Error: This script must be run from the root of the project. Unable to set the library path for tensorflow!" + exit 1 +fi + +# Path to the dynamic library +LIB_PATH="install/face_mesh/lib/face_mesh/libtensorflowlite.so" + +# Check if the dynamic library exists +if [ -f "$LIB_PATH" ]; then + echo "Library $LIB_PATH found. Added to LD_LIBRARY_PATH!" + + # Get the directory path of the library + LIB_DIR=$(dirname "$LIB_PATH") + + export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$LIB_DIR +fi \ No newline at end of file diff --git a/colcon.pkg b/colcon.pkg new file mode 100644 index 0000000..16d5731 --- /dev/null +++ b/colcon.pkg @@ -0,0 +1,8 @@ +{ + "name": "face_mesh", + "cmake-args":[ + "-DCLFML_ROS2_PACKAGE_BUILD=ON", + "-G Ninja" + ], + "hooks": ["lib/face_mesh/set_ld_path.sh"] +} \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..1775f2e --- /dev/null +++ b/package.xml @@ -0,0 +1,33 @@ + + + + face_mesh + 3.11.5 + + Face mesh generator using the Face Mesh Mediapipe model with a CPU delegate written in C++ + - Plain C/C++ implementation with minimal dependencies (Tensorflow Lite + OpenCV) + - Google MediaPipe models without the MediaPipe framework + - Runs on ARM + + jeroen + Apache-2.0 + + ament_cmake + rclcpp + sensor_msgs + cv_bridge + std_msgs + geometry_msgs + std_srvs + v4l2_camera + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + + From 0a99fe0d7b1e03c19e8aab3d2e528b29aa3566da Mon Sep 17 00:00:00 2001 From: Victor Hogeweij Date: Fri, 31 Jan 2025 23:44:21 +0100 Subject: [PATCH 2/4] -add ros2 support to face mesh --- .github/workflows/code_compiles.yml | 31 +++++++ .github/workflows/ros2_code_compiles.yml | 38 ++++++++ CMakeLists.txt | 43 ++++++--- README.md | 53 ++++++++++- bindings/ros2/face_mesh_node.cpp | 63 ++++++------- example/ros2/face_mesh_viewer.cpp | 109 +++++++++++++++++++++++ example/ros2/launch.py | 12 +++ 7 files changed, 301 insertions(+), 48 deletions(-) create mode 100644 .github/workflows/code_compiles.yml create mode 100644 .github/workflows/ros2_code_compiles.yml create mode 100644 example/ros2/face_mesh_viewer.cpp create mode 100644 example/ros2/launch.py diff --git a/.github/workflows/code_compiles.yml b/.github/workflows/code_compiles.yml new file mode 100644 index 0000000..963c63b --- /dev/null +++ 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 new file mode 100644 index 0000000..b0ce685 --- /dev/null +++ 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 index 2da8369..41676dc 100644 --- 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 index 32e0719..e7c50a2 100644 --- a/README.md +++ b/README.md @@ -77,6 +77,58 @@ 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/)... @@ -84,7 +136,6 @@ See our [wiki](https://clfml.github.io/Face_Mesh.Cpp/)... - 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 index 867fcae..f28d514 100644 --- 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( - 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( - 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( - 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( + 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( + camera_topic_, rclcpp::SensorDataQoS(), + std::bind(&FaceMeshNode::store_latest_image, this, std::placeholders::_1)); + face_mesh_landmarks_pub_ = create_publisher(face_landmarks_topic_, 10); toggle_service_ = create_service( @@ -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::SharedPtr image_sub_; - rclcpp::Subscription::SharedPtr on_demand_sub_; + rclcpp::Subscription::SharedPtr on_demand_sub_; rclcpp::Publisher::SharedPtr face_mesh_landmarks_pub_; rclcpp::Service::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 new file mode 100644 index 0000000..d854359 --- /dev/null +++ b/example/ros2/face_mesh_viewer.cpp @@ -0,0 +1,109 @@ +#include "rclcpp/rclcpp.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 "std_srvs/srv/set_bool.hpp" +#include + +class FaceMeshViewer : public rclcpp::Node +{ +public: + FaceMeshViewer() : Node("face_mesh_viewer") + { + // Declare parameters for topic names + this->declare_parameter("camera_topic", "/image_raw"); + this->declare_parameter("face_landmarks_topic", "/face_mesh_landmarks"); + + // Get parameter values + std::string camera_topic = this->get_parameter("camera_topic").as_string(); + std::string face_landmarks_topic = this->get_parameter("face_landmarks_topic").as_string(); + + // Create subscriptions + image_sub_ = this->create_subscription( + camera_topic, 10, + std::bind(&FaceMeshViewer::image_callback, this, std::placeholders::_1)); + + face_landmarks_sub_ = this->create_subscription( + face_landmarks_topic, 10, + std::bind(&FaceMeshViewer::face_landmarks_callback, this, std::placeholders::_1)); + + // Create OpenCV window + cv::namedWindow("Face Mesh Viewer", cv::WINDOW_NORMAL); + cv::resizeWindow("Face Mesh Viewer", 800, 600); + } + + ~FaceMeshViewer() + { + cv::destroyAllWindows(); + } + +private: + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + try + { + current_frame_ = cv_bridge::toCvShare(msg, "bgr8")->image.clone(); + update_display(); + } + catch (cv_bridge::Exception &e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + } + } + + void face_landmarks_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + if (msg->width * msg->height != face_landmarks_.size()) + { + RCLCPP_ERROR(this->get_logger(), "Unexpected face landmarks size: %d", msg->width * msg->height); + return; + } + + // Create iterators for PointCloud2 + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); + + // Extract landmark points + for (size_t i = 0; i < face_landmarks_.size(); ++i, ++iter_x, ++iter_y, ++iter_z) + { + face_landmarks_[i] = cv::Point3f(*iter_x, *iter_y, *iter_z); + } + + update_display(); + } + + void update_display() + { + if (current_frame_.empty()) + return; + + cv::Mat display_frame = current_frame_.clone(); + + // Draw face landmarks + for (cv::Point3f keypoint : face_landmarks_) + { + cv::circle(display_frame, cv::Point(keypoint.x, keypoint.y), 2, cv::Scalar(0, 255, 0), -1); + } + + // Display the frame + cv::imshow("Face Mesh Viewer", display_frame); + cv::waitKey(1); + } + + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Subscription::SharedPtr face_landmarks_sub_; + + cv::Mat current_frame_; + std::array face_landmarks_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/example/ros2/launch.py b/example/ros2/launch.py new file mode 100644 index 0000000..5704207 --- /dev/null +++ b/example/ros2/launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +import launch_ros.actions + +def generate_launch_description(): + return LaunchDescription([ + launch_ros.actions.Node( + namespace= "face_detector", package='face_detector', executable='face_detector_node', output='screen'), + launch_ros.actions.Node( + namespace= "face_mesh", package='face_mesh', executable='face_mesh_node', output='screen'), + launch_ros.actions.Node( + namespace= "face_mesh", package='face_mesh', executable='face_mesh_viewer', output='screen'), + ]) \ No newline at end of file From b89885883bbc108b43be43a3b067f93d31325e36 Mon Sep 17 00:00:00 2001 From: Victor Hogeweij Date: Fri, 31 Jan 2025 23:48:19 +0100 Subject: [PATCH 3/4] ROS should be by default off! --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 41676dc..08c2b16 100644 --- 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" ON) +option(CLFML_ROS2_PACKAGE_BUILD "Build a ROS2 package" OFF) if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) set(CMAKE_CXX_STANDARD 17) From 6c5432f54d2e522c1cf28622d6811a95052c709e Mon Sep 17 00:00:00 2001 From: Victor Hogeweij Date: Fri, 31 Jan 2025 23:50:08 +0100 Subject: [PATCH 4/4] small change to the set_ld_path script to use current dir wd --- bindings/ros2/set_ld_path.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/ros2/set_ld_path.sh b/bindings/ros2/set_ld_path.sh index 4bb4bdb..a6067ce 100644 --- a/bindings/ros2/set_ld_path.sh +++ b/bindings/ros2/set_ld_path.sh @@ -7,7 +7,7 @@ if [ ! -d "install" ]; then fi # Path to the dynamic library -LIB_PATH="install/face_mesh/lib/face_mesh/libtensorflowlite.so" +LIB_PATH="$(pwd)/install/face_mesh/lib/face_mesh/libtensorflowlite.so" # Check if the dynamic library exists if [ -f "$LIB_PATH" ]; then