From f2df00a94805f4c844bce4af739c72d8ca020f78 Mon Sep 17 00:00:00 2001 From: Siyao Date: Sat, 12 Oct 2024 12:07:37 +0100 Subject: [PATCH 01/10] bodypix and cv2_img cv2_pcl --- common/helpers/cv2_img/CMakeLists.txt | 91 ++++ common/helpers/cv2_img/package.xml | 35 ++ common/helpers/cv2_img/setup.py | 23 + .../helpers/cv2_img/src/cv2_img/__init__.py | 104 +++++ common/helpers/cv2_pcl/CMakeLists.txt | 68 +++ common/helpers/cv2_pcl/package.xml | 27 ++ common/helpers/cv2_pcl/setup.py | 23 + .../helpers/cv2_pcl/src/cv2_pcl/__init__.py | 144 +++++++ common/helpers/video_stream_opencv | 1 + common/install/.colcon_install_layout | 1 + common/install/COLCON_IGNORE | 0 common/install/_local_setup_util_ps1.py | 407 ++++++++++++++++++ common/install/_local_setup_util_sh.py | 407 ++++++++++++++++++ common/install/lasr_vision_bodypix/.catkin | 0 .../colcon-core/packages/lasr_vision_bodypix | 1 + .../share/lasr_vision_bodypix/package.bash | 31 ++ .../share/lasr_vision_bodypix/package.dsv | 0 .../share/lasr_vision_bodypix/package.ps1 | 108 +++++ .../share/lasr_vision_bodypix/package.sh | 52 +++ .../share/lasr_vision_bodypix/package.zsh | 42 ++ common/install/local_setup.bash | 121 ++++++ common/install/local_setup.ps1 | 55 +++ common/install/local_setup.sh | 137 ++++++ common/install/local_setup.zsh | 134 ++++++ common/install/setup.bash | 40 ++ common/install/setup.ps1 | 32 ++ common/install/setup.sh | 57 +++ common/install/setup.zsh | 40 ++ common/log/COLCON_IGNORE | 0 common/log/latest | 1 + common/log/latest_build | 1 + common/third_party/leg_tracker | 1 + .../vision/lasr_vision_bodypix/CMakeLists.txt | 33 ++ .../launch/bodypix_launch.py | 25 ++ .../launch/camera_keypoint_launch.py | 58 +++ .../launch/camera_mask_launch.py | 60 +++ .../nodes/bodypix_services.py | 154 +++++++ common/vision/lasr_vision_bodypix/package.xml | 33 ++ .../lasr_vision_bodypix/requirements.in | 7 + .../lasr_vision_bodypix/requirements.txt | 77 ++++ common/vision/lasr_vision_bodypix/setup.py | 23 + .../src/lasr_vision_bodypix/__init__.py | 6 + .../src/lasr_vision_bodypix/bodypix.py | 190 ++++++++ common/vision/lasr_vision_msgs/CMakeLists.txt | 43 ++ .../lasr_vision_msgs/msg/BodyPixKeypoint.msg | 8 + .../msg/BodyPixKeypointNormalized.msg | 8 + .../lasr_vision_msgs/msg/BodyPixMask.msg | 9 + common/vision/lasr_vision_msgs/package.xml | 34 ++ .../srv/BodyPixKeypointDetection.srv | 17 + .../srv/BodyPixMaskDetection.srv | 16 + .../lasr_vision_msgs/srv/DetectWave.srv | 18 + 51 files changed, 3003 insertions(+) create mode 100644 common/helpers/cv2_img/CMakeLists.txt create mode 100644 common/helpers/cv2_img/package.xml create mode 100644 common/helpers/cv2_img/setup.py create mode 100644 common/helpers/cv2_img/src/cv2_img/__init__.py create mode 100644 common/helpers/cv2_pcl/CMakeLists.txt create mode 100644 common/helpers/cv2_pcl/package.xml create mode 100644 common/helpers/cv2_pcl/setup.py create mode 100644 common/helpers/cv2_pcl/src/cv2_pcl/__init__.py create mode 160000 common/helpers/video_stream_opencv create mode 100644 common/install/.colcon_install_layout create mode 100644 common/install/COLCON_IGNORE create mode 100644 common/install/_local_setup_util_ps1.py create mode 100644 common/install/_local_setup_util_sh.py create mode 100644 common/install/lasr_vision_bodypix/.catkin create mode 100644 common/install/lasr_vision_bodypix/share/colcon-core/packages/lasr_vision_bodypix create mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.bash create mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.dsv create mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.ps1 create mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.sh create mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.zsh create mode 100644 common/install/local_setup.bash create mode 100644 common/install/local_setup.ps1 create mode 100644 common/install/local_setup.sh create mode 100644 common/install/local_setup.zsh create mode 100644 common/install/setup.bash create mode 100644 common/install/setup.ps1 create mode 100644 common/install/setup.sh create mode 100644 common/install/setup.zsh create mode 100644 common/log/COLCON_IGNORE create mode 120000 common/log/latest create mode 120000 common/log/latest_build create mode 160000 common/third_party/leg_tracker create mode 100644 common/vision/lasr_vision_bodypix/CMakeLists.txt create mode 100644 common/vision/lasr_vision_bodypix/launch/bodypix_launch.py create mode 100644 common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py create mode 100644 common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py create mode 100644 common/vision/lasr_vision_bodypix/nodes/bodypix_services.py create mode 100644 common/vision/lasr_vision_bodypix/package.xml create mode 100644 common/vision/lasr_vision_bodypix/requirements.in create mode 100644 common/vision/lasr_vision_bodypix/requirements.txt create mode 100644 common/vision/lasr_vision_bodypix/setup.py create mode 100755 common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/__init__.py create mode 100755 common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py create mode 100644 common/vision/lasr_vision_msgs/CMakeLists.txt create mode 100644 common/vision/lasr_vision_msgs/msg/BodyPixKeypoint.msg create mode 100644 common/vision/lasr_vision_msgs/msg/BodyPixKeypointNormalized.msg create mode 100644 common/vision/lasr_vision_msgs/msg/BodyPixMask.msg create mode 100644 common/vision/lasr_vision_msgs/package.xml create mode 100644 common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv create mode 100644 common/vision/lasr_vision_msgs/srv/BodyPixMaskDetection.srv create mode 100644 common/vision/lasr_vision_msgs/srv/DetectWave.srv 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..8edff4000 --- /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..4f47537f9 --- /dev/null +++ b/common/helpers/cv2_img/src/cv2_img/__init__.py @@ -0,0 +1,104 @@ +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..400b5f3ba --- /dev/null +++ b/common/helpers/cv2_pcl/package.xml @@ -0,0 +1,27 @@ + + + cv2_pcl + 0.0.0 + The cv2_pcl package + + + Jared Swift + + + MIT + + + ament_cmake + + + rclpy + sensor_msgs + cv_bridge + cv2_img + + + + ament_cmake + + +s \ 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..8a5ed0286 --- /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..cc1d99f94 --- /dev/null +++ b/common/helpers/cv2_pcl/src/cv2_pcl/__init__.py @@ -0,0 +1,144 @@ +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/helpers/video_stream_opencv b/common/helpers/video_stream_opencv new file mode 160000 index 000000000..65949bdc5 --- /dev/null +++ b/common/helpers/video_stream_opencv @@ -0,0 +1 @@ +Subproject commit 65949bdc5c9468d18c51aed9073d020bec892532 diff --git a/common/install/.colcon_install_layout b/common/install/.colcon_install_layout new file mode 100644 index 000000000..3aad5336a --- /dev/null +++ b/common/install/.colcon_install_layout @@ -0,0 +1 @@ +isolated diff --git a/common/install/COLCON_IGNORE b/common/install/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/common/install/_local_setup_util_ps1.py b/common/install/_local_setup_util_ps1.py new file mode 100644 index 000000000..3c6d9e877 --- /dev/null +++ b/common/install/_local_setup_util_ps1.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/common/install/_local_setup_util_sh.py b/common/install/_local_setup_util_sh.py new file mode 100644 index 000000000..f67eaa989 --- /dev/null +++ b/common/install/_local_setup_util_sh.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' +FORMAT_STR_USE_ENV_VAR = '${name}' +FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/common/install/lasr_vision_bodypix/.catkin b/common/install/lasr_vision_bodypix/.catkin new file mode 100644 index 000000000..e69de29bb diff --git a/common/install/lasr_vision_bodypix/share/colcon-core/packages/lasr_vision_bodypix b/common/install/lasr_vision_bodypix/share/colcon-core/packages/lasr_vision_bodypix new file mode 100644 index 000000000..1529e8996 --- /dev/null +++ b/common/install/lasr_vision_bodypix/share/colcon-core/packages/lasr_vision_bodypix @@ -0,0 +1 @@ +ament_python:cv2_img:lasr_vision_msgs:rosidl_default_runtime \ No newline at end of file diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.bash b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.bash new file mode 100644 index 000000000..24600eb16 --- /dev/null +++ b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.bash @@ -0,0 +1,31 @@ +# generated from colcon_bash/shell/template/package.bash.em + +# This script extends the environment for this package. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + # the prefix is two levels up from the package specific share directory + _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)" +else + _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +# additional arguments: arguments to the script +_colcon_package_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$@" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source sh script of this package +_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/lasr_vision_bodypix/package.sh" + +unset _colcon_package_bash_source_script +unset _colcon_package_bash_COLCON_CURRENT_PREFIX diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.dsv b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.dsv new file mode 100644 index 000000000..e69de29bb diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.ps1 b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.ps1 new file mode 100644 index 000000000..4198e42ec --- /dev/null +++ b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.ps1 @@ -0,0 +1,108 @@ +# generated from colcon_powershell/shell/template/package.ps1.em + +# function to append a value to a variable +# which uses colons as separators +# duplicates as well as leading separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +function colcon_append_unique_value { + param ( + $_listname, + $_value + ) + + # get values from variable + if (Test-Path Env:$_listname) { + $_values=(Get-Item env:$_listname).Value + } else { + $_values="" + } + $_duplicate="" + # start with no values + $_all_values="" + # iterate over existing values in the variable + if ($_values) { + $_values.Split(";") | ForEach { + # not an empty string + if ($_) { + # not a duplicate of _value + if ($_ -eq $_value) { + $_duplicate="1" + } + if ($_all_values) { + $_all_values="${_all_values};$_" + } else { + $_all_values="$_" + } + } + } + } + # append only non-duplicates + if (!$_duplicate) { + # avoid leading separator + if ($_all_values) { + $_all_values="${_all_values};${_value}" + } else { + $_all_values="${_value}" + } + } + + # export the updated variable + Set-Item env:\$_listname -Value "$_all_values" +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +function colcon_prepend_unique_value { + param ( + $_listname, + $_value + ) + + # get values from variable + if (Test-Path Env:$_listname) { + $_values=(Get-Item env:$_listname).Value + } else { + $_values="" + } + # start with the new value + $_all_values="$_value" + # iterate over existing values in the variable + if ($_values) { + $_values.Split(";") | ForEach { + # not an empty string + if ($_) { + # not a duplicate of _value + if ($_ -ne $_value) { + # keep non-duplicate values + $_all_values="${_all_values};$_" + } + } + } + } + # export the updated variable + Set-Item env:\$_listname -Value "$_all_values" +} + +# function to source another script with conditional trace output +# first argument: the path of the script +# additional arguments: arguments to the script +function colcon_package_source_powershell_script { + param ( + $_colcon_package_source_powershell_script + ) + # source script with conditional trace output + if (Test-Path $_colcon_package_source_powershell_script) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_package_source_powershell_script'" + } + . "$_colcon_package_source_powershell_script" + } else { + Write-Error "not found: '$_colcon_package_source_powershell_script'" + } +} + + diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.sh b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.sh new file mode 100644 index 000000000..7d7278e5f --- /dev/null +++ b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.sh @@ -0,0 +1,52 @@ +# generated from colcon_core/shell/template/package.sh.em + +# This script extends the environment for this package. + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prepend_unique_value_IFS=$IFS + IFS=":" + # start with the new value + _all_values="$_value" + # workaround SH_WORD_SPLIT not being set in zsh + if [ "$(command -v colcon_zsh_convert_to_array)" ]; then + colcon_zsh_convert_to_array _values + fi + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + # restore the field separator + IFS=$_colcon_prepend_unique_value_IFS + unset _colcon_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.zsh b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.zsh new file mode 100644 index 000000000..59979cc29 --- /dev/null +++ b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.zsh @@ -0,0 +1,42 @@ +# generated from colcon_zsh/shell/template/package.zsh.em + +# This script extends the environment for this package. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + # the prefix is two levels up from the package specific share directory + _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)" +else + _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +# additional arguments: arguments to the script +_colcon_package_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$@" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +colcon_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# source sh script of this package +_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/lasr_vision_bodypix/package.sh" +unset convert_zsh_to_array + +unset _colcon_package_zsh_source_script +unset _colcon_package_zsh_COLCON_CURRENT_PREFIX diff --git a/common/install/local_setup.bash b/common/install/local_setup.bash new file mode 100644 index 000000000..03f00256c --- /dev/null +++ b/common/install/local_setup.bash @@ -0,0 +1,121 @@ +# generated from colcon_bash/shell/template/prefix.bash.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +else + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_bash_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" + unset _colcon_prefix_bash_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_bash_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/common/install/local_setup.ps1 b/common/install/local_setup.ps1 new file mode 100644 index 000000000..6f68c8ded --- /dev/null +++ b/common/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python3 executable" + exit 1 + } + $_colcon_python_executable="python3" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +if ($_colcon_ordered_commands) { + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression +} diff --git a/common/install/local_setup.sh b/common/install/local_setup.sh new file mode 100644 index 000000000..b384bf289 --- /dev/null +++ b/common/install/local_setup.sh @@ -0,0 +1,137 @@ +# generated from colcon_core/shell/template/prefix.sh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/siyao/project/robo_cup_mine/src/Base/common/install" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX + return 1 + fi +else + _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_sh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" + unset _colcon_prefix_sh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_sh_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "_colcon_prefix_sh_source_script() { + if [ -f \"\$1\" ]; then + if [ -n \"\$COLCON_TRACE\" ]; then + echo \"# . \\\"\$1\\\"\" + fi + . \"\$1\" + else + echo \"not found: \\\"\$1\\\"\" 1>&2 + fi + }" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/common/install/local_setup.zsh b/common/install/local_setup.zsh new file mode 100644 index 000000000..b6487102f --- /dev/null +++ b/common/install/local_setup.zsh @@ -0,0 +1,134 @@ +# generated from colcon_zsh/shell/template/prefix.zsh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +else + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +_colcon_prefix_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_zsh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # workaround SH_WORD_SPLIT not being set + _colcon_prefix_zsh_convert_to_array _values + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" + unset _colcon_prefix_zsh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_zsh_prepend_unique_value +unset _colcon_prefix_zsh_convert_to_array + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/common/install/setup.bash b/common/install/setup.bash new file mode 100644 index 000000000..66d590f66 --- /dev/null +++ b/common/install/setup.bash @@ -0,0 +1,40 @@ +# generated from colcon_bash/shell/template/prefix_chain.bash.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/humble" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/siyao/project/Learning_project/ros2_dev_ws/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/tiago_public_ws/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/siyao/project/robo_cup_mine/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_bash_source_script diff --git a/common/install/setup.ps1 b/common/install/setup.ps1 new file mode 100644 index 000000000..ff66f3750 --- /dev/null +++ b/common/install/setup.ps1 @@ -0,0 +1,32 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source chained prefixes +_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/siyao/project/Learning_project/ros2_dev_ws/install\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/opt/tiago_public_ws/install\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/siyao/project/robo_cup_mine/install\local_setup.ps1" + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/common/install/setup.sh b/common/install/setup.sh new file mode 100644 index 000000000..8823bfda6 --- /dev/null +++ b/common/install/setup.sh @@ -0,0 +1,57 @@ +# generated from colcon_core/shell/template/prefix_chain.sh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/siyao/project/robo_cup_mine/src/Base/common/install +if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX + return 1 +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/humble" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/siyao/project/Learning_project/ros2_dev_ws/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/opt/tiago_public_ws/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/siyao/project/robo_cup_mine/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_sh_source_script +unset COLCON_CURRENT_PREFIX diff --git a/common/install/setup.zsh b/common/install/setup.zsh new file mode 100644 index 000000000..e7ba993d8 --- /dev/null +++ b/common/install/setup.zsh @@ -0,0 +1,40 @@ +# generated from colcon_zsh/shell/template/prefix_chain.zsh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/humble" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/siyao/project/Learning_project/ros2_dev_ws/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/tiago_public_ws/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/siyao/project/robo_cup_mine/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_zsh_source_script diff --git a/common/log/COLCON_IGNORE b/common/log/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/common/log/latest b/common/log/latest new file mode 120000 index 000000000..b57d247c7 --- /dev/null +++ b/common/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/common/log/latest_build b/common/log/latest_build new file mode 120000 index 000000000..c785255f2 --- /dev/null +++ b/common/log/latest_build @@ -0,0 +1 @@ +build_2024-10-06_21-04-11 \ No newline at end of file diff --git a/common/third_party/leg_tracker b/common/third_party/leg_tracker new file mode 160000 index 000000000..e6cbb2bba --- /dev/null +++ b/common/third_party/leg_tracker @@ -0,0 +1 @@ +Subproject commit e6cbb2bba218e0684714a08972a7bdfa99118e3c diff --git a/common/vision/lasr_vision_bodypix/CMakeLists.txt b/common/vision/lasr_vision_bodypix/CMakeLists.txt new file mode 100644 index 000000000..b3afdb5e6 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/CMakeLists.txt @@ -0,0 +1,33 @@ +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_msgs 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 launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +# Macro for generating setup.py +ament_package() 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..a3593bdeb --- /dev/null +++ b/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py @@ -0,0 +1,25 @@ +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 + preload_arg = DeclareLaunchArgument( + 'preload', default_value="['resnet50']", 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..7036b9854 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py @@ -0,0 +1,58 @@ +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 +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' + ) + + # 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 video stream launch file + video_stream_launch_file = os.path.join( + get_package_share_directory('video_stream_opencv'), 'launch', '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(), # Corrected argument + ), + + # Show debug topic using rqt_image_view + Node( + package='rqt_image_view', + executable='rqt_image_view', + name='image_view', + output='screen', + arguments=[LaunchConfiguration('model')], # Corrected arguments + ), + + # Start the keypoint relay service (assuming the executable is installed without .py) + Node( + package='lasr_vision_bodypix', + executable='keypoint_relay', # Remove the .py extension + name='keypoint_relay', + output='screen', + arguments=[LaunchConfiguration('model')], # Corrected arguments + ), + + # Include the video stream launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource(video_stream_launch_file), + launch_arguments={'visualize': 'true'}.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..bca602f8c --- /dev/null +++ b/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py @@ -0,0 +1,60 @@ +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 +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Declare launch arguments + model_arg = DeclareLaunchArgument( + 'model', default_value='resnet50', description='Model to use for the demo' + ) + + # 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 video stream launch file + video_stream_launch_file = os.path.join( + get_package_share_directory('video_stream_opencv'), 'launch', 'camera.launch.py' + ) + + return LaunchDescription([ + # Declare model argument + model_arg, + + # Include BodyPix launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource(bodypix_launch_file), + launch_arguments={'preload': LaunchConfiguration('model')}.items(), # Corrected LaunchConfiguration usage + ), + + # Show debug topic using rqt_image_view + Node( + package='rqt_image_view', + executable='rqt_image_view', + name='image_view', + output='screen', + # Corrected: Directly passing LaunchConfiguration in arguments + arguments=[LaunchConfiguration('model')], + ), + + # Start the mask relay service + Node( + package='lasr_vision_bodypix', + executable='mask_relay', # Remove .py extension, assuming it's installed as executable + name='mask_relay', + output='screen', + # Corrected: Directly passing LaunchConfiguration in arguments + arguments=['/camera/image_raw', LaunchConfiguration('model')], + ), + + # Include video stream launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource(video_stream_launch_file), + launch_arguments={'visualize': 'true'}.items(), + ), + ]) 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..6470ad369 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +import lasr_vision_bodypix as bodypix +from lasr_vision_msgs.srv import ( + BodyPixMaskDetection, + BodyPixKeypointDetection, + DetectWave, +) +from sensor_msgs.msg import Image, PointCloud2 +from geometry_msgs.msg import PointStamped, Point +from std_msgs.msg import Header +from cv2_pcl import pcl_to_img_msg +import numpy as np +import struct +from typing import Union + +class BodyPixServiceNode(Node): + def __init__(self): + super().__init__("bodypix_service_node") + + # Initialize parameters + self.declare_parameter('preload', []) + preload_models = self.get_parameter('preload').get_parameter_value().string_array_value + + # Preload models if specified + 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.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): + """Detects body part masks.""" + response = bodypix.detect_masks(request) + return response + + def detect_keypoints(self, request, response): + """Detects keypoints.""" + response = bodypix.detect_keypoints(request) + return response + + def detect_wave(self, request, response): + """Detects waving gesture.""" + try: + # Convert PointCloud2 to image (if needed by your BodyPix model) + bp_req = BodyPixKeypointDetection.Request() + bp_req.image_raw = pcl_to_img_msg(request.pcl_msg) # Assuming you have a pcl_to_img_msg function. + bp_req.dataset = request.dataset + bp_req.confidence = request.confidence + + # Detect keypoints using the BodyPix model + detected_keypoints = bodypix.detect_keypoints(bp_req).keypoints + except Exception as e: + self.get_logger().error(f"Error detecting keypoints: {e}") + return DetectWave.Response() + + # Analyze keypoints + 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}") + + # Convert PointCloud2 to XYZ array + pcl_xyz = self.pointcloud2_to_xyz_array(request.pcl_msg) + + # Determine wave position + try: + wave_position = np.zeros(3) + wave_point = keypoint_info.get( + "leftShoulder" if gesture_to_detect == "raising_left_arm" else "rightShoulder" + ) + + # Take the average of points around the detected keypoint + 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 pointcloud2_to_xyz_array(self, cloud_msg): + """ + Converts a PointCloud2 message to a numpy array of shape (N, 3), with fields x, y, z. + """ + points = [] + for point in self.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=True): + points.append([point[0], point[1], point[2]]) + return np.array(points) + + def read_points(self, cloud, field_names=None, skip_nans=True): + """ + Generator for reading points from a PointCloud2 message. + Optionally filters by field_names and skips NaNs. + """ + fmt = "fff" # Format for x, y, z + point_step = cloud.point_step + for i in range(0, len(cloud.data), point_step): + x, y, z = struct.unpack_from(fmt, cloud.data, i) + if skip_nans and (x != x or y != y or z != z): # Check for NaNs + continue + yield (x, y, z) + + +def main(args=None): + rclpy.init(args=args) + node = BodyPixServiceNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/common/vision/lasr_vision_bodypix/package.xml b/common/vision/lasr_vision_bodypix/package.xml new file mode 100644 index 000000000..8b3104cb2 --- /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_msgs + 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..9042d8a33 --- /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=["lasr_vision_bodypix"], + package_dir={'': 'src'}, + install_requires=['setuptools'], + zip_safe=True, + + entry_points={ + 'console_scripts': [ + 'bodypix_service_node = lasr_vision_bodypix.nodes.bodypix_services:main', + 'bodypix_node = lasr_vision_bodypix.src.lasr_vision_bodypix.bodypix: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..fb8a7222c --- /dev/null +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/__init__.py @@ -0,0 +1,6 @@ +from .bodypix import ( + snake_to_camel, + camel_to_snake, + load_model_cached, + BodyPixNode, +) 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..653b1c253 --- /dev/null +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -0,0 +1,190 @@ +from __future__ import annotations +from typing import List +import rclpy +from rclpy.node import Node +import cv2 +import numpy as np +import re +import tensorflow as tf +import cv2_img +from sensor_msgs.msg import Image as SensorImage +from tf_bodypix.api import download_model, load_model, BodyPixModelPaths +from lasr_vision_msgs.msg import BodyPixMask, BodyPixKeypoint, BodyPixKeypointNormalized +from lasr_vision_msgs.srv import ( + BodyPixMaskDetection, + BodyPixMaskDetection_Request, + BodyPixMaskDetection_Response, + BodyPixKeypointDetection, + BodyPixKeypointDetection_Request, + BodyPixKeypointDetection_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"(? BodyPixMaskDetection_Response: + result, mask = self.run_inference(request.dataset, request.confidence, request.image_raw) + + masks = [] + for part_name in request.parts: + part_mask = result.get_part_mask(mask=tf.identity(mask), part_names=[part_name]).squeeze() + + if np.max(part_mask) == 0: + self.get_logger().warn(f"No masks found for part {part_name}") + continue + + bodypix_mask = BodyPixMask() + bodypix_mask.mask = part_mask.flatten().astype(bool).tolist() + bodypix_mask.shape = list(part_mask.shape) + bodypix_mask.part_name = part_name + masks.append(bodypix_mask) + + # Publish to debug topic + if self.debug_publisher_mask is not None: + from tf_bodypix.draw import draw_poses + coloured_mask = result.get_colored_part_mask(mask).astype(np.uint8) + poses = result.get_poses() + coloured_mask = draw_poses( + coloured_mask.copy(), + poses, + keypoints_color=(255, 100, 100), + skeleton_color=(100, 100, 255), + ) + self.debug_publisher_mask.publish(cv2_img.cv2_img_to_msg(coloured_mask)) + + response.masks = masks + return response + + def detect_keypoints( + self, request: BodyPixKeypointDetection_Request, response: BodyPixKeypointDetection_Response + ) -> BodyPixKeypointDetection_Response: + result, mask = self.run_inference(request.dataset, request.confidence, request.image_raw) + + poses = result.get_poses() + detected_keypoints: List[BodyPixKeypoint] = [] + detected_keypoints_normalized: List[BodyPixKeypointNormalized] = [] + + for pose in poses: + for keypoint in pose.keypoints.values(): + x = int(keypoint.position.x) + y = int(keypoint.position.y) + try: + if not request.keep_out_of_bounds: + if x < 0.0 or y < 0.0: + continue + if x >= mask.shape[1] or y >= mask.shape[0]: + continue + except: + continue + self.get_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], + ) + ) + + if self.debug_publisher_keypoint is not None: + 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, + ) + self.debug_publisher_keypoint.publish(cv2_img.cv2_img_to_msg(coloured_mask)) + + response.keypoints = detected_keypoints + response.normalized_keypoints = detected_keypoints_normalized + return response + + +def main(args=None): + rclpy.init(args=args) + node = BodyPixNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + diff --git a/common/vision/lasr_vision_msgs/CMakeLists.txt b/common/vision/lasr_vision_msgs/CMakeLists.txt new file mode 100644 index 000000000..0ad7dec3f --- /dev/null +++ b/common/vision/lasr_vision_msgs/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(lasr_vision_msgs) + +# 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 + +################################################ +## 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_msgs/msg/BodyPixKeypoint.msg b/common/vision/lasr_vision_msgs/msg/BodyPixKeypoint.msg new file mode 100644 index 000000000..ed439bd9b --- /dev/null +++ b/common/vision/lasr_vision_msgs/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_msgs/msg/BodyPixKeypointNormalized.msg b/common/vision/lasr_vision_msgs/msg/BodyPixKeypointNormalized.msg new file mode 100644 index 000000000..82e594446 --- /dev/null +++ b/common/vision/lasr_vision_msgs/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_msgs/msg/BodyPixMask.msg b/common/vision/lasr_vision_msgs/msg/BodyPixMask.msg new file mode 100644 index 000000000..5aeef47ad --- /dev/null +++ b/common/vision/lasr_vision_msgs/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_msgs/package.xml b/common/vision/lasr_vision_msgs/package.xml new file mode 100644 index 000000000..b47cee490 --- /dev/null +++ b/common/vision/lasr_vision_msgs/package.xml @@ -0,0 +1,34 @@ + + + lasr_vision_msgs + 0.0.0 + Messages and services for vision processing + Paul Makles + + MIT + + + ament_cmake + + + rosidl_default_generators + rosidl_default_runtime + + + + sensor_msgs + sensor_msgs + builtin_interfaces + builtin_interfaces + + + + rosidl_interface_packages + + + ament_cmake + + + + + diff --git a/common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv b/common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv new file mode 100644 index 000000000..36034b64f --- /dev/null +++ b/common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv @@ -0,0 +1,17 @@ +# 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 +--- +# keypoints +lasr_vision_msgs/BodyPixKeypoint[] keypoints + +# keypoints +lasr_vision_msgs/BodyPixKeypointNormalized[] normalized_keypoints \ No newline at end of file diff --git a/common/vision/lasr_vision_msgs/srv/BodyPixMaskDetection.srv b/common/vision/lasr_vision_msgs/srv/BodyPixMaskDetection.srv new file mode 100644 index 000000000..19accfaea --- /dev/null +++ b/common/vision/lasr_vision_msgs/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_msgs/BodyPixMask[] masks \ No newline at end of file diff --git a/common/vision/lasr_vision_msgs/srv/DetectWave.srv b/common/vision/lasr_vision_msgs/srv/DetectWave.srv new file mode 100644 index 000000000..b200ba7a6 --- /dev/null +++ b/common/vision/lasr_vision_msgs/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_msgs/BodyPixKeypoint[] keypoints + +# waving +bool wave_detected + +# waving position +geometry_msgs/PointStamped wave_position \ No newline at end of file From 24a5d505eb750c4b49fec50394d49b6a8027a8c3 Mon Sep 17 00:00:00 2001 From: Siyao Date: Fri, 8 Nov 2024 14:04:11 +0000 Subject: [PATCH 02/10] changes --- common/helpers/video_stream_opencv | 1 - 1 file changed, 1 deletion(-) delete mode 160000 common/helpers/video_stream_opencv diff --git a/common/helpers/video_stream_opencv b/common/helpers/video_stream_opencv deleted file mode 160000 index 65949bdc5..000000000 --- a/common/helpers/video_stream_opencv +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 65949bdc5c9468d18c51aed9073d020bec892532 From 04ce569627ff00836381dedf9509e00162f5a9ba Mon Sep 17 00:00:00 2001 From: Siyao Date: Fri, 8 Nov 2024 14:56:33 +0000 Subject: [PATCH 03/10] changes --- common/helpers/cv2_pcl/package.xml | 3 +- .../vision/lasr_vision_bodypix/CMakeLists.txt | 5 ++ .../examples/keypoint_relay.py | 85 ++++++++++++++++++ .../launch/camera_keypoint_launch.py | 37 +++++--- .../launch/camera_mask_launch.py | 40 +++++---- .../launch/v4l2_camera_launch.py | 19 ++++ .../nodes/bodypix_services.py | 89 +++++++------------ common/vision/lasr_vision_bodypix/setup.py | 13 +-- .../src/lasr_vision_bodypix/bodypix.py | 14 ++- common/vision/lasr_vision_msgs/CMakeLists.txt | 1 + 10 files changed, 206 insertions(+), 100 deletions(-) create mode 100755 common/vision/lasr_vision_bodypix/examples/keypoint_relay.py create mode 100644 common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py diff --git a/common/helpers/cv2_pcl/package.xml b/common/helpers/cv2_pcl/package.xml index 400b5f3ba..e4b2d903a 100644 --- a/common/helpers/cv2_pcl/package.xml +++ b/common/helpers/cv2_pcl/package.xml @@ -23,5 +23,4 @@ ament_cmake - -s \ No newline at end of file + \ No newline at end of file diff --git a/common/vision/lasr_vision_bodypix/CMakeLists.txt b/common/vision/lasr_vision_bodypix/CMakeLists.txt index b3afdb5e6..80ac5e1e4 100644 --- a/common/vision/lasr_vision_bodypix/CMakeLists.txt +++ b/common/vision/lasr_vision_bodypix/CMakeLists.txt @@ -23,6 +23,11 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) +install(PROGRAMS + examples/keypoint_relay.py + DESTINATION lib/${PROJECT_NAME} +) + # Install launch files install(DIRECTORY launch 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..7673b7efd --- /dev/null +++ b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 +import sys +import rclpy +from rclpy.node import Node +import threading + +from sensor_msgs.msg import Image +from lasr_vision_msgs.srv import BodyPixKeypointDetection, BodyPixKeypointDetectionRequest + +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 = BodyPixKeypointDetectionRequest() + 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): + # 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 = sys.argv[1] + model = sys.argv[2] if len(sys.argv) >= 3 else "resnet50" + + rclpy.init(args=args) + keypoint_relay_node = KeypointRelay(listen_topic, model) + + 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/launch/camera_keypoint_launch.py b/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py index 7036b9854..a8003d7c0 100644 --- a/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py @@ -2,7 +2,7 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch_ros.actions import Node from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, TextSubstitution import os from ament_index_python.packages import get_package_share_directory @@ -14,12 +14,16 @@ def generate_launch_description(): # Path to the BodyPix launch file bodypix_launch_file = os.path.join( - get_package_share_directory('lasr_vision_bodypix'), 'launch', 'bodypix.launch.py' + get_package_share_directory('lasr_vision_bodypix'), 'launch', 'bodypix_launch.py' ) - # Path to the video stream launch file - video_stream_launch_file = os.path.join( - get_package_share_directory('video_stream_opencv'), 'launch', 'camera.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([ @@ -29,7 +33,9 @@ def generate_launch_description(): # Include BodyPix launch file IncludeLaunchDescription( PythonLaunchDescriptionSource(bodypix_launch_file), - launch_arguments={'preload': LaunchConfiguration('model')}.items(), # Corrected argument + launch_arguments={ + 'preload': LaunchConfiguration('model') + }.items(), ), # Show debug topic using rqt_image_view @@ -38,21 +44,26 @@ def generate_launch_description(): executable='rqt_image_view', name='image_view', output='screen', - arguments=[LaunchConfiguration('model')], # Corrected arguments + arguments=[ + TextSubstitution(text='/bodypix/debug/'), + LaunchConfiguration('model') + ], # Constructs the topic path dynamically ), - # Start the keypoint relay service (assuming the executable is installed without .py) + # Start the keypoint relay service Node( package='lasr_vision_bodypix', - executable='keypoint_relay', # Remove the .py extension + executable='keypoint_relay.py', # Specifying the subdirectory name='keypoint_relay', output='screen', - arguments=[LaunchConfiguration('model')], # Corrected arguments + arguments=[LaunchConfiguration('model')], ), - # Include the video stream launch file + + + # Include the v4l2_camera launch file IncludeLaunchDescription( - PythonLaunchDescriptionSource(video_stream_launch_file), - launch_arguments={'visualize': 'true'}.items(), + 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 index bca602f8c..08460b312 100644 --- a/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py @@ -2,9 +2,9 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch_ros.actions import Node from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration, TextSubstitution import os +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): # Declare launch arguments @@ -14,22 +14,24 @@ def generate_launch_description(): # Path to the BodyPix launch file bodypix_launch_file = os.path.join( - get_package_share_directory('lasr_vision_bodypix'), 'launch', 'bodypix.launch.py' + get_package_share_directory('lasr_vision_bodypix'), 'launch', 'bodypix_launch.py' ) - # Path to the video stream launch file - video_stream_launch_file = os.path.join( - get_package_share_directory('video_stream_opencv'), 'launch', 'camera.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' ) return LaunchDescription([ - # Declare model argument + # Declare the model argument model_arg, # Include BodyPix launch file IncludeLaunchDescription( PythonLaunchDescriptionSource(bodypix_launch_file), - launch_arguments={'preload': LaunchConfiguration('model')}.items(), # Corrected LaunchConfiguration usage + launch_arguments={ + 'preload': LaunchConfiguration('model') + }.items(), ), # Show debug topic using rqt_image_view @@ -38,23 +40,25 @@ def generate_launch_description(): executable='rqt_image_view', name='image_view', output='screen', - # Corrected: Directly passing LaunchConfiguration in arguments - arguments=[LaunchConfiguration('model')], + arguments=[ + TextSubstitution(text='/bodypix/debug/'), + LaunchConfiguration('model') + ], # Constructs the topic path dynamically ), - # Start the mask relay service + # Start the keypoint relay service Node( package='lasr_vision_bodypix', - executable='mask_relay', # Remove .py extension, assuming it's installed as executable - name='mask_relay', + executable='keypoint_relay.py', # Removed .py extension, assuming installed without it + name='keypoint_relay', output='screen', - # Corrected: Directly passing LaunchConfiguration in arguments - arguments=['/camera/image_raw', LaunchConfiguration('model')], + arguments=[LaunchConfiguration('model')], ), - # Include video stream launch file + # Include the v4l2_camera launch file IncludeLaunchDescription( - PythonLaunchDescriptionSource(video_stream_launch_file), - launch_arguments={'visualize': 'true'}.items(), + 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..b5a74b82d --- /dev/null +++ b/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py @@ -0,0 +1,19 @@ +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', # 摄像头设备路径 + 'image_width': 640, # 图像宽度 + 'image_height': 480, # 图像高度 + 'pixel_format': 'YUYV', # 像素格式 + '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 index 6470ad369..3745b17bd 100644 --- a/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py +++ b/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 + import rclpy from rclpy.node import Node import lasr_vision_bodypix as bodypix @@ -7,65 +8,66 @@ BodyPixKeypointDetection, DetectWave, ) -from sensor_msgs.msg import Image, PointCloud2 +from sensor_msgs.msg import Image +import ros2_numpy as rnp from geometry_msgs.msg import PointStamped, Point from std_msgs.msg import Header -from cv2_pcl import pcl_to_img_msg import numpy as np -import struct +from cv2_pcl import pcl_to_img_msg from typing import Union class BodyPixServiceNode(Node): def __init__(self): super().__init__("bodypix_service_node") - # Initialize parameters + # Declare and load parameters self.declare_parameter('preload', []) preload_models = self.get_parameter('preload').get_parameter_value().string_array_value - # Preload models if specified + # 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) + BodyPixMaskDetection, '/bodypix/mask_detection', self.detect_masks + ) self.keypoint_service = self.create_service( - BodyPixKeypointDetection, '/bodypix/keypoint_detection', self.detect_keypoints) + BodyPixKeypointDetection, '/bodypix/keypoint_detection', self.detect_keypoints + ) self.detect_wave_service = self.create_service( - DetectWave, '/bodypix/detect_wave', self.detect_wave) + 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): - """Detects body part masks.""" + """Handles mask detection request.""" response = bodypix.detect_masks(request) return response def detect_keypoints(self, request, response): - """Detects keypoints.""" + """Handles keypoint detection request.""" response = bodypix.detect_keypoints(request) return response def detect_wave(self, request, response): - """Detects waving gesture.""" + """Detects a waving gesture.""" try: - # Convert PointCloud2 to image (if needed by your BodyPix model) + # Prepare request for keypoint detection bp_req = BodyPixKeypointDetection.Request() - bp_req.image_raw = pcl_to_img_msg(request.pcl_msg) # Assuming you have a pcl_to_img_msg function. + bp_req.image_raw = pcl_to_img_msg(request.pcl_msg) bp_req.dataset = request.dataset bp_req.confidence = request.confidence - # Detect keypoints using the BodyPix model + # 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() - # Analyze keypoints gesture_to_detect = None keypoint_info = { keypoint.keypoint_name: {"x": keypoint.x, "y": keypoint.y} @@ -75,7 +77,6 @@ def detect_wave(self, request, response): 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" @@ -83,25 +84,23 @@ def detect_wave(self, request, response): if gesture_to_detect is not None: self.get_logger().info(f"Detected gesture: {gesture_to_detect}") - # Convert PointCloud2 to XYZ array - pcl_xyz = self.pointcloud2_to_xyz_array(request.pcl_msg) + # 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 + ) - # Determine wave position try: wave_position = np.zeros(3) - wave_point = keypoint_info.get( - "leftShoulder" if gesture_to_detect == "raising_left_arm" else "rightShoulder" - ) - - # Take the average of points around the detected keypoint 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") + 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), @@ -119,36 +118,12 @@ def detect_wave(self, request, response): response.wave_position = wave_position_msg return response - def pointcloud2_to_xyz_array(self, cloud_msg): - """ - Converts a PointCloud2 message to a numpy array of shape (N, 3), with fields x, y, z. - """ - points = [] - for point in self.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=True): - points.append([point[0], point[1], point[2]]) - return np.array(points) - - def read_points(self, cloud, field_names=None, skip_nans=True): - """ - Generator for reading points from a PointCloud2 message. - Optionally filters by field_names and skips NaNs. - """ - fmt = "fff" # Format for x, y, z - point_step = cloud.point_step - for i in range(0, len(cloud.data), point_step): - x, y, z = struct.unpack_from(fmt, cloud.data, i) - if skip_nans and (x != x or y != y or z != z): # Check for NaNs - continue - yield (x, y, z) - - def main(args=None): rclpy.init(args=args) node = BodyPixServiceNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/common/vision/lasr_vision_bodypix/setup.py b/common/vision/lasr_vision_bodypix/setup.py index 9042d8a33..df1ee2e57 100644 --- a/common/vision/lasr_vision_bodypix/setup.py +++ b/common/vision/lasr_vision_bodypix/setup.py @@ -1,23 +1,24 @@ from setuptools import setup, find_packages - setup( name='lasr_vision_bodypix', version='0.0.0', - packages=["lasr_vision_bodypix"], - package_dir={'': 'src'}, + 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.src.lasr_vision_bodypix.bodypix:main', + 'bodypix_node = lasr_vision_bodypix.bodypix:main', + 'keypoint_relay = lasr_vision_bodypix.examples.keypoint_relay:main', ], } ) - - 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 index 653b1c253..d6ec521eb 100755 --- a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -12,13 +12,15 @@ from lasr_vision_msgs.msg import BodyPixMask, BodyPixKeypoint, BodyPixKeypointNormalized from lasr_vision_msgs.srv import ( BodyPixMaskDetection, - BodyPixMaskDetection_Request, - BodyPixMaskDetection_Response, BodyPixKeypointDetection, - BodyPixKeypointDetection_Request, - BodyPixKeypointDetection_Response, + ) +BodyPixKeypointDetection_Request = BodyPixKeypointDetection.Request() +BodyPixKeypointDetection_Response = BodyPixKeypointDetection.Response() +BodyPixMaskDetection_Request = BodyPixMaskDetection.Request() +BodyPixMaskDetection_Response = BodyPixMaskDetection.Response() + # model cache loaded_models = {} @@ -188,3 +190,7 @@ def main(args=None): node.destroy_node() rclpy.shutdown() + +if __name__ == "__main__": + main() + diff --git a/common/vision/lasr_vision_msgs/CMakeLists.txt b/common/vision/lasr_vision_msgs/CMakeLists.txt index 0ad7dec3f..097ce718d 100644 --- a/common/vision/lasr_vision_msgs/CMakeLists.txt +++ b/common/vision/lasr_vision_msgs/CMakeLists.txt @@ -25,6 +25,7 @@ set(srv_files "srv/DetectWave.srv" ) + # Generate messages and services rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} From 2c6134e1f3a06551c90777133ad0057ff5b39e4f Mon Sep 17 00:00:00 2001 From: Siyao Date: Tue, 19 Nov 2024 14:15:57 +0000 Subject: [PATCH 04/10] finially the ROS2 version bodypix all good! --- .../vision/lasr_vision_bodypix/CMakeLists.txt | 4 + .../examples/keypoint_relay.py | 13 +- .../examples/mask_relay.py | 97 ++++++++ .../launch/bodypix_launch.py | 9 +- .../launch/camera_keypoint_launch.py | 35 ++- .../launch/camera_mask_launch.py | 35 ++- .../launch/v4l2_camera_launch.py | 10 +- .../nodes/bodypix_services.py | 6 +- common/vision/lasr_vision_bodypix/setup.py | 1 + .../src/lasr_vision_bodypix/__init__.py | 4 +- .../src/lasr_vision_bodypix/bodypix.py | 230 +++++++++--------- .../srv/BodyPixKeypointDetection.srv | 2 + 12 files changed, 303 insertions(+), 143 deletions(-) create mode 100644 common/vision/lasr_vision_bodypix/examples/mask_relay.py diff --git a/common/vision/lasr_vision_bodypix/CMakeLists.txt b/common/vision/lasr_vision_bodypix/CMakeLists.txt index 80ac5e1e4..8e7841486 100644 --- a/common/vision/lasr_vision_bodypix/CMakeLists.txt +++ b/common/vision/lasr_vision_bodypix/CMakeLists.txt @@ -28,6 +28,10 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) +install(PROGRAMS + examples/mask_relay.py + DESTINATION lib/${PROJECT_NAME} +) # Install launch files install(DIRECTORY launch diff --git a/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py index 7673b7efd..a588d88a2 100755 --- a/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py +++ b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py @@ -5,7 +5,7 @@ import threading from sensor_msgs.msg import Image -from lasr_vision_msgs.srv import BodyPixKeypointDetection, BodyPixKeypointDetectionRequest +from lasr_vision_msgs.srv import BodyPixKeypointDetection class KeypointRelay(Node): def __init__(self, listen_topic, model): @@ -31,9 +31,8 @@ def __init__(self, listen_topic, model): def detect(self, image): self.processing = True self.get_logger().info("Received image message") - # Create a request for the service - req = BodyPixKeypointDetectionRequest() + req = BodyPixKeypointDetection.Request() req.image_raw = image req.dataset = self.model req.confidence = 0.7 @@ -60,17 +59,23 @@ def image_callback(self, image): 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 = sys.argv[1] + + 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) 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..bdfbbf66f --- /dev/null +++ b/common/vision/lasr_vision_bodypix/examples/mask_relay.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 + +import sys +import threading +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from lasr_vision_msgs.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 index a3593bdeb..7cb3ae339 100644 --- a/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py @@ -4,11 +4,14 @@ from launch.substitutions import LaunchConfiguration def generate_launch_description(): - # Declare preload argument with default value + # Declare preload argument with default value as a YAML list preload_arg = DeclareLaunchArgument( - 'preload', default_value="['resnet50']", description='Array of models to preload when starting the service' + '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', diff --git a/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py b/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py index a8003d7c0..b23c87d3e 100644 --- a/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py @@ -2,16 +2,24 @@ 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 +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' + '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' @@ -45,8 +53,15 @@ def generate_launch_description(): name='image_view', output='screen', arguments=[ - TextSubstitution(text='/bodypix/debug/'), - LaunchConfiguration('model') + + # [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 ), @@ -56,7 +71,17 @@ def generate_launch_description(): executable='keypoint_relay.py', # Specifying the subdirectory name='keypoint_relay', output='screen', - arguments=[LaunchConfiguration('model')], + 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], ), diff --git a/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py b/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py index 08460b312..935ecf8e4 100644 --- a/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py @@ -2,14 +2,20 @@ 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 +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' + '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 @@ -19,7 +25,7 @@ def generate_launch_description(): # 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' + get_package_share_directory('lasr_vision_bodypix'), 'launch', 'v4l2_camera_launch.py' ) return LaunchDescription([ @@ -41,18 +47,31 @@ def generate_launch_description(): name='image_view', output='screen', arguments=[ - TextSubstitution(text='/bodypix/debug/'), - LaunchConfiguration('model') + # [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', # Removed .py extension, assuming installed without it - name='keypoint_relay', + executable='mask_relay.py', # Removed .py extension, assuming installed without it + name='mask_relay', output='screen', - arguments=[LaunchConfiguration('model')], + 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 diff --git a/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py b/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py index b5a74b82d..27aeac56a 100644 --- a/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py @@ -9,11 +9,11 @@ def generate_launch_description(): name='v4l2_camera_node', output='screen', parameters=[{ - 'video_device': '/dev/video0', # 摄像头设备路径 - 'image_width': 640, # 图像宽度 - 'image_height': 480, # 图像高度 - 'pixel_format': 'YUYV', # 像素格式 - 'frame_rate': 30, # 帧率 + '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 index 3745b17bd..ddc9bf288 100644 --- a/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py +++ b/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py @@ -15,15 +15,18 @@ 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', []) + 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) @@ -35,6 +38,7 @@ def __init__(self): 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 ) diff --git a/common/vision/lasr_vision_bodypix/setup.py b/common/vision/lasr_vision_bodypix/setup.py index df1ee2e57..255cd85aa 100644 --- a/common/vision/lasr_vision_bodypix/setup.py +++ b/common/vision/lasr_vision_bodypix/setup.py @@ -16,6 +16,7 @@ '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 index fb8a7222c..46ef209da 100755 --- a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/__init__.py +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/__init__.py @@ -2,5 +2,7 @@ snake_to_camel, camel_to_snake, load_model_cached, - BodyPixNode, + 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 index d6ec521eb..84add7cfa 100755 --- a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -58,139 +58,137 @@ def load_model_cached(dataset: str): return model -class BodyPixNode(Node): - def __init__(self): - super().__init__('bodypix_service_node') - self.mask_service = self.create_service( - BodyPixMaskDetection, 'detect_masks', self.detect_masks - ) - self.keypoint_service = self.create_service( - BodyPixKeypointDetection, 'detect_keypoints', self.detect_keypoints - ) - self.debug_publisher_mask = self.create_publisher( - SensorImage, '/bodypix/mask_debug', 1 - ) - self.debug_publisher_keypoint = self.create_publisher( - SensorImage, '/bodypix/keypoint_debug', 1 - ) +def run_inference(dataset: str, confidence: float, img: SensorImage, logger=None): + """ + Run inference on an image. + """ + # Decode the image + if logger: + logger.info("Decoding") + img = cv2_img.msg_to_cv2_img(img) - def run_inference(self, dataset: str, confidence: float, img: SensorImage): - self.get_logger().info("Decoding") - img = cv2_img.msg_to_cv2_img(img) + # Load model + if logger: + logger.info("Loading model") + model = load_model_cached(dataset) - self.get_logger().info("Loading model") - model = load_model_cached(dataset) + # Run inference + if logger: + logger.info("Running inference") + result = model.predict_single(img) - self.get_logger().info("Running inference") - result = model.predict_single(img) + mask = result.get_mask(threshold=confidence) + if logger: + logger.info("Inference complete") - mask = result.get_mask(threshold=confidence) - self.get_logger().info("Inference complete") - return result, mask + return result, mask - def detect_masks( - self, request: BodyPixMaskDetection_Request, response: BodyPixMaskDetection_Response - ) -> BodyPixMaskDetection_Response: - result, mask = self.run_inference(request.dataset, request.confidence, request.image_raw) - masks = [] - for part_name in request.parts: - part_mask = result.get_part_mask(mask=tf.identity(mask), part_names=[part_name]).squeeze() +def detect_masks(request: BodyPixMaskDetection_Request, debug_publisher=None, logger=None): + """ + Run BodyPix inference for mask detection. + """ + result, mask = run_inference(request.dataset, request.confidence, request.image_raw, logger) - if np.max(part_mask) == 0: - self.get_logger().warn(f"No masks found for part {part_name}") - continue + masks = [] - bodypix_mask = BodyPixMask() - bodypix_mask.mask = part_mask.flatten().astype(bool).tolist() - bodypix_mask.shape = list(part_mask.shape) - bodypix_mask.part_name = part_name - masks.append(bodypix_mask) - - # Publish to debug topic - if self.debug_publisher_mask is not None: - from tf_bodypix.draw import draw_poses - coloured_mask = result.get_colored_part_mask(mask).astype(np.uint8) - poses = result.get_poses() - coloured_mask = draw_poses( - coloured_mask.copy(), - poses, - keypoints_color=(255, 100, 100), - skeleton_color=(100, 100, 255), - ) - self.debug_publisher_mask.publish(cv2_img.cv2_img_to_msg(coloured_mask)) + for part_name in request.parts: + part_mask = result.get_part_mask(mask=tf.identity(mask), part_names=[part_name]).squeeze() - response.masks = masks - return response + if np.max(part_mask) == 0: + if logger: + logger.warning(f"No masks found for part {part_name}") + continue - def detect_keypoints( - self, request: BodyPixKeypointDetection_Request, response: BodyPixKeypointDetection_Response - ) -> BodyPixKeypointDetection_Response: - result, mask = self.run_inference(request.dataset, request.confidence, request.image_raw) + bodypix_mask = BodyPixMask() + bodypix_mask.mask = part_mask.flatten().astype(bool).tolist() + bodypix_mask.shape = list(part_mask.shape) + bodypix_mask.part_name = part_name + masks.append(bodypix_mask) + # 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) poses = result.get_poses() - detected_keypoints: List[BodyPixKeypoint] = [] - detected_keypoints_normalized: List[BodyPixKeypointNormalized] = [] - - for pose in poses: - for keypoint in pose.keypoints.values(): - x = int(keypoint.position.x) - y = int(keypoint.position.y) - try: - if not request.keep_out_of_bounds: - if x < 0.0 or y < 0.0: - continue - if x >= mask.shape[1] or y >= mask.shape[0]: - continue - except: - continue - self.get_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], - ) - ) + coloured_mask = draw_poses( + coloured_mask.copy(), + poses, + keypoints_color=(255, 100, 100), + skeleton_color=(100, 100, 255), + ) + debug_publisher.publish(cv2_img.cv2_img_to_msg(coloured_mask)) + + response = BodyPixMaskDetection_Response + response.masks = masks + return response + + +def detect_keypoints(request: BodyPixKeypointDetection_Request, debug_publisher=None, logger=None): + """ + Run BodyPix inference for keypoint detection. + """ + result, mask = run_inference(request.dataset, request.confidence, request.image_raw, logger) + + poses = result.get_poses() + detected_keypoints: List[BodyPixKeypoint] = [] + detected_keypoints_normalized: List[BodyPixKeypointNormalized] = [] + + for pose in poses: + for keypoint in pose.keypoints.values(): + x = int(keypoint.position.x) + y = int(keypoint.position.y) + try: + if not request.keep_out_of_bounds: + if x < 0.0 or y < 0.0: + continue + if x >= 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") - if self.debug_publisher_keypoint is not None: - 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), + detected_keypoints.append( + BodyPixKeypoint(keypoint_name=keypoint.part, x=x, y=y) ) - 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, + detected_keypoints_normalized.append( + BodyPixKeypointNormalized( + keypoint_name=keypoint.part, + x=float(x) / mask.shape[1], + y=float(y) / mask.shape[0], ) - self.debug_publisher_keypoint.publish(cv2_img.cv2_img_to_msg(coloured_mask)) - - response.keypoints = detected_keypoints - response.normalized_keypoints = detected_keypoints_normalized - return response + ) + # Publish debug visualization if enabled + if debug_publisher: + from tf_bodypix.draw import draw_poses -def main(args=None): - rclpy.init(args=args) - node = BodyPixNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() + 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)) -if __name__ == "__main__": - main() + response = BodyPixKeypointDetection_Response + response.keypoints = detected_keypoints + response.normalized_keypoints = detected_keypoints_normalized + return response \ No newline at end of file diff --git a/common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv b/common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv index 36034b64f..2587bd0ed 100644 --- a/common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv +++ b/common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv @@ -1,3 +1,4 @@ +# Request # Image to run inference on sensor_msgs/Image image_raw @@ -10,6 +11,7 @@ float32 confidence # Whether to return keypoints that are out of bound bool keep_out_of_bounds --- +# Response # keypoints lasr_vision_msgs/BodyPixKeypoint[] keypoints From 460ba5d24900de133607590a42e443522d73c93d Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Tue, 19 Nov 2024 14:48:13 +0000 Subject: [PATCH 05/10] chore: remove leg_tracker. --- common/third_party/leg_tracker | 1 - 1 file changed, 1 deletion(-) delete mode 160000 common/third_party/leg_tracker diff --git a/common/third_party/leg_tracker b/common/third_party/leg_tracker deleted file mode 160000 index e6cbb2bba..000000000 --- a/common/third_party/leg_tracker +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e6cbb2bba218e0684714a08972a7bdfa99118e3c From 796e543cf9f99862c3c723bfd56043bb17ce427c Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Tue, 19 Nov 2024 14:50:17 +0000 Subject: [PATCH 06/10] chore: delete workspace files. --- common/install/.colcon_install_layout | 1 - common/install/COLCON_IGNORE | 0 common/install/_local_setup_util_ps1.py | 407 ------------------ common/install/_local_setup_util_sh.py | 407 ------------------ common/install/lasr_vision_bodypix/.catkin | 0 .../colcon-core/packages/lasr_vision_bodypix | 1 - .../share/lasr_vision_bodypix/package.bash | 31 -- .../share/lasr_vision_bodypix/package.dsv | 0 .../share/lasr_vision_bodypix/package.ps1 | 108 ----- .../share/lasr_vision_bodypix/package.sh | 52 --- .../share/lasr_vision_bodypix/package.zsh | 42 -- common/install/local_setup.bash | 121 ------ common/install/local_setup.ps1 | 55 --- common/install/local_setup.sh | 137 ------ common/install/local_setup.zsh | 134 ------ common/install/setup.bash | 40 -- common/install/setup.ps1 | 32 -- common/install/setup.sh | 57 --- common/install/setup.zsh | 40 -- common/log/COLCON_IGNORE | 0 common/log/latest | 1 - common/log/latest_build | 1 - 22 files changed, 1667 deletions(-) delete mode 100644 common/install/.colcon_install_layout delete mode 100644 common/install/COLCON_IGNORE delete mode 100644 common/install/_local_setup_util_ps1.py delete mode 100644 common/install/_local_setup_util_sh.py delete mode 100644 common/install/lasr_vision_bodypix/.catkin delete mode 100644 common/install/lasr_vision_bodypix/share/colcon-core/packages/lasr_vision_bodypix delete mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.bash delete mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.dsv delete mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.ps1 delete mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.sh delete mode 100644 common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.zsh delete mode 100644 common/install/local_setup.bash delete mode 100644 common/install/local_setup.ps1 delete mode 100644 common/install/local_setup.sh delete mode 100644 common/install/local_setup.zsh delete mode 100644 common/install/setup.bash delete mode 100644 common/install/setup.ps1 delete mode 100644 common/install/setup.sh delete mode 100644 common/install/setup.zsh delete mode 100644 common/log/COLCON_IGNORE delete mode 120000 common/log/latest delete mode 120000 common/log/latest_build diff --git a/common/install/.colcon_install_layout b/common/install/.colcon_install_layout deleted file mode 100644 index 3aad5336a..000000000 --- a/common/install/.colcon_install_layout +++ /dev/null @@ -1 +0,0 @@ -isolated diff --git a/common/install/COLCON_IGNORE b/common/install/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/common/install/_local_setup_util_ps1.py b/common/install/_local_setup_util_ps1.py deleted file mode 100644 index 3c6d9e877..000000000 --- a/common/install/_local_setup_util_ps1.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' -FORMAT_STR_USE_ENV_VAR = '$env:{name}' -FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/common/install/_local_setup_util_sh.py b/common/install/_local_setup_util_sh.py deleted file mode 100644 index f67eaa989..000000000 --- a/common/install/_local_setup_util_sh.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' -FORMAT_STR_USE_ENV_VAR = '${name}' -FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/common/install/lasr_vision_bodypix/.catkin b/common/install/lasr_vision_bodypix/.catkin deleted file mode 100644 index e69de29bb..000000000 diff --git a/common/install/lasr_vision_bodypix/share/colcon-core/packages/lasr_vision_bodypix b/common/install/lasr_vision_bodypix/share/colcon-core/packages/lasr_vision_bodypix deleted file mode 100644 index 1529e8996..000000000 --- a/common/install/lasr_vision_bodypix/share/colcon-core/packages/lasr_vision_bodypix +++ /dev/null @@ -1 +0,0 @@ -ament_python:cv2_img:lasr_vision_msgs:rosidl_default_runtime \ No newline at end of file diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.bash b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.bash deleted file mode 100644 index 24600eb16..000000000 --- a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.bash +++ /dev/null @@ -1,31 +0,0 @@ -# generated from colcon_bash/shell/template/package.bash.em - -# This script extends the environment for this package. - -# a bash script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - # the prefix is two levels up from the package specific share directory - _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)" -else - _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -_colcon_package_bash_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$@" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source sh script of this package -_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/lasr_vision_bodypix/package.sh" - -unset _colcon_package_bash_source_script -unset _colcon_package_bash_COLCON_CURRENT_PREFIX diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.dsv b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.dsv deleted file mode 100644 index e69de29bb..000000000 diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.ps1 b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.ps1 deleted file mode 100644 index 4198e42ec..000000000 --- a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.ps1 +++ /dev/null @@ -1,108 +0,0 @@ -# generated from colcon_powershell/shell/template/package.ps1.em - -# function to append a value to a variable -# which uses colons as separators -# duplicates as well as leading separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -function colcon_append_unique_value { - param ( - $_listname, - $_value - ) - - # get values from variable - if (Test-Path Env:$_listname) { - $_values=(Get-Item env:$_listname).Value - } else { - $_values="" - } - $_duplicate="" - # start with no values - $_all_values="" - # iterate over existing values in the variable - if ($_values) { - $_values.Split(";") | ForEach { - # not an empty string - if ($_) { - # not a duplicate of _value - if ($_ -eq $_value) { - $_duplicate="1" - } - if ($_all_values) { - $_all_values="${_all_values};$_" - } else { - $_all_values="$_" - } - } - } - } - # append only non-duplicates - if (!$_duplicate) { - # avoid leading separator - if ($_all_values) { - $_all_values="${_all_values};${_value}" - } else { - $_all_values="${_value}" - } - } - - # export the updated variable - Set-Item env:\$_listname -Value "$_all_values" -} - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -function colcon_prepend_unique_value { - param ( - $_listname, - $_value - ) - - # get values from variable - if (Test-Path Env:$_listname) { - $_values=(Get-Item env:$_listname).Value - } else { - $_values="" - } - # start with the new value - $_all_values="$_value" - # iterate over existing values in the variable - if ($_values) { - $_values.Split(";") | ForEach { - # not an empty string - if ($_) { - # not a duplicate of _value - if ($_ -ne $_value) { - # keep non-duplicate values - $_all_values="${_all_values};$_" - } - } - } - } - # export the updated variable - Set-Item env:\$_listname -Value "$_all_values" -} - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -function colcon_package_source_powershell_script { - param ( - $_colcon_package_source_powershell_script - ) - # source script with conditional trace output - if (Test-Path $_colcon_package_source_powershell_script) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_package_source_powershell_script'" - } - . "$_colcon_package_source_powershell_script" - } else { - Write-Error "not found: '$_colcon_package_source_powershell_script'" - } -} - - diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.sh b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.sh deleted file mode 100644 index 7d7278e5f..000000000 --- a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.sh +++ /dev/null @@ -1,52 +0,0 @@ -# generated from colcon_core/shell/template/package.sh.em - -# This script extends the environment for this package. - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prepend_unique_value_IFS=$IFS - IFS=":" - # start with the new value - _all_values="$_value" - # workaround SH_WORD_SPLIT not being set in zsh - if [ "$(command -v colcon_zsh_convert_to_array)" ]; then - colcon_zsh_convert_to_array _values - fi - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - # restore the field separator - IFS=$_colcon_prepend_unique_value_IFS - unset _colcon_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks diff --git a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.zsh b/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.zsh deleted file mode 100644 index 59979cc29..000000000 --- a/common/install/lasr_vision_bodypix/share/lasr_vision_bodypix/package.zsh +++ /dev/null @@ -1,42 +0,0 @@ -# generated from colcon_zsh/shell/template/package.zsh.em - -# This script extends the environment for this package. - -# a zsh script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - # the prefix is two levels up from the package specific share directory - _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)" -else - _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -_colcon_package_zsh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$@" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# function to convert array-like strings into arrays -# to workaround SH_WORD_SPLIT not being set -colcon_zsh_convert_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# source sh script of this package -_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/lasr_vision_bodypix/package.sh" -unset convert_zsh_to_array - -unset _colcon_package_zsh_source_script -unset _colcon_package_zsh_COLCON_CURRENT_PREFIX diff --git a/common/install/local_setup.bash b/common/install/local_setup.bash deleted file mode 100644 index 03f00256c..000000000 --- a/common/install/local_setup.bash +++ /dev/null @@ -1,121 +0,0 @@ -# generated from colcon_bash/shell/template/prefix.bash.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a bash script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -else - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_bash_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" - unset _colcon_prefix_bash_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_bash_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/common/install/local_setup.ps1 b/common/install/local_setup.ps1 deleted file mode 100644 index 6f68c8ded..000000000 --- a/common/install/local_setup.ps1 +++ /dev/null @@ -1,55 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix.ps1.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# check environment variable for custom Python executable -if ($env:COLCON_PYTHON_EXECUTABLE) { - if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { - echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" - exit 1 - } - $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" -} else { - # use the Python executable known at configure time - $_colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { - if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { - echo "error: unable to find python3 executable" - exit 1 - } - $_colcon_python_executable="python3" - } -} - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_powershell_source_script { - param ( - $_colcon_prefix_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_powershell_source_script_param'" - } - . "$_colcon_prefix_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" - } -} - -# get all commands in topological order -$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 - -# execute all commands in topological order -if ($env:COLCON_TRACE) { - echo "Execute generated script:" - echo "<<<" - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output - echo ">>>" -} -if ($_colcon_ordered_commands) { - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression -} diff --git a/common/install/local_setup.sh b/common/install/local_setup.sh deleted file mode 100644 index b384bf289..000000000 --- a/common/install/local_setup.sh +++ /dev/null @@ -1,137 +0,0 @@ -# generated from colcon_core/shell/template/prefix.sh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/siyao/project/robo_cup_mine/src/Base/common/install" -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX - return 1 - fi -else - _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_sh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" - unset _colcon_prefix_sh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_sh_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "_colcon_prefix_sh_source_script() { - if [ -f \"\$1\" ]; then - if [ -n \"\$COLCON_TRACE\" ]; then - echo \"# . \\\"\$1\\\"\" - fi - . \"\$1\" - else - echo \"not found: \\\"\$1\\\"\" 1>&2 - fi - }" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/common/install/local_setup.zsh b/common/install/local_setup.zsh deleted file mode 100644 index b6487102f..000000000 --- a/common/install/local_setup.zsh +++ /dev/null @@ -1,134 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix.zsh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a zsh script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -else - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to convert array-like strings into arrays -# to workaround SH_WORD_SPLIT not being set -_colcon_prefix_zsh_convert_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_zsh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # workaround SH_WORD_SPLIT not being set - _colcon_prefix_zsh_convert_to_array _values - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" - unset _colcon_prefix_zsh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_zsh_prepend_unique_value -unset _colcon_prefix_zsh_convert_to_array - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/common/install/setup.bash b/common/install/setup.bash deleted file mode 100644 index 66d590f66..000000000 --- a/common/install/setup.bash +++ /dev/null @@ -1,40 +0,0 @@ -# generated from colcon_bash/shell/template/prefix_chain.bash.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_bash_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/home/siyao/project/Learning_project/ros2_dev_ws/install" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/tiago_public_ws/install" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/home/siyao/project/robo_cup_mine/install" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_bash_source_script diff --git a/common/install/setup.ps1 b/common/install/setup.ps1 deleted file mode 100644 index ff66f3750..000000000 --- a/common/install/setup.ps1 +++ /dev/null @@ -1,32 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix_chain.ps1.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_chain_powershell_source_script { - param ( - $_colcon_prefix_chain_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_chain_powershell_source_script_param'" - } - . "$_colcon_prefix_chain_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" - } -} - -# source chained prefixes -_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1" -_colcon_prefix_chain_powershell_source_script "/home/siyao/project/Learning_project/ros2_dev_ws/install\local_setup.ps1" -_colcon_prefix_chain_powershell_source_script "/opt/tiago_public_ws/install\local_setup.ps1" -_colcon_prefix_chain_powershell_source_script "/home/siyao/project/robo_cup_mine/install\local_setup.ps1" - -# source this prefix -$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) -_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/common/install/setup.sh b/common/install/setup.sh deleted file mode 100644 index 8823bfda6..000000000 --- a/common/install/setup.sh +++ /dev/null @@ -1,57 +0,0 @@ -# generated from colcon_core/shell/template/prefix_chain.sh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/siyao/project/robo_cup_mine/src/Base/common/install -if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX - return 1 -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/home/siyao/project/Learning_project/ros2_dev_ws/install" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/opt/tiago_public_ws/install" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/home/siyao/project/robo_cup_mine/install" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_sh_source_script -unset COLCON_CURRENT_PREFIX diff --git a/common/install/setup.zsh b/common/install/setup.zsh deleted file mode 100644 index e7ba993d8..000000000 --- a/common/install/setup.zsh +++ /dev/null @@ -1,40 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix_chain.zsh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_zsh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/home/siyao/project/Learning_project/ros2_dev_ws/install" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/tiago_public_ws/install" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/home/siyao/project/robo_cup_mine/install" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_zsh_source_script diff --git a/common/log/COLCON_IGNORE b/common/log/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/common/log/latest b/common/log/latest deleted file mode 120000 index b57d247c7..000000000 --- a/common/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_build \ No newline at end of file diff --git a/common/log/latest_build b/common/log/latest_build deleted file mode 120000 index c785255f2..000000000 --- a/common/log/latest_build +++ /dev/null @@ -1 +0,0 @@ -build_2024-10-06_21-04-11 \ No newline at end of file From 968af070f04914dde287a567c49394bdc69722e4 Mon Sep 17 00:00:00 2001 From: Jared Swift Date: Tue, 19 Nov 2024 14:52:12 +0000 Subject: [PATCH 07/10] refactor: black format. --- common/helpers/cv2_img/setup.py | 20 +-- .../helpers/cv2_img/src/cv2_img/__init__.py | 3 + common/helpers/cv2_pcl/setup.py | 20 +-- .../helpers/cv2_pcl/src/cv2_pcl/__init__.py | 22 ++- .../examples/keypoint_relay.py | 24 ++-- .../examples/mask_relay.py | 25 ++-- .../launch/bodypix_launch.py | 29 ++-- .../launch/camera_keypoint_launch.py | 131 +++++++++--------- .../launch/camera_mask_launch.py | 122 ++++++++-------- .../launch/v4l2_camera_launch.py | 35 +++-- .../nodes/bodypix_services.py | 36 +++-- common/vision/lasr_vision_bodypix/setup.py | 34 +++-- .../src/lasr_vision_bodypix/bodypix.py | 23 ++- 13 files changed, 292 insertions(+), 232 deletions(-) diff --git a/common/helpers/cv2_img/setup.py b/common/helpers/cv2_img/setup.py index 8edff4000..4327576d8 100644 --- a/common/helpers/cv2_img/setup.py +++ b/common/helpers/cv2_img/setup.py @@ -1,21 +1,21 @@ from setuptools import setup -package_name = 'cv2_img' +package_name = "cv2_img" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], - package_dir={'': 'src'}, - install_requires=['setuptools'], + 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'], + 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': [ + "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 index 4f47537f9..ec47676ed 100644 --- a/common/helpers/cv2_img/src/cv2_img/__init__.py +++ b/common/helpers/cv2_img/src/cv2_img/__init__.py @@ -33,6 +33,7 @@ def cv2_img_to_msg(img, stamp=None): return msg + def msg_to_pillow_img(msg: SensorImage): """ Convert a given sensor image to a pillow image @@ -53,6 +54,7 @@ def msg_to_pillow_img(msg: SensorImage): return img + def msg_to_cv2_img(msg: SensorImage): """ Convert a given sensor image to a cv2 image @@ -68,6 +70,7 @@ def msg_to_cv2_img(msg: SensorImage): 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. diff --git a/common/helpers/cv2_pcl/setup.py b/common/helpers/cv2_pcl/setup.py index 8a5ed0286..08d389031 100644 --- a/common/helpers/cv2_pcl/setup.py +++ b/common/helpers/cv2_pcl/setup.py @@ -1,21 +1,21 @@ from setuptools import setup -package_name = 'cv2_pcl' +package_name = "cv2_pcl" setup( name=package_name, - version='0.0.0', + version="0.0.0", packages=[package_name], - package_dir={'': 'src'}, - install_requires=['setuptools'], + 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'], + 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': [ + "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 index cc1d99f94..e473b43a2 100644 --- a/common/helpers/cv2_pcl/src/cv2_pcl/__init__.py +++ b/common/helpers/cv2_pcl/src/cv2_pcl/__init__.py @@ -11,6 +11,7 @@ Mat = np.ndarray + def pointcloud2_to_xyz_array(pointcloud: PointCloud2, remove_nans=True): """ Convert a sensor_msgs/PointCloud2 message to an Nx3 NumPy array. @@ -24,8 +25,12 @@ def pointcloud2_to_xyz_array(pointcloud: PointCloud2, remove_nans=True): 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) + 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 @@ -33,9 +38,10 @@ def pointcloud2_to_xyz_array(pointcloud: PointCloud2, remove_nans=True): 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. @@ -43,9 +49,10 @@ def _get_struct_fmt(cloud_msg: PointCloud2): :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) + 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. @@ -55,6 +62,7 @@ def pcl_to_img_msg(pcl: PointCloud2) -> Mat: 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: @@ -69,7 +77,9 @@ def pcl_to_cv2( # 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 = (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. @@ -77,6 +87,7 @@ def pcl_to_cv2( return frame + def seg_to_centroid( pcl: PointCloud2, xyseg: np.ndarray, @@ -112,6 +123,7 @@ def seg_to_centroid( # Compute the centroid of the points return np.nanmean(xyz_points, axis=0) + def bb_to_centroid( pcl: PointCloud2, x: int, diff --git a/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py index a588d88a2..579781a00 100755 --- a/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py +++ b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py @@ -7,26 +7,28 @@ from sensor_msgs.msg import Image from lasr_vision_msgs.srv import BodyPixKeypointDetection + class KeypointRelay(Node): def __init__(self, listen_topic, model): - super().__init__('keypoint_relay') + 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') + 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...') + 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 + 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}" ) - self.get_logger().info(f'Started listening on topic: {self.listen_topic} with model: {self.model}') def detect(self, image): self.processing = True @@ -62,12 +64,14 @@ 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|...]") + print( + "Usage: ros2 run lasr_vision_bodypix keypoint_relay.py [resnet50|mobilenet50|...]" + ) sys.exit(1) # Parse the command-line arguments - listen_topic = '/image_raw' + listen_topic = "/image_raw" if isinstance(sys.argv[1], list): listen_topic = sys.argv[1][0] diff --git a/common/vision/lasr_vision_bodypix/examples/mask_relay.py b/common/vision/lasr_vision_bodypix/examples/mask_relay.py index bdfbbf66f..67fb28a1e 100644 --- a/common/vision/lasr_vision_bodypix/examples/mask_relay.py +++ b/common/vision/lasr_vision_bodypix/examples/mask_relay.py @@ -10,24 +10,25 @@ class ImageListener(Node): def __init__(self, listen_topic, model): - super().__init__('image_listener') + 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') + 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...') + 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 + 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}" ) - self.get_logger().info(f'Started listening on topic: {self.listen_topic} with model: {self.model}') def detect(self, image): self.processing = True @@ -70,14 +71,16 @@ 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|...]") + print( + "Usage: ros2 run lasr_vision_bodypix mask_relay.py [resnet50|mobilenet50|...]" + ) sys.exit(1) # Parse the command-line arguments - listen_topic = '/image_raw' + 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) diff --git a/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py b/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py index 7cb3ae339..3c723a3d4 100644 --- a/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/bodypix_launch.py @@ -3,26 +3,29 @@ 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'], + "preload", + default_value=["resnet50"], # default_value="['resnet50', 'mobilenet50']", - description='Array of models to preload when starting the service' + 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')}], + 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 - ]) + 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 index b23c87d3e..b5d0f8631 100644 --- a/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/camera_keypoint_launch.py @@ -7,22 +7,24 @@ 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' + "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' + "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' + get_package_share_directory("lasr_vision_bodypix"), + "launch", + "bodypix_launch.py", ) # # Path to the v4l2_camera launch file (replacement for video_stream_opencv) @@ -31,64 +33,63 @@ def generate_launch_description(): # ) v4l2_camera_launch_file = os.path.join( - get_package_share_directory('lasr_vision_bodypix'), 'launch', 'v4l2_camera_launch.py' + 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(), - ), - ]) + 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 index 935ecf8e4..c033273f2 100644 --- a/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/camera_mask_launch.py @@ -6,78 +6,82 @@ 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' + "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' + "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' + 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' + 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(), - ), - ]) - + 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 index 27aeac56a..91351e459 100644 --- a/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py +++ b/common/vision/lasr_vision_bodypix/launch/v4l2_camera_launch.py @@ -1,19 +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, - }], - ), - ]) + 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 index ddc9bf288..39084a4dc 100644 --- a/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py +++ b/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py @@ -17,15 +17,21 @@ 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 - + 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: @@ -33,14 +39,16 @@ def __init__(self): # Create service servers self.mask_service = self.create_service( - BodyPixMaskDetection, '/bodypix/mask_detection', self.detect_masks + BodyPixMaskDetection, "/bodypix/mask_detection", self.detect_masks ) self.keypoint_service = self.create_service( - BodyPixKeypointDetection, '/bodypix/keypoint_detection', self.detect_keypoints + 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 + DetectWave, "/bodypix/detect_wave", self.detect_wave ) # Debug publisher for detect_wave @@ -90,7 +98,9 @@ def detect_wave(self, request, response): # Process wave point in point cloud wave_point = keypoint_info.get( - "leftShoulder" if gesture_to_detect == "raising_left_arm" else "rightShoulder" + "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 @@ -100,10 +110,16 @@ def detect_wave(self, request, response): 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])): + 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 += pcl_xyz[int(wave_point["y"]) + i][ + int(wave_point["x"]) + j + ] wave_position /= 100 wave_position_msg = PointStamped( point=Point(*wave_position), @@ -122,6 +138,7 @@ def detect_wave(self, request, response): response.wave_position = wave_position_msg return response + def main(args=None): rclpy.init(args=args) node = BodyPixServiceNode() @@ -129,5 +146,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() + if __name__ == "__main__": main() diff --git a/common/vision/lasr_vision_bodypix/setup.py b/common/vision/lasr_vision_bodypix/setup.py index 255cd85aa..c58d27524 100644 --- a/common/vision/lasr_vision_bodypix/setup.py +++ b/common/vision/lasr_vision_bodypix/setup.py @@ -1,25 +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'], + 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', - + 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' + "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/bodypix.py b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py index 84add7cfa..a034c1326 100755 --- a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -13,7 +13,6 @@ from lasr_vision_msgs.srv import ( BodyPixMaskDetection, BodyPixKeypointDetection, - ) BodyPixKeypointDetection_Request = BodyPixKeypointDetection.Request() @@ -84,16 +83,22 @@ def run_inference(dataset: str, confidence: float, img: SensorImage, logger=None return result, mask -def detect_masks(request: BodyPixMaskDetection_Request, debug_publisher=None, logger=None): +def detect_masks( + request: BodyPixMaskDetection_Request, debug_publisher=None, logger=None +): """ Run BodyPix inference for mask detection. """ - result, mask = run_inference(request.dataset, request.confidence, request.image_raw, logger) + result, mask = run_inference( + request.dataset, request.confidence, request.image_raw, logger + ) masks = [] for part_name in request.parts: - part_mask = result.get_part_mask(mask=tf.identity(mask), part_names=[part_name]).squeeze() + part_mask = result.get_part_mask( + mask=tf.identity(mask), part_names=[part_name] + ).squeeze() if np.max(part_mask) == 0: if logger: @@ -125,11 +130,15 @@ def detect_masks(request: BodyPixMaskDetection_Request, debug_publisher=None, lo return response -def detect_keypoints(request: BodyPixKeypointDetection_Request, debug_publisher=None, logger=None): +def detect_keypoints( + request: BodyPixKeypointDetection_Request, debug_publisher=None, logger=None +): """ Run BodyPix inference for keypoint detection. """ - result, mask = run_inference(request.dataset, request.confidence, request.image_raw, logger) + result, mask = run_inference( + request.dataset, request.confidence, request.image_raw, logger + ) poses = result.get_poses() detected_keypoints: List[BodyPixKeypoint] = [] @@ -191,4 +200,4 @@ def detect_keypoints(request: BodyPixKeypointDetection_Request, debug_publisher= response.keypoints = detected_keypoints response.normalized_keypoints = detected_keypoints_normalized - return response \ No newline at end of file + return response From 11b90b11324cac08fcda526fb76b0d8d46eea095 Mon Sep 17 00:00:00 2001 From: Siyao Date: Tue, 19 Nov 2024 15:02:13 +0000 Subject: [PATCH 08/10] Rename msgs packages to interfaces --- .gitignore | 2 ++ .../{lasr_vision_msgs => lasr_vision_interfaces}/CMakeLists.txt | 2 +- .../msg/BodyPixKeypoint.msg | 0 .../msg/BodyPixKeypointNormalized.msg | 0 .../msg/BodyPixMask.msg | 0 .../{lasr_vision_msgs => lasr_vision_interfaces}/package.xml | 2 +- .../srv/BodyPixKeypointDetection.srv | 0 .../srv/BodyPixMaskDetection.srv | 0 .../srv/DetectWave.srv | 0 9 files changed, 4 insertions(+), 2 deletions(-) rename common/vision/{lasr_vision_msgs => lasr_vision_interfaces}/CMakeLists.txt (97%) rename common/vision/{lasr_vision_msgs => lasr_vision_interfaces}/msg/BodyPixKeypoint.msg (100%) rename common/vision/{lasr_vision_msgs => lasr_vision_interfaces}/msg/BodyPixKeypointNormalized.msg (100%) rename common/vision/{lasr_vision_msgs => lasr_vision_interfaces}/msg/BodyPixMask.msg (100%) rename common/vision/{lasr_vision_msgs => lasr_vision_interfaces}/package.xml (96%) rename common/vision/{lasr_vision_msgs => lasr_vision_interfaces}/srv/BodyPixKeypointDetection.srv (100%) rename common/vision/{lasr_vision_msgs => lasr_vision_interfaces}/srv/BodyPixMaskDetection.srv (100%) rename common/vision/{lasr_vision_msgs => lasr_vision_interfaces}/srv/DetectWave.srv (100%) 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/vision/lasr_vision_msgs/CMakeLists.txt b/common/vision/lasr_vision_interfaces/CMakeLists.txt similarity index 97% rename from common/vision/lasr_vision_msgs/CMakeLists.txt rename to common/vision/lasr_vision_interfaces/CMakeLists.txt index 097ce718d..23cd35a4a 100644 --- a/common/vision/lasr_vision_msgs/CMakeLists.txt +++ b/common/vision/lasr_vision_interfaces/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(lasr_vision_msgs) +project(lasr_vision_interfaces) # Find dependencies find_package(ament_cmake REQUIRED) diff --git a/common/vision/lasr_vision_msgs/msg/BodyPixKeypoint.msg b/common/vision/lasr_vision_interfaces/msg/BodyPixKeypoint.msg similarity index 100% rename from common/vision/lasr_vision_msgs/msg/BodyPixKeypoint.msg rename to common/vision/lasr_vision_interfaces/msg/BodyPixKeypoint.msg diff --git a/common/vision/lasr_vision_msgs/msg/BodyPixKeypointNormalized.msg b/common/vision/lasr_vision_interfaces/msg/BodyPixKeypointNormalized.msg similarity index 100% rename from common/vision/lasr_vision_msgs/msg/BodyPixKeypointNormalized.msg rename to common/vision/lasr_vision_interfaces/msg/BodyPixKeypointNormalized.msg diff --git a/common/vision/lasr_vision_msgs/msg/BodyPixMask.msg b/common/vision/lasr_vision_interfaces/msg/BodyPixMask.msg similarity index 100% rename from common/vision/lasr_vision_msgs/msg/BodyPixMask.msg rename to common/vision/lasr_vision_interfaces/msg/BodyPixMask.msg diff --git a/common/vision/lasr_vision_msgs/package.xml b/common/vision/lasr_vision_interfaces/package.xml similarity index 96% rename from common/vision/lasr_vision_msgs/package.xml rename to common/vision/lasr_vision_interfaces/package.xml index b47cee490..021fbc833 100644 --- a/common/vision/lasr_vision_msgs/package.xml +++ b/common/vision/lasr_vision_interfaces/package.xml @@ -1,6 +1,6 @@ - lasr_vision_msgs + lasr_vision_interfaces 0.0.0 Messages and services for vision processing Paul Makles diff --git a/common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv b/common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv similarity index 100% rename from common/vision/lasr_vision_msgs/srv/BodyPixKeypointDetection.srv rename to common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv diff --git a/common/vision/lasr_vision_msgs/srv/BodyPixMaskDetection.srv b/common/vision/lasr_vision_interfaces/srv/BodyPixMaskDetection.srv similarity index 100% rename from common/vision/lasr_vision_msgs/srv/BodyPixMaskDetection.srv rename to common/vision/lasr_vision_interfaces/srv/BodyPixMaskDetection.srv diff --git a/common/vision/lasr_vision_msgs/srv/DetectWave.srv b/common/vision/lasr_vision_interfaces/srv/DetectWave.srv similarity index 100% rename from common/vision/lasr_vision_msgs/srv/DetectWave.srv rename to common/vision/lasr_vision_interfaces/srv/DetectWave.srv From 932567af1a27e1ee758a119b9fc687bd1b68c662 Mon Sep 17 00:00:00 2001 From: Siyao Date: Tue, 19 Nov 2024 17:51:16 +0000 Subject: [PATCH 09/10] Fix interfaces package naming --- common/vision/lasr_vision_bodypix/CMakeLists.txt | 2 +- common/vision/lasr_vision_bodypix/examples/keypoint_relay.py | 2 +- common/vision/lasr_vision_bodypix/examples/mask_relay.py | 2 +- common/vision/lasr_vision_bodypix/nodes/bodypix_services.py | 2 +- common/vision/lasr_vision_bodypix/package.xml | 2 +- .../lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py | 4 ++-- common/vision/lasr_vision_interfaces/CMakeLists.txt | 2 -- .../lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv | 4 ++-- .../lasr_vision_interfaces/srv/BodyPixMaskDetection.srv | 2 +- common/vision/lasr_vision_interfaces/srv/DetectWave.srv | 2 +- 10 files changed, 11 insertions(+), 13 deletions(-) diff --git a/common/vision/lasr_vision_bodypix/CMakeLists.txt b/common/vision/lasr_vision_bodypix/CMakeLists.txt index 8e7841486..cebc37059 100644 --- a/common/vision/lasr_vision_bodypix/CMakeLists.txt +++ b/common/vision/lasr_vision_bodypix/CMakeLists.txt @@ -7,7 +7,7 @@ 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_msgs REQUIRED) +find_package(lasr_vision_interfaces REQUIRED) # Install Python modules ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME}) diff --git a/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py index 579781a00..68478b13f 100755 --- a/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py +++ b/common/vision/lasr_vision_bodypix/examples/keypoint_relay.py @@ -5,7 +5,7 @@ import threading from sensor_msgs.msg import Image -from lasr_vision_msgs.srv import BodyPixKeypointDetection +from lasr_vision_interfaces.srv import BodyPixKeypointDetection class KeypointRelay(Node): diff --git a/common/vision/lasr_vision_bodypix/examples/mask_relay.py b/common/vision/lasr_vision_bodypix/examples/mask_relay.py index 67fb28a1e..97e07a698 100644 --- a/common/vision/lasr_vision_bodypix/examples/mask_relay.py +++ b/common/vision/lasr_vision_bodypix/examples/mask_relay.py @@ -5,7 +5,7 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import Image -from lasr_vision_msgs.srv import BodyPixMaskDetection +from lasr_vision_interfaces.srv import BodyPixMaskDetection class ImageListener(Node): diff --git a/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py b/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py index 39084a4dc..64f08769a 100644 --- a/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py +++ b/common/vision/lasr_vision_bodypix/nodes/bodypix_services.py @@ -3,7 +3,7 @@ import rclpy from rclpy.node import Node import lasr_vision_bodypix as bodypix -from lasr_vision_msgs.srv import ( +from lasr_vision_interfaces.srv import ( BodyPixMaskDetection, BodyPixKeypointDetection, DetectWave, diff --git a/common/vision/lasr_vision_bodypix/package.xml b/common/vision/lasr_vision_bodypix/package.xml index 8b3104cb2..4ba216815 100644 --- a/common/vision/lasr_vision_bodypix/package.xml +++ b/common/vision/lasr_vision_bodypix/package.xml @@ -20,7 +20,7 @@ ament_python ament_python - lasr_vision_msgs + lasr_vision_interfaces cv2_img 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 index a034c1326..479399046 100755 --- a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -9,8 +9,8 @@ import cv2_img from sensor_msgs.msg import Image as SensorImage from tf_bodypix.api import download_model, load_model, BodyPixModelPaths -from lasr_vision_msgs.msg import BodyPixMask, BodyPixKeypoint, BodyPixKeypointNormalized -from lasr_vision_msgs.srv import ( +from lasr_vision_interfaces.msg import BodyPixMask, BodyPixKeypoint, BodyPixKeypointNormalized +from lasr_vision_interfaces.srv import ( BodyPixMaskDetection, BodyPixKeypointDetection, ) diff --git a/common/vision/lasr_vision_interfaces/CMakeLists.txt b/common/vision/lasr_vision_interfaces/CMakeLists.txt index 7307eeb13..e6dd3854d 100644 --- a/common/vision/lasr_vision_interfaces/CMakeLists.txt +++ b/common/vision/lasr_vision_interfaces/CMakeLists.txt @@ -10,8 +10,6 @@ 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}) -ament_export_dependencies(rosidl_default_runtime) ################################################ ## Declare ROS messages, services, and actions # diff --git a/common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv b/common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv index 2587bd0ed..3c491377c 100644 --- a/common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv +++ b/common/vision/lasr_vision_interfaces/srv/BodyPixKeypointDetection.srv @@ -13,7 +13,7 @@ bool keep_out_of_bounds --- # Response # keypoints -lasr_vision_msgs/BodyPixKeypoint[] keypoints +lasr_vision_interfaces/BodyPixKeypoint[] keypoints # keypoints -lasr_vision_msgs/BodyPixKeypointNormalized[] normalized_keypoints \ No newline at end of file +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 index 19accfaea..3c1a2fcfe 100644 --- a/common/vision/lasr_vision_interfaces/srv/BodyPixMaskDetection.srv +++ b/common/vision/lasr_vision_interfaces/srv/BodyPixMaskDetection.srv @@ -13,4 +13,4 @@ float32 confidence string[] parts --- # Generated masks -lasr_vision_msgs/BodyPixMask[] masks \ No newline at end of file +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 index b200ba7a6..817c903b9 100644 --- a/common/vision/lasr_vision_interfaces/srv/DetectWave.srv +++ b/common/vision/lasr_vision_interfaces/srv/DetectWave.srv @@ -9,7 +9,7 @@ float32 confidence --- # keypoints -lasr_vision_msgs/BodyPixKeypoint[] keypoints +lasr_vision_interfaces/BodyPixKeypoint[] keypoints # waving bool wave_detected From e815e9aa08752d4865cd4657b322d569ba6fe046 Mon Sep 17 00:00:00 2001 From: Ma'ayan Armony Date: Tue, 19 Nov 2024 18:40:24 +0000 Subject: [PATCH 10/10] refactor: black format bodypix.py --- .../src/lasr_vision_bodypix/bodypix.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) 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 index 479399046..f31339725 100755 --- a/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py +++ b/common/vision/lasr_vision_bodypix/src/lasr_vision_bodypix/bodypix.py @@ -1,26 +1,29 @@ from __future__ import annotations + +import re from typing import List -import rclpy -from rclpy.node import Node + import cv2 +import cv2_img import numpy as np -import re import tensorflow as tf -import cv2_img -from sensor_msgs.msg import Image as SensorImage -from tf_bodypix.api import download_model, load_model, BodyPixModelPaths -from lasr_vision_interfaces.msg import BodyPixMask, BodyPixKeypoint, BodyPixKeypointNormalized +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 = {}