Ros2 bodypix

.gitignore

@@ -9,6 +9,8 @@ __pycache__/
 # Distribution / packaging
 .Python
 build/
+install/
+log/
 develop-eggs/
 dist/
 downloads/

common/helpers/cv2_img/CMakeLists.txt

@@ -0,0 +1,91 @@
+cmake_minimum_required(VERSION 3.5)
+project(cv2_img)
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)  # If you're using OpenCV
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See https://index.ros.org/doc/ros2/Tutorials/Creating-Your-First-ROS2-Package/#create-setup-py
+# ament_python_install_package(${PROJECT_NAME})
+ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME})
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## In ROS2, message generation is typically done in separate steps.
+## If you have custom messages or services, use the rosidl packages:
+## Uncomment below if needed.
+
+# find_package(rosidl_default_generators REQUIRED)
+# Uncomment if you have messages/services/actions to generate.
+# rosidl_generate_interfaces(${PROJECT_NAME}
+#   "msg/YourMessage.msg"
+#   DEPENDENCIES std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## In ROS2, dynamic reconfigure is replaced with parameters. You can
+## configure parameters via YAML or parameter server directly in your nodes.
+## No need for dynamic_reconfigure in CMakeLists.
+
+###################################
+## ament specific configuration  ##
+###################################
+
+## Generate configuration files for dependent packages
+ament_package()
+
+###########
+## Build ##
+###########
+
+## If you have custom C++ code, declare libraries or executables below:
+
+# include_directories(
+#   include
+#   ${rclpy_INCLUDE_DIRS}
+#   ${sensor_msgs_INCLUDE_DIRS}
+# )
+
+## If you have a C++ library:
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/cv2_img.cpp
+# )
+
+## If you have a C++ executable:
+# add_executable(${PROJECT_NAME}_node src/cv2_img_node.cpp)
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${ament_LIBRARIES}
+#   ${rclcpp_LIBRARIES}
+#   ${cv_bridge_LIBRARIES}  # If OpenCV is used
+# )
+
+#############
+## Install ##
+#############
+
+# Install Python scripts, if you have any
+# install(PROGRAMS
+#   scripts/my_python_script.py
+#   DESTINATION lib/${PROJECT_NAME}
+# )
+
+# Install launch files, config, and other files if necessary
+# install(DIRECTORY
+#   launch
+#   DESTINATION share/${PROJECT_NAME}
+# )
+
+#############
+## Testing ##
+#############
+
+## If you have unit tests, add them here. Example for Python:
+# ament_add_pytest_test(test_cv2_img test/test_cv2_img.py)

common/helpers/cv2_img/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>cv2_img</name>
+  <version>0.0.0</version>
+  <description>Various Python utilities for working with OpenCV and ROS2.</description>
+
+  <!-- Maintainer information -->
+  <maintainer email="[email protected]">Paul Makles</maintainer>
+
+  <!-- License -->
+  <license>MIT</license>
+
+  <!-- Optional URL tags (uncomment and modify if needed) -->
+  <!-- <url type="website">http://wiki.ros.org/cv2_img</url> -->
+
+  <!-- Author information (optional) -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+  <!-- Build tool dependency: ament is used for ROS2 -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- Runtime and build dependencies -->
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend> <!-- If using OpenCV and ROS image conversions -->
+  <depend>python3-numpy</depend> <!-- Include system dependencies like numpy -->
+
+  <!-- Testing dependencies (optional) -->
+  <!-- <test_depend>ament_cmake_pytest</test_depend> -->
+
+  <!-- Export section for additional tools -->
+  <export>
+    <!-- No specific exports needed, but ROS2 tools may require some information here -->
+  </export>
+</package>

common/helpers/cv2_img/setup.py

@@ -0,0 +1,23 @@
+from setuptools import setup
+
+package_name = "cv2_img"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=[package_name],
+    package_dir={"": "src"},
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Paul Makles",
+    maintainer_email="[email protected]",
+    description="Various Python utilities for working with cv2 and ROS2.",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            # Add any scripts you want to be able to run from the command line
+            # Example: 'script_name = cv2_img.script_module:main'
+        ],
+    },
+)

common/helpers/cv2_img/src/cv2_img/__init__.py

