diff --git a/.gitignore b/.gitignore index f1bdb36be..ccb9b18b1 100644 --- a/.gitignore +++ b/.gitignore @@ -9,6 +9,8 @@ __pycache__/ # Distribution / packaging .Python build/ +install/ +log/ develop-eggs/ dist/ downloads/ diff --git a/common/helpers/cv2_img/CMakeLists.txt b/common/helpers/cv2_img/CMakeLists.txt new file mode 100644 index 000000000..c515db2db --- /dev/null +++ b/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) diff --git a/common/helpers/cv2_img/package.xml b/common/helpers/cv2_img/package.xml new file mode 100644 index 000000000..bf0e844be --- /dev/null +++ b/common/helpers/cv2_img/package.xml @@ -0,0 +1,35 @@ + + + cv2_img + 0.0.0 + Various Python utilities for working with OpenCV and ROS2. + + + Paul Makles + + + MIT + + + + + + + + + ament_cmake + + + rclpy + sensor_msgs + cv_bridge + python3-numpy + + + + + + + + + diff --git a/common/helpers/cv2_img/setup.py b/common/helpers/cv2_img/setup.py new file mode 100644 index 000000000..4327576d8 --- /dev/null +++ b/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="me@insrt.uk", + 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' + ], + }, +) diff --git a/common/helpers/cv2_img/src/cv2_img/__init__.py b/common/helpers/cv2_img/src/cv2_img/__init__.py new file mode 100644 index 000000000..ec47676ed --- /dev/null +++ b/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 diff --git a/common/helpers/cv2_pcl/CMakeLists.txt b/common/helpers/cv2_pcl/CMakeLists.txt new file mode 100644 index 000000000..9a70d5938 --- /dev/null +++ b/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) diff --git a/common/helpers/cv2_pcl/package.xml b/common/helpers/cv2_pcl/package.xml new file mode 100644 index 000000000..e4b2d903a --- /dev/null +++ b/common/helpers/cv2_pcl/package.xml @@ -0,0 +1,26 @@ + + + cv2_pcl + 0.0.0 + The cv2_pcl package + + + Jared Swift + + + MIT + + + ament_cmake + + + rclpy + sensor_msgs + cv_bridge + cv2_img + + + + ament_cmake + + \ No newline at end of file diff --git a/common/helpers/cv2_pcl/setup.py b/common/helpers/cv2_pcl/setup.py new file mode 100644 index 000000000..08d389031 --- /dev/null +++ b/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="jared.swift@kcl.ac.uk", + 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' + ], + }, +) diff --git a/common/helpers/cv2_pcl/src/cv2_pcl/__init__.py b/common/helpers/cv2_pcl/src/cv2_pcl/__init__.py new file mode 100644 index 000000000..e473b43a2 --- /dev/null +++ b/common/helpers/cv2_pcl/src/cv2_pcl/__init__.py @@ -0,0 +1,156 @@ +import numpy as np +import struct +from sensor_msgs.msg import PointCloud2 +import cv2 +from cv2_img import cv2_img_to_msg +from typing import Tuple, Union + +# ROS2-specific imports +import rclpy +from rclpy.node import Node + +Mat = np.ndarray + + +def pointcloud2_to_xyz_array(pointcloud: PointCloud2, remove_nans=True): + """ + Convert a sensor_msgs/PointCloud2 message to an Nx3 NumPy array. + :param pointcloud: ROS2 PointCloud2 message. + :param remove_nans: If True, NaN values will be removed. + :return: Nx3 NumPy array of XYZ points. + """ + fmt = _get_struct_fmt(pointcloud) + width, height = pointcloud.width, pointcloud.height + unpacker = struct.Struct(fmt) + points = [] + + for i in range(height * width): + point_data = pointcloud.data[ + i * pointcloud.point_step : (i + 1) * pointcloud.point_step + ] + point = unpacker.unpack( + point_data[:12] + ) # Assuming XYZ are first 12 bytes (3 floats) + points.append(point) + + # Convert to a NumPy array + points = np.array(points) + + if remove_nans: + points = points[~np.isnan(points).any(axis=1)] + + return points + + +def _get_struct_fmt(cloud_msg: PointCloud2): + """ + Generate the struct format string from the PointCloud2 fields. + :param cloud_msg: ROS2 PointCloud2 message. + :return: Struct format string for unpacking the data. + """ + # Define the data structure format string (assuming XYZ are all float32) + fmt = "fff" # XYZ are three 32-bit floats (4 bytes each) + return fmt + + +def pcl_to_img_msg(pcl: PointCloud2) -> Mat: + """ + Convert a given PointCloud2 message to img_msg. + """ + # keep the same timestamp + cv2_img = pcl_to_cv2(pcl) + + return cv2_img_to_msg(cv2_img, pcl.header.stamp) + + +def pcl_to_cv2( + pcl: PointCloud2, height: Union[int, None] = None, width: Union[int, None] = None +) -> Mat: + """ + Convert a given PointCloud2 message to a cv2 image. + """ + height = height or pcl.height + width = width or pcl.width + + # Extract XYZ points and pack as an image (for example, RGB image representation) + xyz_array = pointcloud2_to_xyz_array(pcl) + + # Placeholder for converting XYZ to RGB or any other visualization. + # For example, scale and shift XYZ to [0, 255] for visualization as an image. + frame = (xyz_array[:, :3] - np.min(xyz_array[:, :3])) / ( + np.max(xyz_array[:, :3]) - np.min(xyz_array[:, :3]) + ) + frame = (frame * 255).astype(np.uint8) + + # Reshape into a height x width x 3 image. + frame = frame.reshape((height, width, 3)) + + return frame + + +def seg_to_centroid( + pcl: PointCloud2, + xyseg: np.ndarray, + height: Union[int, None] = None, + width: Union[int, None] = None, +) -> np.ndarray: + """ + Computes the centroid of a given segment in a pointcloud. + """ + height = height or pcl.height + width = width or pcl.width + + # Convert xyseg to contours + contours = xyseg.reshape(-1, 2) + + # Convert PointCloud2 to NumPy array + pcl_xyz = pointcloud2_to_xyz_array(pcl, remove_nans=False) + pcl_xyz = pcl_xyz.reshape(height, width, 3) + + # Compute mask from contours + mask = np.zeros((height, width), dtype=np.uint8) + cv2.fillPoly(mask, pts=[contours], color=(255)) + + # Extract mask indices + indices = np.argwhere(mask) + + if indices.shape[0] == 0: + return np.full(3, np.nan) + + # Extract points of interest based on mask + xyz_points = [pcl_xyz[x, y] for x, y in indices] + + # Compute the centroid of the points + return np.nanmean(xyz_points, axis=0) + + +def bb_to_centroid( + pcl: PointCloud2, + x: int, + y: int, + w: int, + h: int, + height: Union[int, None] = None, + width: Union[int, None] = None, +) -> np.ndarray: + """ + Computes the centroid of a given bounding box in a pointcloud. + """ + height = height or pcl.height + width = width or pcl.width + + # Convert PointCloud2 to NumPy array + pcl_xyz = pointcloud2_to_xyz_array(pcl, remove_nans=False) + pcl_xyz = pcl_xyz.reshape(height, width, 3) + + # Bounding box indices + x1, y1, x2, y2 = x, y, x + w, y + h + + # Extract points in the bounding box + xyz_points = pcl_xyz[y1:y2, x1:x2].reshape(-1, 3) + + if xyz_points.shape[0] == 0: + return np.full(3, np.nan) + + # Compute the centroid of the points + return np.nanmean(xyz_points, axis=0) diff --git a/common/vision/lasr_vision_bodypix/CMakeLists.txt b/common/vision/lasr_vision_bodypix/CMakeLists.txt new file mode 100644 index 000000000..cebc37059 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(lasr_vision_bodypix) + +# Find ament packages and libraries for building +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) # If you are using rclpy in your Python scripts +find_package(sensor_msgs REQUIRED) # Example ROS2 package dependency +find_package(std_msgs REQUIRED) +find_package(lasr_vision_interfaces REQUIRED) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME}) + +# Install Python scripts (these are the executables) +install(PROGRAMS + nodes/bodypix_services.py + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS + src/lasr_vision_bodypix/bodypix.py + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS + examples/keypoint_relay.py + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS + examples/mask_relay.py + DESTINATION lib/${PROJECT_NAME} +) + +# Install launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +# Macro for generating setup.py +ament_package() diff --git a/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py new file mode 100755 index 000000000..68478b13f --- /dev/null +++ b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 +import sys +import rclpy +from rclpy.node import Node +import threading + +from sensor_msgs.msg import Image +from lasr_vision_interfaces.srv import BodyPixKeypointDetection + + +class KeypointRelay(Node): + def __init__(self, listen_topic, model): + super().__init__("keypoint_relay") + self.listen_topic = listen_topic + self.model = model + self.processing = False + + # Set up the service client + self.detect_service_client = self.create_client( + BodyPixKeypointDetection, "/bodypix/keypoint_detection" + ) + while not self.detect_service_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("Service not available, waiting...") + + # Set up the subscriber + self.subscription = self.create_subscription( + Image, self.listen_topic, self.image_callback, 10 # QoS profile + ) + self.get_logger().info( + f"Started listening on topic: {self.listen_topic} with model: {self.model}" + ) + + def detect(self, image): + self.processing = True + self.get_logger().info("Received image message") + # Create a request for the service + req = BodyPixKeypointDetection.Request() + req.image_raw = image + req.dataset = self.model + req.confidence = 0.7 + + # Call the service asynchronously + future = self.detect_service_client.call_async(req) + future.add_done_callback(self.detect_callback) + + def detect_callback(self, future): + try: + response = future.result() + self.get_logger().info(f"Detection response: {response}") + except Exception as e: + self.get_logger().error(f"Service call failed: {e}") + finally: + self.processing = False + + def image_callback(self, image): + if self.processing: + return + + # Start a new thread for detection to avoid blocking + threading.Thread(target=self.detect, args=(image,)).start() + + +def main(args=None): + print("start keypoint_relay") + # Check if command-line arguments are sufficient + if len(sys.argv) < 2: + print( + "Usage: ros2 run lasr_vision_bodypix keypoint_relay.py [resnet50|mobilenet50|...]" + ) + sys.exit(1) + + # Parse the command-line arguments + + listen_topic = "/image_raw" + if isinstance(sys.argv[1], list): + listen_topic = sys.argv[1][0] + + model = sys.argv[2] if len(sys.argv) >= 3 else "resnet50" + + rclpy.init(args=args) + keypoint_relay_node = KeypointRelay(listen_topic, model) + keypoint_relay_node.get_logger().info("Keypoint relay node started") + + try: + rclpy.spin(keypoint_relay_node) + except KeyboardInterrupt: + pass + finally: + keypoint_relay_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/common/vision/lasr_vision_bodypix/examples/mask_relay.py b/common/vision/lasr_vision_bodypix/examples/mask_relay.py new file mode 100644 index 000000000..97e07a698 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/examples/mask_relay.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 + +import sys +import threading +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from lasr_vision_interfaces.srv import BodyPixMaskDetection + + +class ImageListener(Node): + def __init__(self, listen_topic, model): + super().__init__("image_listener") + self.listen_topic = listen_topic + self.model = model + self.processing = False + + # Set up the service client + self.detect_service_client = self.create_client( + BodyPixMaskDetection, "/bodypix/mask_detection" + ) + while not self.detect_service_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("Service not available, waiting...") + + # Set up the subscriber + self.subscription = self.create_subscription( + Image, self.listen_topic, self.image_callback, 10 # QoS profile + ) + self.get_logger().info( + f"Started listening on topic: {self.listen_topic} with model: {self.model}" + ) + + def detect(self, image): + self.processing = True + self.get_logger().info("Received image message") + # Create a request for the service + req = BodyPixMaskDetection.Request() + req.image_raw = image + req.dataset = self.model + req.confidence = 0.7 + req.parts = ["left_face", "right_face"] + + # Call the service asynchronously + future = self.detect_service_client.call_async(req) + future.add_done_callback(self.detect_callback) + + def detect_callback(self, future): + try: + response = future.result() + if response is not None: + # Modify masks for demonstration purposes + for mask in response.masks: + mask.mask = [True, False, True, False] + self.get_logger().info(f"Detection response received: {response}") + else: + self.get_logger().error("Service call returned no response") + except Exception as e: + self.get_logger().error(f"Service call failed: {e}") + finally: + self.processing = False + + def image_callback(self, image): + if self.processing: + return + + # Start a new thread for detection to avoid blocking + threading.Thread(target=self.detect, args=(image,)).start() + + +def main(args=None): + print("Starting mask_relay node") + # Check if command-line arguments are sufficient + if len(sys.argv) < 2: + print( + "Usage: ros2 run lasr_vision_bodypix mask_relay.py [resnet50|mobilenet50|...]" + ) + sys.exit(1) + + # Parse the command-line arguments + listen_topic = "/image_raw" + if isinstance(sys.argv[1], list): + listen_topic = sys.argv[1][0] + + model = sys.argv[2] if len(sys.argv) >= 3 else "resnet50" + + rclpy.init(args=args) + mask_relay_node = ImageListener(listen_topic, model) + mask_relay_node.get_logger().info("Mask relay node started") + + try: + rclpy.spin(mask_relay_node) + except KeyboardInterrupt: + pass + finally: + mask_relay_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py b/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py new file mode 100644 index 000000000..3c723a3d4 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py @@ -0,0 +1,31 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + # Declare preload argument with default value as a YAML list + preload_arg = DeclareLaunchArgument( + "preload", + default_value=["resnet50"], + # default_value="['resnet50', 'mobilenet50']", + description="Array of models to preload when starting the service", + ) + + # Create the BodyPix service node + bodypix_node = Node( + package="lasr_vision_bodypix", + executable="bodypix_services.py", + name="bodypix_services", + output="screen", + parameters=[{"preload": LaunchConfiguration("preload")}], + ) + + # Return the launch description + return LaunchDescription( + [ + preload_arg, # Argument declaration + bodypix_node, # Node for the BodyPix service + ] + ) diff --git a/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py b/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py new file mode 100644 index 000000000..b5d0f8631 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py @@ -0,0 +1,95 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, TextSubstitution, PythonExpression + +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # Declare launch arguments + model_arg = DeclareLaunchArgument( + "model", default_value="['resnet50']", description="Model to use for the demo" + ) + + image_topic_arg = DeclareLaunchArgument( + "image_topic", + default_value="/image_raw", # Default input image topic + description="Input image topic for keypoint relay", + ) + + # Path to the BodyPix launch file + bodypix_launch_file = os.path.join( + get_package_share_directory("lasr_vision_bodypix"), + "launch", + "bodypix_launch.py", + ) + + # # Path to the v4l2_camera launch file (replacement for video_stream_opencv) + # v4l2_camera_launch_file = os.path.join( + # get_package_share_directory('v4l2_camera'), 'launch', 'v4l2_camera_node.launch.py' + # ) + + v4l2_camera_launch_file = os.path.join( + get_package_share_directory("lasr_vision_bodypix"), + "launch", + "v4l2_camera_launch.py", + ) + + return LaunchDescription( + [ + # Declare the model argument + model_arg, + # Include BodyPix launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource(bodypix_launch_file), + launch_arguments={"preload": LaunchConfiguration("model")}.items(), + ), + # Show debug topic using rqt_image_view + Node( + package="rqt_image_view", + executable="rqt_image_view", + name="image_view", + output="screen", + arguments=[ + # [TextSubstitution(text='/bodypix/debug/'), LaunchConfiguration('model')]# Topic to visualize + PythonExpression( + [ + "'/bodypix/debug/' + ", # Static string + "''.join(", # Convert list to string + LaunchConfiguration("model"), + ")", + ] + ) + # '/bodypix/debug/{}'.format(LaunchConfiguration('resnet50')) + ], # Constructs the topic path dynamically + ), + # Start the keypoint relay service + Node( + package="lasr_vision_bodypix", + executable="keypoint_relay.py", # Specifying the subdirectory + name="keypoint_relay", + output="screen", + arguments=[ + # TextSubstitution(text='/bodypix/debug/'), + # LaunchConfiguration('model') + # '/camera/image_raw /{}'.format(LaunchConfiguration('model')) + "/image_raw ", + PythonExpression( + [ + "''.join(", # Convert model list to a single string + LaunchConfiguration("model"), + ")", + ] + ), + ], # Constructs the topic path dynamically], + ), + # Include the v4l2_camera launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource(v4l2_camera_launch_file), + launch_arguments={"image_size": "640x480"}.items(), + ), + ] + ) diff --git a/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py b/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py new file mode 100644 index 000000000..c033273f2 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py @@ -0,0 +1,87 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, TextSubstitution, PythonExpression +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # Declare launch arguments + model_arg = DeclareLaunchArgument( + "model", default_value="['resnet50']", description="Model to use for the demo" + ) + + image_topic_arg = DeclareLaunchArgument( + "image_topic", + default_value="/image_raw", # Default input image topic + description="Input image topic for keypoint relay", + ) + + # Path to the BodyPix launch file + bodypix_launch_file = os.path.join( + get_package_share_directory("lasr_vision_bodypix"), + "launch", + "bodypix_launch.py", + ) + + # Path to the v4l2_camera launch file (replacement for video_stream_opencv) + v4l2_camera_launch_file = os.path.join( + get_package_share_directory("lasr_vision_bodypix"), + "launch", + "v4l2_camera_launch.py", + ) + + return LaunchDescription( + [ + # Declare the model argument + model_arg, + # Include BodyPix launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource(bodypix_launch_file), + launch_arguments={"preload": LaunchConfiguration("model")}.items(), + ), + # Show debug topic using rqt_image_view + Node( + package="rqt_image_view", + executable="rqt_image_view", + name="image_view", + output="screen", + arguments=[ + # [TextSubstitution(text='/bodypix/debug/'), LaunchConfiguration('model')]# Topic to visualize + PythonExpression( + [ + "'/bodypix/debug/' + ", # Static string + "''.join(", # Convert list to string + LaunchConfiguration("model"), + ")", + ] + ) + # '/bodypix/debug/{}'.format(LaunchConfiguration('resnet50')) + ], # Constructs the topic path dynamically + ), + # Start the keypoint relay service + Node( + package="lasr_vision_bodypix", + executable="mask_relay.py", # Removed .py extension, assuming installed without it + name="mask_relay", + output="screen", + arguments=[ + "/image_raw ", + PythonExpression( + [ + "''.join(", # Convert model list to a single string + LaunchConfiguration("model"), + ")", + ] + ), + ], # Constructs the topic path dynamically],], + ), + # Include the v4l2_camera launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource(v4l2_camera_launch_file), + launch_arguments={"image_size": "640x480"}.items(), + ), + ] + ) diff --git a/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py b/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py new file mode 100644 index 000000000..91351e459 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + Node( + package="v4l2_camera", + executable="v4l2_camera_node", + name="v4l2_camera_node", + output="screen", + parameters=[ + { + "video_device": "/dev/video0", # the video device to open + "image_width": 640, + "image_height": 480, + "pixel_format": "YUYV", # the pixel format of the image + "frame_rate": 30, + } + ], + ), + ] + ) diff --git a/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py b/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py new file mode 100644 index 000000000..64f08769a --- /dev/null +++ b/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +import lasr_vision_bodypix as bodypix +from lasr_vision_interfaces.srv import ( + BodyPixMaskDetection, + BodyPixKeypointDetection, + DetectWave, +) +from sensor_msgs.msg import Image +import ros2_numpy as rnp +from geometry_msgs.msg import PointStamped, Point +from std_msgs.msg import Header +import numpy as np +from cv2_pcl import pcl_to_img_msg +from typing import Union +from rcl_interfaces.msg import ParameterDescriptor, ParameterType + + +class BodyPixServiceNode(Node): + def __init__(self): + super().__init__("bodypix_service_node") + + # Declare and load parameters + # self.declare_parameter('preload', []) + self.declare_parameter( + "preload", + [""], + ParameterDescriptor(type=ParameterType.PARAMETER_STRING_ARRAY), + ) + preload_models = ( + self.get_parameter("preload").get_parameter_value().string_array_value + ) + + # Preload models + for model in preload_models: + bodypix.load_model_cached(model) + + # Create service servers + self.mask_service = self.create_service( + BodyPixMaskDetection, "/bodypix/mask_detection", self.detect_masks + ) + self.keypoint_service = self.create_service( + BodyPixKeypointDetection, + "/bodypix/keypoint_detection", + self.detect_keypoints, + ) + self.get_logger().info("Keypoint detection service registered.") + self.detect_wave_service = self.create_service( + DetectWave, "/bodypix/detect_wave", self.detect_wave + ) + + # Debug publisher for detect_wave + self.debug_publisher = self.create_publisher(Image, "debug_waving", 1) + self.get_logger().info("BodyPix service node started") + + def detect_masks(self, request, response): + """Handles mask detection request.""" + response = bodypix.detect_masks(request) + return response + + def detect_keypoints(self, request, response): + """Handles keypoint detection request.""" + response = bodypix.detect_keypoints(request) + return response + + def detect_wave(self, request, response): + """Detects a waving gesture.""" + try: + # Prepare request for keypoint detection + bp_req = BodyPixKeypointDetection.Request() + bp_req.image_raw = pcl_to_img_msg(request.pcl_msg) + bp_req.dataset = request.dataset + bp_req.confidence = request.confidence + + # Call BodyPix keypoint detection + detected_keypoints = bodypix.detect_keypoints(bp_req).keypoints + except Exception as e: + self.get_logger().error(f"Error detecting keypoints: {e}") + return DetectWave.Response() + + gesture_to_detect = None + keypoint_info = { + keypoint.keypoint_name: {"x": keypoint.x, "y": keypoint.y} + for keypoint in detected_keypoints + } + + if "leftShoulder" in keypoint_info and "leftWrist" in keypoint_info: + if keypoint_info["leftWrist"]["y"] < keypoint_info["leftShoulder"]["y"]: + gesture_to_detect = "raising_left_arm" + if "rightShoulder" in keypoint_info and "rightWrist" in keypoint_info: + if keypoint_info["rightWrist"]["y"] < keypoint_info["rightShoulder"]["y"]: + gesture_to_detect = "raising_right_arm" + + if gesture_to_detect is not None: + self.get_logger().info(f"Detected gesture: {gesture_to_detect}") + + # Process wave point in point cloud + wave_point = keypoint_info.get( + "leftShoulder" + if gesture_to_detect == "raising_left_arm" + else "rightShoulder" + ) + pcl_xyz = rnp.point_cloud2.pointcloud2_to_xyz_array( + request.pcl_msg, remove_nans=False + ) + + try: + wave_position = np.zeros(3) + for i in range(-5, 5): + for j in range(-5, 5): + if np.any( + np.isnan( + pcl_xyz[int(wave_point["y"]) + i][int(wave_point["x"]) + j] + ) + ): + self.get_logger().warn("NaN point in PCL") + continue + wave_position += pcl_xyz[int(wave_point["y"]) + i][ + int(wave_point["x"]) + j + ] + wave_position /= 100 + wave_position_msg = PointStamped( + point=Point(*wave_position), + header=Header(frame_id=request.pcl_msg.header.frame_id), + ) + self.get_logger().info(f"Wave point: {wave_position_msg}") + except Exception as e: + self.get_logger().error(f"Error getting wave point: {e}") + wave_position_msg = PointStamped() + + is_waving = gesture_to_detect is not None + + # Build response + response.keypoints = detected_keypoints + response.wave_detected = is_waving + response.wave_position = wave_position_msg + return response + + +def main(args=None): + rclpy.init(args=args) + node = BodyPixServiceNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/common/vision/lasr_vision_bodypix/package.xml b/common/vision/lasr_vision_bodypix/package.xml new file mode 100644 index 000000000..4ba216815 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/package.xml @@ -0,0 +1,33 @@ + + + lasr_vision_bodypix + 0.0.0 + BodyPix 2.0 vision service + Paul Makles + + MIT + + + ament_cmake + ament_cmake_python + rclpy + + + rosidl_default_generators + rosidl_default_generators + rosidl_default_runtime + + ament_python + ament_python + + lasr_vision_interfaces + cv2_img + + + + requirements.txt + ament_cmake + + + + diff --git a/common/vision/lasr_vision_bodypix/requirements.in b/common/vision/lasr_vision_bodypix/requirements.in new file mode 100644 index 000000000..d6a6c1e80 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/requirements.in @@ -0,0 +1,7 @@ +tf-bodypix==0.4.2 +opencv-python==4.8.1.78 +Pillow==10.1.0 +matplotlib==3.8.1 + +# The following was manually added and freezed into requirements.txt: +tfjs-graph-converter==1.6.3 \ No newline at end of file diff --git a/common/vision/lasr_vision_bodypix/requirements.txt b/common/vision/lasr_vision_bodypix/requirements.txt new file mode 100644 index 000000000..af91959df --- /dev/null +++ b/common/vision/lasr_vision_bodypix/requirements.txt @@ -0,0 +1,77 @@ +absl-py==2.1.0 # via chex, keras, optax, orbax-checkpoint, tensorboard, tensorflow, tensorflow-decision-forests, ydf +astunparse==1.6.3 # via tensorflow +certifi==2024.7.4 # via requests +charset-normalizer==3.3.2 # via requests +chex==0.1.86 # via optax +contourpy==1.2.1 # via matplotlib +cycler==0.12.1 # via matplotlib +etils[epath,epy]==1.5.2 # via orbax-checkpoint +flatbuffers==24.3.25 # via tensorflow +flax==0.8.5 # via tensorflowjs +fonttools==4.53.1 # via matplotlib +fsspec==2024.6.1 # via etils +gast==0.6.0 # via tensorflow +google-pasta==0.2.0 # via tensorflow +grpcio==1.64.1 # via tensorboard, tensorflow +h5py==3.11.0 # via keras, tensorflow +idna==3.7 # via requests +importlib-metadata==8.0.0 # via jax, markdown +importlib-resources==6.4.0 # via etils, matplotlib, tensorflowjs +jax==0.4.30 # via chex, flax, optax, orbax-checkpoint, tensorflowjs +jaxlib==0.4.30 # via chex, jax, optax, orbax-checkpoint, tensorflowjs +keras==3.4.1 # via tensorflow +kiwisolver==1.4.5 # via matplotlib +libclang==18.1.1 # via tensorflow +markdown==3.6 # via tensorboard +markdown-it-py==3.0.0 # via rich +markupsafe==2.1.5 # via werkzeug +matplotlib==3.8.1 # via -r requirements.in +mdurl==0.1.2 # via markdown-it-py +ml-dtypes==0.3.2 # via jax, jaxlib, keras, tensorflow, tensorstore +msgpack==1.0.8 # via flax, orbax-checkpoint +namex==0.0.8 # via keras +nest-asyncio==1.6.0 # via orbax-checkpoint +numpy==1.26.4 # via chex, contourpy, flax, h5py, jax, jaxlib, keras, matplotlib, ml-dtypes, opencv-python, opt-einsum, optax, orbax-checkpoint, pandas, scipy, tensorboard, tensorflow, tensorflow-decision-forests, tensorflow-hub, tensorstore, ydf +opencv-python==4.8.1.78 # via -r requirements.in +opt-einsum==3.3.0 # via jax, tensorflow +optax==0.2.2 # via flax +optree==0.12.1 # via keras +orbax-checkpoint==0.5.20 # via flax +packaging==23.2 # via keras, matplotlib, tensorflow, tensorflowjs +pandas==2.2.2 # via tensorflow-decision-forests +pillow==10.1.0 # via -r requirements.in, matplotlib +protobuf==4.25.3 # via orbax-checkpoint, tensorboard, tensorflow, tensorflow-hub, ydf +pygments==2.18.0 # via rich +pyparsing==3.1.2 # via matplotlib +python-dateutil==2.9.0.post0 # via matplotlib, pandas +pytz==2024.1 # via pandas +pyyaml==6.0.1 # via flax, orbax-checkpoint +requests==2.32.3 # via tensorflow, tf-bodypix +rich==13.7.1 # via flax, keras +scipy==1.13.1 # via jax, jaxlib +six==1.16.0 # via astunparse, google-pasta, python-dateutil, tensorboard, tensorflow, tensorflow-decision-forests, tensorflowjs +tensorboard==2.16.2 # via tensorflow +tensorboard-data-server==0.7.2 # via tensorboard +tensorflow==2.16.2 # via tensorflow-decision-forests, tensorflowjs, tf-keras +tensorflow-decision-forests==1.9.1 # via tensorflowjs +tensorflow-hub==0.16.1 # via tensorflowjs +tensorflow-io-gcs-filesystem==0.37.1 # via tensorflow +tensorflowjs==4.20.0 # via tfjs-graph-converter +tensorstore==0.1.63 # via flax, orbax-checkpoint +termcolor==2.4.0 # via tensorflow +tf-bodypix==0.4.2 # via -r requirements.in +tf-keras==2.16.0 # via tensorflow-decision-forests, tensorflow-hub, tensorflowjs +tfjs-graph-converter==1.6.3 # via -r requirements.in +toolz==0.12.1 # via chex +typing-extensions==4.12.2 # via chex, etils, flax, optree, orbax-checkpoint, tensorflow +tzdata==2024.1 # via pandas +urllib3==2.2.2 # via requests +werkzeug==3.0.3 # via tensorboard +wheel==0.43.0 # via astunparse, tensorflow-decision-forests +wrapt==1.16.0 # via tensorflow +wurlitzer==3.1.1 # via tensorflow-decision-forests +ydf==0.5.0 # via tensorflow-decision-forests +zipp==3.19.2 # via etils, importlib-metadata, importlib-resources + +# The following packages are considered to be unsafe in a requirements file: +# setuptools \ No newline at end of file diff --git a/common/vision/lasr_vision_bodypix/setup.py b/common/vision/lasr_vision_bodypix/setup.py new file mode 100644 index 000000000..c58d27524 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/setup.py @@ -0,0 +1,23 @@ +from setuptools import setup, find_packages + +setup( + name="lasr_vision_bodypix", + version="0.0.0", + packages=find_packages( + "src" + ), # Specify 'src' to find packages in the src directory + package_dir={"": "src"}, # Maps the root package to the 'src' directory + install_requires=["setuptools"], + zip_safe=True, + maintainer="Siyao Li", + maintainer_email="sveali41@gmail.com", + description="Description of lasr_vision_bodypix package", + entry_points={ + "console_scripts": [ + "bodypix_service_node = lasr_vision_bodypix.nodes.bodypix_services:main", + "bodypix_node = lasr_vision_bodypix.bodypix:main", + "keypoint_relay = lasr_vision_bodypix.examples.keypoint_relay:main", + "mask_relay = lasr_vision_bodypix.examples.mask_relay:main", + ], + }, +) diff --git a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/__init__.py b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/__init__.py new file mode 100755 index 000000000..46ef209da --- /dev/null +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/__init__.py @@ -0,0 +1,8 @@ +from .bodypix import ( + snake_to_camel, + camel_to_snake, + load_model_cached, + run_inference, + detect_masks, + detect_keypoints, +) diff --git a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py new file mode 100755 index 000000000..f31339725 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -0,0 +1,206 @@ +from __future__ import annotations + +import re +from typing import List + +import cv2 +import cv2_img +import numpy as np +import tensorflow as tf +from lasr_vision_interfaces.msg import ( + BodyPixMask, + BodyPixKeypoint, + BodyPixKeypointNormalized, +) +from lasr_vision_interfaces.srv import ( + BodyPixMaskDetection, + BodyPixKeypointDetection, +) +from sensor_msgs.msg import Image as SensorImage +from tf_bodypix.api import download_model, load_model, BodyPixModelPaths + +BodyPixKeypointDetection_Request = BodyPixKeypointDetection.Request() +BodyPixKeypointDetection_Response = BodyPixKeypointDetection.Response() +BodyPixMaskDetection_Request = BodyPixMaskDetection.Request() +BodyPixMaskDetection_Response = BodyPixMaskDetection.Response() + +# model cache +loaded_models = {} + + +def snake_to_camel(snake_str): + components = snake_str.split("_") + return components[0] + "".join(x.title() for x in components[1:]) + + +def camel_to_snake(name): + return re.sub(r"(?= mask.shape[1] or y >= mask.shape[0]: + continue + except: + continue + + if logger: + logger.info(f"Keypoint {keypoint.part} at {x}, {y} is in mask") + + detected_keypoints.append( + BodyPixKeypoint(keypoint_name=keypoint.part, x=x, y=y) + ) + detected_keypoints_normalized.append( + BodyPixKeypointNormalized( + keypoint_name=keypoint.part, + x=float(x) / mask.shape[1], + y=float(y) / mask.shape[0], + ) + ) + + # Publish debug visualization if enabled + if debug_publisher: + from tf_bodypix.draw import draw_poses + + coloured_mask = result.get_colored_part_mask(mask).astype(np.uint8) + coloured_mask = draw_poses( + coloured_mask.copy(), + poses, + keypoints_color=(255, 100, 100), + skeleton_color=(100, 100, 255), + ) + + for keypoint in detected_keypoints: + cv2.putText( + coloured_mask, + f"{keypoint.keypoint_name}", + (int(keypoint.x), int(keypoint.y)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (255, 255, 255), + 2, + cv2.LINE_AA, + ) + debug_publisher.publish(cv2_img.cv2_img_to_msg(coloured_mask)) + + response = BodyPixKeypointDetection_Response + response.keypoints = detected_keypoints + response.normalized_keypoints = detected_keypoints_normalized + + return response diff --git a/common/vision/lasr_vision_interfaces/CMakeLists.txt b/common/vision/lasr_vision_interfaces/CMakeLists.txt index 89a53481d..e6dd3854d 100644 --- a/common/vision/lasr_vision_interfaces/CMakeLists.txt +++ b/common/vision/lasr_vision_interfaces/CMakeLists.txt @@ -5,11 +5,45 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +# Find dependencies find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) # Needed for built-in ROS types like Time -rosidl_generate_interfaces(${PROJECT_NAME}) +################################################ +## Declare ROS messages, services, and actions # +################################################ + +# Specify message files +set(msg_files + "msg/BodyPixKeypoint.msg" + "msg/BodyPixKeypointNormalized.msg" + "msg/BodyPixMask.msg" +) + +# Specify service files +set(srv_files + "srv/BodyPixMaskDetection.srv" + "srv/BodyPixKeypointDetection.srv" + "srv/DetectWave.srv" +) + + +# Generate messages and services +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES sensor_msgs builtin_interfaces # List all dependencies here +) + +################################### +## Export dependencies and package # +################################### + +# Export dependencies ament_export_dependencies(rosidl_default_runtime) +# This must be called after all targets and before any installation commands + ament_package() diff --git a/common/vision/lasr_vision_interfaces/msg/BodyPixKeypoint.msg b/common/vision/lasr_vision_interfaces/msg/BodyPixKeypoint.msg new file mode 100644 index 000000000..ed439bd9b --- /dev/null +++ b/common/vision/lasr_vision_interfaces/msg/BodyPixKeypoint.msg @@ -0,0 +1,8 @@ +# Keypoint.msg + +# name of the keypoint +string keypoint_name + +# the x and y coordinates of the body part +int32 x +int32 y \ No newline at end of file diff --git a/common/vision/lasr_vision_interfaces/msg/BodyPixKeypointNormalized.msg b/common/vision/lasr_vision_interfaces/msg/BodyPixKeypointNormalized.msg new file mode 100644 index 000000000..82e594446 --- /dev/null +++ b/common/vision/lasr_vision_interfaces/msg/BodyPixKeypointNormalized.msg @@ -0,0 +1,8 @@ +# Keypoint.msg + +# name of the keypoint +string keypoint_name + +# the x and y coordinates of the body part +float32 x +float32 y \ No newline at end of file diff --git a/common/vision/lasr_vision_interfaces/msg/BodyPixMask.msg b/common/vision/lasr_vision_interfaces/msg/BodyPixMask.msg new file mode 100644 index 000000000..5aeef47ad --- /dev/null +++ b/common/vision/lasr_vision_interfaces/msg/BodyPixMask.msg @@ -0,0 +1,9 @@ +# 1D array of mask +bool[] mask + +# Shape +# +# Use in mask.reshape(...shape) to get back 2D array of mask +uint32[] shape + +string part_name \ No newline at end of file diff --git a/common/vision/lasr_vision_interfaces/package.xml b/common/vision/lasr_vision_interfaces/package.xml index 339040076..ab49dfc15 100644 --- a/common/vision/lasr_vision_interfaces/package.xml +++ b/common/vision/lasr_vision_interfaces/package.xml @@ -1,4 +1,5 @@ + lasr_vision_interfaces @@ -7,9 +8,20 @@ Jared Swift GPL-3.0-only + ament_cmake - rosidl_default_generators + + + rosidl_default_generators rosidl_default_runtime + + + sensor_msgs + sensor_msgs + builtin_interfaces + builtin_interfaces + + rosidl_interface_packages ament_lint_auto diff --git a/common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv b/common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv new file mode 100644 index 000000000..3c491377c --- /dev/null +++ b/common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv @@ -0,0 +1,19 @@ +# Request +# Image to run inference on +sensor_msgs/Image image_raw + +# BodyPix model to use +string dataset + +# How certain the detection should be to include +float32 confidence + +# Whether to return keypoints that are out of bound +bool keep_out_of_bounds +--- +# Response +# keypoints +lasr_vision_interfaces/BodyPixKeypoint[] keypoints + +# keypoints +lasr_vision_interfaces/BodyPixKeypointNormalized[] normalized_keypoints \ No newline at end of file diff --git a/common/vision/lasr_vision_interfaces/srv/BodyPixMaskDetection.srv b/common/vision/lasr_vision_interfaces/srv/BodyPixMaskDetection.srv new file mode 100644 index 000000000..3c1a2fcfe --- /dev/null +++ b/common/vision/lasr_vision_interfaces/srv/BodyPixMaskDetection.srv @@ -0,0 +1,16 @@ +# Image to run inference on +sensor_msgs/Image image_raw + +# BodyPix model to use +string dataset + +# How certain the detection should be to include +float32 confidence + +# Name of parts to get the masks for +# A full list is available here: +# https://github.com/de-code/python-tf-bodypix/blob/develop/tf_bodypix/bodypix_js_utils/part_channels.py#L5 +string[] parts +--- +# Generated masks +lasr_vision_interfaces/BodyPixMask[] masks \ No newline at end of file diff --git a/common/vision/lasr_vision_interfaces/srv/DetectWave.srv b/common/vision/lasr_vision_interfaces/srv/DetectWave.srv new file mode 100644 index 000000000..817c903b9 --- /dev/null +++ b/common/vision/lasr_vision_interfaces/srv/DetectWave.srv @@ -0,0 +1,18 @@ +# Image to run inference on +sensor_msgs/PointCloud2 pcl_msg + +# BodyPix model to use +string dataset + +# How certain the detection should be to include +float32 confidence + +--- +# keypoints +lasr_vision_interfaces/BodyPixKeypoint[] keypoints + +# waving +bool wave_detected + +# waving position +geometry_msgs/PointStamped wave_position \ No newline at end of file