@@ -0,0 +1,107 @@
+import rclpy
+from rclpy.node import Node
+import numpy as np
+
+from PIL import Image
+from sensor_msgs.msg import Image as SensorImage
+
+from rclpy.clock import Clock
+
+
+def cv2_img_to_msg(img, stamp=None):
+    """
+    Convert a given cv2 image to sensor image
+
+    :param img: cv2 image (as numpy array)
+    :param stamp: rospy Time
+    :return: Sensor Image
+    """
+    height, width, _ = img.shape
+
+    msg = SensorImage()
+    # Instantiate the clock
+    clock = Clock()
+
+    # Set the header stamp to the current time if stamp is None, otherwise use the provided stamp
+    msg.header.stamp = clock.now().to_msg() if stamp is None else stamp
+    msg.width = width
+    msg.height = height
+    msg.encoding = "bgr8"
+    msg.is_bigendian = 1
+    msg.step = 3 * width
+    msg.data = img.tobytes()
+
+    return msg
+
+
+def msg_to_pillow_img(msg: SensorImage):
+    """
+    Convert a given sensor image to a pillow image
+
+    :param msg: Sensor Image
+    :return: Pillow Image
+    """
+    size = (msg.width, msg.height)
+    if msg.encoding in ["bgr8", "8UC3"]:
+        img = Image.frombytes("RGB", size, msg.data, "raw")
+
+        # BGR => RGB
+        img = Image.fromarray(np.array(img)[:, :, ::-1])
+    elif msg.encoding == "rgb8":
+        img = Image.frombytes("RGB", size, msg.data, "raw")
+    else:
+        raise Exception("Unsupported format.")
+
+    return img
+
+
+def msg_to_cv2_img(msg: SensorImage):
+    """
+    Convert a given sensor image to a cv2 image
+
+    :param msg: Sensor Image
+    :return: numpy array
+    """
+    img = msg_to_pillow_img(msg)
+
+    # now bring it back into OpenCV format
+    img = np.array(img)
+    img = img[:, :, ::-1].copy()
+
+    return img
+
+
+def extract_mask_region(frame, mask, expand_x=0.5, expand_y=0.5):
+    """
+    Extracts the face region from the image and expands the region by the specified amount.
+
+    :param frame: The source image.
+    :param mask: The mask with the face part.
+    :param expand_x: The percentage to expand the width of the bounding box.
+    :param expand_y: The percentage to expand the height of the bounding box.
+    :return: The extracted face region as a numpy array, or None if not found.
+    """
+    # only requiring cv2 if we need it
+    import cv2
+
+    contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+
+    if contours:
+        largest_contour = max(contours, key=cv2.contourArea)
+        x, y, w, h = cv2.boundingRect(largest_contour)
+
+        # Expand the bounding box
+        new_w = w * (1 + expand_x)
+        new_h = h * (1 + expand_y)
+        x -= (new_w - w) // 2
+        y -= (new_h - h) // 2
+
+        # Ensure the new bounding box is within the frame dimensions
+        x = int(max(0, x))
+        y = int(max(0, y))
+        new_w = min(frame.shape[1] - x, new_w)
+        new_h = min(frame.shape[0] - y, new_h)
+
+        face_region = frame[y : y + int(new_h), x : x + int(new_w)]
+        return face_region
+    return None

common/helpers/cv2_pcl/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.5)
+project(cv2_pcl)
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+# find_package(ros_numpy REQUIRED)  # If you are using ros_numpy for pointcloud conversions
+find_package(cv2_img REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed.
+## See https://index.ros.org/doc/ros2/Tutorials/Creating-Your-First-ROS2-Package/#create-setup-py
+ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME})
+
+###################################
+## ament specific configuration  ##
+###################################
+
+## Generate configuration files for dependent packages
+ament_package()
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+include_directories(
+  # include
+  ${rclpy_INCLUDE_DIRS}
+  ${sensor_msgs_INCLUDE_DIRS}
+  ${cv_bridge_INCLUDE_DIRS}
+)
+
+## If you have a C++ library (uncomment if needed)
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/cv2_pcl.cpp
+# )
+
+## If you have a C++ executable (uncomment if needed)
+# add_executable(${PROJECT_NAME}_node src/cv2_pcl_node.cpp)
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${ament_LIBRARIES}
+#   ${cv_bridge_LIBRARIES}  # Link OpenCV libraries if used
+# )
+
+#############
+## Install ##
+#############
+
+# Install Python scripts (for Python-based nodes)
+# install(PROGRAMS
+#   scripts/my_python_script.py  # Add your Python script
+#   DESTINATION lib/${PROJECT_NAME}
+# )
+
+# # Install other files (e.g., launch, config files)
+# install(DIRECTORY
+#   launch
+#   DESTINATION share/${PROJECT_NAME}
+# )
+
+#############
+## Testing ##
+#############
+
+## Example of adding tests (for Python-based tests)
+# ament_add_pytest_test(test_cv2_pcl test/test_cv2_pcl.py)

common/helpers/cv2_pcl/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>cv2_pcl</name>
+  <version>0.0.0</version>
+  <description>The cv2_pcl package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <maintainer email="[email protected]">Jared Swift</maintainer>
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <license>MIT</license>
+
+  <!-- Dependency for build tool in ROS2 -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- ROS2 package dependencies -->
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend>  <!-- if using OpenCV -->
+  <depend>cv2_img</depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/helpers/cv2_pcl/setup.py

@@ -0,0 +1,23 @@
+from setuptools import setup
+
+package_name = "cv2_pcl"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=[package_name],
+    package_dir={"": "src"},
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Jared Swift",
+    maintainer_email="[email protected]",
+    description="The cv2_pcl package for ROS2, providing utilities for working with OpenCV and PointClouds.",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            # Add any scripts you want to expose as command-line executables here
+            # For example: 'pcl_processor = cv2_pcl.pcl_processor:main'
+        ],
+    },
+)