diff --git a/.github/workflows/catkin-build.yml b/.github/workflows/catkin-build.yml deleted file mode 100644 index dab66e1..0000000 --- a/.github/workflows/catkin-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: catkin build CI - -on: push - -jobs: - build: - # Although the build is run inside a 20.04 container, we need to explicitly - # specify 20.04 here so actions/setup-python installs the right Python 3.10 - # binary. - runs-on: ubuntu-20.04 - container: - image: osrf/ros:noetic-desktop - - steps: - - uses: actions/checkout@v4 - with: - path: "src/${{ github.repository_id }}" - - name: Inspect directory contents - run: apt-get update && apt-get install --yes tree && pwd && tree - - uses: actions/setup-python@v5 - with: - python-version: | - 3.10 - 3.8 - - name: Install build dependencies - run: python3.8 -m pip install catkin-pkg catkin-tools empy - - name: Run catkin build - run: . /opt/ros/noetic/setup.sh && catkin build --no-status - - name: Run yolo checks - run: | - .venv/bin/yolo check | tee check.log - ! grep -q "❌" check.log - working-directory: "${{ github.workspace }}/src/${{ github.repository_id }}" diff --git a/.github/workflows/colcon-build.yml b/.github/workflows/colcon-build.yml new file mode 100644 index 0000000..77f5ccd --- /dev/null +++ b/.github/workflows/colcon-build.yml @@ -0,0 +1,25 @@ +name: colcon build + +on: push + +jobs: + build: + runs-on: ubuntu-22.04 + container: + image: osrf/ros:humble-desktop + + steps: + - uses: actions/checkout@v4 + with: + path: "src/${{ github.repository_id }}" + - name: Inspect directory contents + run: apt-get update && apt-get install --yes tree && pwd && tree + - name: Install Poetry + uses: snok/install-poetry@v1 + - name: Run colcon build + run: . /opt/ros/humble/setup.sh && colcon build --symlink-install + - name: Run yolo checks + run: | + .venv/bin/yolo check | tee check.log + ! grep -q "❌" check.log + working-directory: "${{ github.workspace }}/src/${{ github.repository_id }}" diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md new file mode 100644 index 0000000..8cc69a2 --- /dev/null +++ b/ARCHITECTURE.md @@ -0,0 +1,41 @@ +# Package Organization + +This repository is a ROS 2 wrapper around the Ultralytics YOLOv8 library. As is +standard with learning-based Python packages, a virtual environment is used to +avoid Python dependency conflicts with other packages on the system. Virtual +environments appear to be incompatible with the standard ROS 2 Python package +architecture, thus `ament_cmake` is used instead of `ament_python`. + +## Virtual Environment Specification + +`pyproject.toml`, `poetry.lock`, and `poetry.toml` are used by the Poetry +dependency manager to set up a virtual environment. `pyproject.toml` contains +the editable list of Python dependencies used by the package. In general, +`poetry.lock` and `poetry.toml` do not need to be manually edited. + +## `yolov8_ros/` + +This folder contains this package's ROS 2 nodes. ROS 2 nodes are implemented as +modules with the suffix `_node` and must contain a `main()` function that can +accept zero arguments. Nodes are not meant to be imported by other Python +modules. `parameters.py` contains the sets of ROS 2 parameters declared by each +node and their accompanying documentation. + +## ROS 2 Files + +### Package and Build System Files + +`package.xml` is a required ROS 2 package specification file. + +`CMakeLists.txt` defines where files and folders in this repository are copied +or linked during the ROS 2 workspace build and install process. + +### Runtime Files + +`launch/` contains ROS 2 launch files. + +`scripts/python_entrypoint.py` is an internal script used to run Python modules +within the virtual environment from launch files. See additional documentation +at the top of this file. + +`rviz2/` contains saved configuration files for the `rviz2` tool. diff --git a/CMakeLists.txt b/CMakeLists.txt index 957bbd7..d5750d6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,208 +1,58 @@ cmake_minimum_required(VERSION 3.16) -project(yolov8-ros) +project(yolov8_ros) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +# Find CMake dependencies +find_package(ament_cmake REQUIRED) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +# Build Python virtual environment add_custom_target( poetry_virtualenv ALL - COMMAND ${CMAKE_SOURCE_DIR}/scripts/cmake_virtualenv_setup.py + COMMAND poetry install --no-root --no-ansi --no-interaction WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - COMMENT "Building ${CMAKE_PROJECT_NAME} virtualenv... may take some time" + COMMENT "Building ${CMAKE_PROJECT_NAME} virtualenv... this may take some time" ) -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES yolov8-ros -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib +# Install launch files. +# https://docs.ros.org/en/humble/Tutorials/Intermediate/Launch/Launch-system.html +install( + DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} +# ROS 1 provided a mechanism in XML launch files to find the corresponding +# source directory of a package. ROS 2 removed this mechanism but instead allows +# users to reference the "install" path of a package within a workspace. Thus, +# the virtual environment in this source directory must be discoverable inside +# the workspace's install directory. We manually symlink the virtual environment +# directory for two reasons: +# 1) "colcon build" would otherwise copy gigabytes worth of libraries +# 2) "colcon build --symlink-install" creates a symlink for each file in the +# directory, rather than a single symlink for the directory itself. +# +# ROS 2 looks for executables in ${CMAKE_INSTALL_PREFIX}/lib. +file( + # "file" commands are executed at build time. The install directory won't + # exist yet during a fresh build, which will cause the next CREATE_LINK to + # fail. + MAKE_DIRECTORY + ${CMAKE_INSTALL_PREFIX} +) +file( + CREATE_LINK + ${CMAKE_SOURCE_DIR}/.venv + ${CMAKE_INSTALL_PREFIX}/.venv + SYMBOLIC +) +install( + DIRECTORY + yolov8_ros + DESTINATION lib/${PROJECT_NAME} +) +install( + PROGRAMS + scripts/python_entrypoint.py + DESTINATION lib/${PROJECT_NAME} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/yolov8-ros.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/yolov8-ros_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_yolov8-ros.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/README.md b/README.md index b210f7a..a112d6d 100644 --- a/README.md +++ b/README.md @@ -1,54 +1,45 @@ # YOLOv8 ROS Integration -This repository contains a [Poetry][poetry-home] virtual environment -to use Ultralytics YOLOv8 with ROS 1 Noetic. - -[poetry-home]: https://python-poetry.org/ +This repository contains Python modules and a dependency specification to use +Ultralytics YOLOv8 with ROS 2. Additional documentation about how this +repository is organized can be found in [`ARCHITECTURE.md`](/ARCHITECTURE.md). ## Getting Started -### Install Python 3.10 (Ubuntu 20.04 only) - -The [Deadsnakes Ubuntu PPA][deadsnakes] provides newer Python versions that are -not present in the official package repositories. These commands will set up the -PPA and install the required Python 3.10 packages: +### Requirements -```shell -apt update && apt install software-properties-common -add-apt-repository ppa:deadsnakes/ppa -apt update -apt install python3.10 python3.10-distutils python3.10-venv -``` +- ROS 2 Humble +- Python 3.10 + - Included with Ubuntu 22.04 + - For other supported Ubuntu LTS distributions, the [Deadsnakes Ubuntu + PPA][deadsnakes] provides the required `python3.10 python3.10-distutils + python3.10-venv` packages. +- The [Poetry][poetry-docs] dependency manager +- A CUDA 11.8.x runtime environment -[deadsnakes]: +[deadsnakes]: https://launchpad.net/~deadsnakes/+archive/ubuntu/ppa +[poetry-docs]: https://python-poetry.org/docs/ ### Install dependencies to a virtual environment -This repository is set up to build using `catkin` and/or `cmake`. The build -process installs the Poetry dependency manager if it is not already present on -the system and installs Python dependencies to `.venv`. +This repository uses Poetry to install dependencies to a virtual environment. +The virtual environment is located at the top level of this repository +regardless of the next command used to initialize it. -If this package is within a `catkin` workspace, run `catkin build` within the -workspace. Otherwise, you can run `cmake` directly from the top-level directory -of this repository: +If you are developing outside of a ROS 2 workspace, run `poetry install +--no-root` within this repository. -```shell -cmake -B build && cmake --build build -``` +This repository is set up to build with `colcon` using the `ament_cmake` +backend. If this repository is inside a ROS 2 workspace, run `colcon build` from +the top level of the ROS 2 workspace. ### (Optional) Run YOLO Checks -After the virtual environment is initialized you can activate it by running +After the virtual environment is initialized, you can run YOLO status checks +using `poetry run yolo check`. You should see output similar to below: ```shell -poetry shell -``` - -Then, run YOLO status checks using `yolo check`. You should see output similar -to below: - -```shell -$ yolo check +$ poetry run yolo check Ultralytics YOLOv8.0.200 🚀 Python-3.10.13 torch-2.1.0+cu118 CUDA:0 (NVIDIA RTX A2000 12GB, 12017MiB) Setup complete ✅ (24 CPUs, 31.0 GB RAM, 53.8/198.7 GB disk) @@ -77,21 +68,27 @@ py-cpuinfo ✅ 9.0.0 thop ✅ 0.1.1-2209072238>=0.1.1 ``` -## Running ROS Nodes +### Running ROS Nodes +All ROS nodes are Python modules that must run within the virtual environment. There are multiple ways to run ROS nodes depending on context. -### Launch Files +#### Launch Files + +If this repository is inside a ROS 2 workspace, use the following commands at +the top level of the workspace. -If you are in a catkin workspace, source `devel/setup.$(basename $SHELL)` in the -top level of the workspace and then run `roslaunch yolov8-ros $LAUNCH_FILE_NAME`, -where `$LAUNCH_FILE_NAME` is the name of one of the files in -[`launch/`](/launch/). +```shell +source install/setup.$(basename $SHELL) + +# Replace LAUNCH_FILE_NAME with one of the files in launch/ +ros2 launch yolov8_ros LAUNCH_FILE_NAME +``` -### Direct Invocation +#### Direct Invocation -Using the virtual environment directly is sometimes easier for prototyping and -testing. From the top level of this repository, +Running nodes directly with Python may be useful for development and testing. +Use the following commands from the top level of this repository. ```shell # Activate the virtual environment diff --git a/launch/right_wrist_node.launch b/launch/right_wrist_node.launch deleted file mode 100644 index 39bf4f1..0000000 --- a/launch/right_wrist_node.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - diff --git a/launch/right_wrist_node_launch.xml b/launch/right_wrist_node_launch.xml new file mode 100644 index 0000000..c3ef802 --- /dev/null +++ b/launch/right_wrist_node_launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 54735d6..78548a2 100644 --- a/package.xml +++ b/package.xml @@ -1,17 +1,21 @@ - - yolov8-ros + + + yolov8_ros 0.0.0 ROS integration for Ultralytics YOLOv8 - Elvin Yang - - AGPL-3.0 + rclpy + builtin_interfaces + cv_bridge + geometry_msgs + sensor_msgs + visualization_msgs - catkin - - rospy + + ament_cmake + diff --git a/pyproject.toml b/pyproject.toml index b2fe8ce..8afab5d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [tool.poetry] -name = "yolov8-ros" -version = "0.1.0" +name = "yolov8_ros" +version = "0.0.0" description = "" authors = ["Elvin Yang "] readme = "README.md" diff --git a/rviz/proto.rviz b/rviz/proto.rviz deleted file mode 100644 index 58e6496..0000000 --- a/rviz/proto.rviz +++ /dev/null @@ -1,145 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 520 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: Image -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /yolov8/pose - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /yolov8/seg - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 10 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 - Target Frame: - Yaw: 0.785398006439209 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1004 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002ba0000034efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000034e000000e60100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb000000100044006900730070006c0061007900730100000000000001560000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002a40000034efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d0000034e000000c10100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb0000000a0056006900650077007301000003a10000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000058b0000003efc0100000002fb0000000800540069006d006501000000000000058b000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000000210000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1419 - X: 1094 - Y: 131 diff --git a/rviz/wrist_tracking.rviz b/rviz2/yolov8_pose_tracking.rviz similarity index 56% rename from rviz/wrist_tracking.rviz rename to rviz2/yolov8_pose_tracking.rviz index 7bfa79d..55ab77a 100644 --- a/rviz/wrist_tracking.rviz +++ b/rviz2/yolov8_pose_tracking.rviz @@ -1,43 +1,53 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - - /TF1 - - /PointCloud21/Status1 - Splitter Ratio: 0.5 - Tree Height: 993 - - Class: rviz/Selection + - /PointStamped1 + - /Marker1 + Splitter Ratio: 0.3393939435482025 + Tree Height: 662 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 182 + - Class: rviz_common/Transformation + Name: Transformation + - Class: rviz_common/Time + Experimental: false Name: Time SyncMode: 0 - SyncSource: Image -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -53,11 +63,9 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 9999 + Frame Timeout: 15 Frames: All Enabled: false base_link: @@ -66,8 +74,6 @@ Visualization Manager: Value: false camera_accel_optical_frame: Value: false - camera_aligned_depth_to_color_frame: - Value: false camera_bottom_screw_frame: Value: false camera_color_frame: @@ -82,8 +88,6 @@ Visualization Manager: Value: false camera_gyro_optical_frame: Value: false - camera_imu_optical_frame: - Value: false camera_infra1_frame: Value: false camera_infra1_optical_frame: @@ -96,6 +100,8 @@ Visualization Manager: Value: false caster_link: Value: false + imu_mobile_base: + Value: false laser: Value: false link_arm_l0: @@ -120,6 +126,8 @@ Visualization Manager: Value: false link_grasp_center: Value: false + link_gripper: + Value: false link_gripper_finger_left: Value: false link_gripper_finger_right: @@ -142,28 +150,21 @@ Visualization Manager: Value: false link_right_wheel: Value: false - link_straight_gripper: - Value: false - link_wrist_pitch: - Value: false - link_wrist_roll: - Value: false link_wrist_yaw: Value: false - link_wrist_yaw_bottom: - Value: false respeaker_base: Value: false - Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true - Show Names: false + Show Names: true Tree: base_link: caster_link: {} + imu_mobile_base: + {} laser: {} link_aruco_left_base: @@ -181,19 +182,15 @@ Visualization Manager: camera_accel_frame: camera_accel_optical_frame: {} - camera_aligned_depth_to_color_frame: + camera_color_frame: camera_color_optical_frame: {} - camera_color_frame: - {} camera_depth_frame: camera_depth_optical_frame: {} camera_gyro_frame: camera_gyro_optical_frame: {} - camera_imu_optical_frame: - {} camera_infra1_frame: camera_infra1_optical_frame: {} @@ -211,18 +208,15 @@ Visualization Manager: link_aruco_top_wrist: {} link_wrist_yaw: - link_wrist_yaw_bottom: - link_wrist_pitch: - link_wrist_roll: - link_straight_gripper: - link_grasp_center: - {} - link_gripper_finger_left: - link_gripper_fingertip_left: - {} - link_gripper_finger_right: - link_gripper_fingertip_right: - {} + link_gripper: + link_grasp_center: + {} + link_gripper_finger_left: + link_gripper_fingertip_left: + {} + link_gripper_finger_right: + link_gripper_fingertip_right: + {} link_aruco_shoulder: {} respeaker_base: @@ -231,38 +225,6 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /camera/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /yolov8/wrist/vis - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /yolov8/wrist/marker - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -271,91 +233,173 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /camera/depth/color/points - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/color/image_raw/compressed + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /yolov8/pose/vis/compressed + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: false + History Length: 1 + Name: PointStamped + Radius: 0.20000000298023224 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /yolov8/pose/right_wrist + Value: false + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /yolov8/markers/right_wrist + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: base_link Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 5.2576003074646 + Class: rviz_default_plugins/Orbit + Distance: 6.460544586181641 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 1.7059940099716187 - Y: 0.8883990049362183 - Z: 0.9190242886543274 + X: 1.4275530576705933 + Y: -0.06080014258623123 + Z: 1.2394773960113525 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.09539756178855896 + Pitch: 0.734796404838562 Target Frame: - Yaw: 3.110419273376465 + Value: Orbit (rviz) + Yaw: 3.2053909301757812 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1284 + Height: 1245 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000046afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000046a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002b50000046afc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000000a0000000a000fffffffb0000000a0049006d00610067006501000000e10000018e0000001600fffffffb0000000a0049006d0061006700650100000275000002300000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008f10000003efc0100000002fb0000000800540069006d00650100000000000008f10000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000004da0000046a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c600000446fc020000000cfb0000001200530065006c0065006300740069006f006e000000003b000000870000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000031f000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006900730070006c006100790073000000034700000137000000c700fffffffb0000001c005400720061006e00730066006f0072006d006100740069006f006e00000003b8000000c60000007900fffffffb0000000800540069006d00650000000447000000370000003700fffffffb0000000a0049006d0061006700650100000360000001210000002800ffffff00000001000001db00000446fc0200000004fb0000000a00560069006500770073010000003b00000122000000a000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0049006d00610067006501000001630000031e0000002800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000088b0000003bfc0100000001fc000000000000088b0000025300fffffffa000000010200000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000800540069006d006501000004840000003e0000003700ffffff000004de0000044600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false + Transformation: + collapsed: false Views: collapsed: false - Width: 2289 - X: 194 - Y: 17 + Width: 2187 + X: 2741 + Y: 100 diff --git a/scripts/cmake_virtualenv_setup.py b/scripts/cmake_virtualenv_setup.py deleted file mode 100755 index 7517d27..0000000 --- a/scripts/cmake_virtualenv_setup.py +++ /dev/null @@ -1,68 +0,0 @@ -#! /usr/bin/env python3 - -""" -This repository uses Poetry / PEP 517 to specify and install dependencies, which -is not natively supported by catkin. Run this script in CMakeLists.txt to create -a virtual environment and install Python dependencies. -""" - -import os -import shutil -import subprocess -import sys -from pathlib import Path - - -def assert_pip_installed() -> None: - try: - import pip # noqa: F401 - except ModuleNotFoundError: - print("[FATAL] pip is not installed on the system.", file=sys.stderr) - sys.exit(1) - - -def ensure_poetry() -> None: - assert_pip_installed() - - # Manually append ~/.local/bin to PATH within this script's environment - # since we may be running in an old shell unaffected by `pipx ensurepath`. - os.environ["PATH"] = f"{os.environ['PATH']}:{Path.home()}/.local/bin".strip(":") - - if shutil.which("poetry") is not None: - return - - # This is one set of the recommended installation instructions for poetry on - # Ubuntu 20.04 as of Dec 2023. - try: - subprocess.run( - ["python3", "-m", "pip", "install", "--user", "pipx"], check=True - ) - subprocess.run(["python3", "-m", "pipx", "ensurepath"], check=True) - subprocess.run(["python3", "-m", "pipx", "install", "poetry"], check=True) - except subprocess.CalledProcessError as err: - print(f"[FATAL]: Failed to execute '{err.cmd}'") - sys.exit(err.returncode) - - if shutil.which("poetry") is None: - print( - "[FATAL] Failed to find poetry executable after installation.", - file=sys.stderr, - ) - sys.exit(1) - - -def poetry_create_venv() -> None: - ensure_poetry() - - try: - subprocess.run( - ["poetry", "install", "--no-root", "--no-ansi", "--no-interaction"], - check=True, - ) - except subprocess.CalledProcessError as err: - print("[FATAL]: Failed to create virtualenv.", file=sys.stderr) - sys.exit(err.returncode) - - -if __name__ == "__main__": - poetry_create_venv() diff --git a/scripts/python_entrypoint.py b/scripts/python_entrypoint.py index 38cf5ae..3b441cc 100755 --- a/scripts/python_entrypoint.py +++ b/scripts/python_entrypoint.py @@ -1,27 +1,73 @@ #! /usr/bin/env python3 -# Assuming the following project structure: -# -# project_dir -# ├── scripts -# │ ├── __file__ -# │ └── ... -# ├── package1 -# │ ├── __init__.py -# │ └── module_a.py -# ├── package2 -# │ ├── __init__.py -# │ └── module_b.py -# └── ... -# -# The objectives of this script are as follows: -# 1. Create Python module objects for module_a and module_b dynamically, -# i.e., without knowing the names "package1", "package2", "module_a", or -# "module_b". -# 2. Read the name of a node spawned by an invocation of this script from a -# launch file and run the main() method of the respective module, again -# without knowing the name of the package that the (uniquely named) module -# belongs to. +""" +This script is intended to be executed from a ROS 2 XML launch file to run a +Python module as a node within a virtual environment. Virtual environments are +frequently used to avoid versioning conflicts of Python package dependencies, +particularly for learning-based projects. + +Recall that Python modules are run using the -m flag, e.g., + python3 -m pypackage.module +This is incompatible with ROS 2 XML launch files since the "exec" property of +the tag expects a single executable file. This script functions as an +executable file for the "exec" property. + +Importantly, this script must be executed inside the virtual environment, e.g., + .venv/bin/python3 python_entrypoint.py +Although this script uses /usr/bin/env in its shebang, PATH should not be relied +upon to provide the correct python3 executable since attempting to find and +source the proper ".venv/bin/activate" from an XML launch file would be +extremely messy. Instead, assuming that this script ("python_entrypoint.py") is +located in the following directory structure after building a ROS 2 workspace: + + ${CMAKE_INSTALL_PATH}/ (typically: ws/install/project_name) + ├── .venv/ + ├── lib/ + │ └── project_name/ + │ ├── python_entrypoint.py + │ ├── pypackage1/ + │ │ ├── __init__.py + │ │ └── module_a.py + │ └── pypackage2/ + │ ├── __init__.py + │ └── module_b.py + └── share/ + └── project_name/ + └── launch/ + └── launch_file.launch + +this script can be executed from a launch file using the "launch-prefix" +property of the tag. In the following example, a node is created by +running "module_a" of Python package "pypackage1" within the ROS 2 package +"project_name". + + + +The directory structure is not arbitrary: ROS 2 expects launch files to be +located in "${CMAKE_INSTALL_PATH}/share/" and executables to be located in +"${CMAKE_INSTALL_PATH}/lib/". In the ROS 2 XML launch file specification, +$(find-pkg-prefix project_name) expands to ${CMAKE_INSTALL_PATH}. + +The objectives of this script are as follows: +1. Create Python module objects for module_a and module_b dynamically, + i.e., without knowing the names "pypackage1", "pypackage2", "module_a", or + "module_b". +2. Read the name of a node from the command invoked by a launch file and run the + main() function of the corresponding Python module, again without knowing the + name of the package that the module belongs to. + +Assumptions and Limitations: +1. All Python packages and modules must be uniquely named. +2. Python modules functioning as nodes need to have a main() function. +3. This script needs to run each module to determine whether the + module contains a main() function. +""" import argparse import importlib.util @@ -32,12 +78,12 @@ def find_valid_modules() -> dict[str, ModuleType]: - # Valid modules are modules with a main() function. + # Valid modules are defined as modules with a main() function. valid_modules: dict[str, ModuleType] = {} - project_dir = Path(__file__).parent.parent + project_dir = Path(__file__).parent - project_packages_info = [ + project_packages_info: list[pkgutil.ModuleInfo] = [ module for module in pkgutil.iter_modules(path=[str(project_dir)]) if module.ispkg @@ -45,14 +91,15 @@ def find_valid_modules() -> dict[str, ModuleType]: # Find valid modules within this project's packages. for package_info in project_packages_info: - # Import the package since submodules may reference the package by - # themselves importing from it. This process is somewhat convoluted and + # Importing the package is necessary since submodules may reference the + # package by importing from it, which requires the package to be loaded + # and present in sys.modules. This process is somewhat convoluted and # filled with landmines: # https://docs.python.org/3.10/library/importlib.html#checking-if-a-module-can-be-imported package_spec = package_info.module_finder.find_spec(package_info.name) # type: ignore if package_spec is None or package_spec.loader is None: continue - package = importlib.util.module_from_spec(package_spec) + package: ModuleType = importlib.util.module_from_spec(package_spec) sys.modules[package_info.name] = package package_spec.loader.exec_module(package) @@ -79,11 +126,11 @@ def find_valid_modules() -> dict[str, ModuleType]: if __name__ == "__main__": valid_modules = find_valid_modules() - # Read the module name from command-line arguments. roslaunch appends the - # command-line argument "__name:=name_property_in_launch_file" + # Read the module name from command-line arguments. The ROS 2 launch system + # appends the command-line argument "__node:=name_of_node". argparser = argparse.ArgumentParser(prefix_chars="_") argparser.add_argument( - "__name:", + "__node:", type=str, choices=valid_modules, required=True, @@ -91,6 +138,6 @@ def find_valid_modules() -> dict[str, ModuleType]: ) namespace, _ = argparser.parse_known_args() - module_name: str = namespace.__getattribute__("name:") + module_name: str = namespace.__getattribute__("node:") valid_modules[module_name].main() diff --git a/yolov8_ros/parameters.py b/yolov8_ros/parameters.py new file mode 100644 index 0000000..7acf2f1 --- /dev/null +++ b/yolov8_ros/parameters.py @@ -0,0 +1,52 @@ +from dataclasses import dataclass, field + +from rclpy.node import Node + + +@dataclass +class YoloSkeletonRightWristNodeParams: + yolov8_model_name: str = field(default="yolov8x-pose.pt") + """ + Basename of the YOLOv8 model to use. + + A list exists here: https://docs.ultralytics.com/tasks/pose/#models + """ + + rgb_sub_topic: str = field(default="/camera/color/image_raw/compressed") + """sensor_msgs/CompressedImage RGB topic to subscribe to.""" + + depth_sub_topic: str = field(default="/camera/aligned_depth_to_color/image_raw") + """ + sensor_msgs/Image aligned depth topic to subscribe to. + + Images published to this topic must share the same intrinsic matrix as the + RGB image topic. + + The uncompressed topic is used as there seems to be some difficulty + subscribing or decoding the compressed depth topic. + """ + + camera_info_sub_topic: str = field(default="/camera/color/camera_info") + """sensor_msgs/CameraInfo topic to subscribe to.""" + + stretch_robot_rotate_image_90deg: bool = field(default=True) + """ + Whether to rotate the input images 90 degrees clockwise before inference. + + This parameter exists because the Stretch robot's camera is mounted + sideways, and YOLOv8 does not appear to perform well on rotated images. + + TODO: This could be implemented better, but it works for our use case at the + time of writing. + """ + + @staticmethod + def read_from_ros(node: Node) -> "YoloSkeletonRightWristNodeParams": + params = YoloSkeletonRightWristNodeParams() + + for param_name, default_value in vars(params).items(): + ros_value = node.declare_parameter(param_name, default_value).value + assert type(ros_value) == type(default_value) + setattr(params, param_name, ros_value) + + return params diff --git a/yolov8_ros/right_wrist_node.py b/yolov8_ros/right_wrist_node.py index c46d369..48e0c65 100755 --- a/yolov8_ros/right_wrist_node.py +++ b/yolov8_ros/right_wrist_node.py @@ -1,3 +1,5 @@ +import sys +import threading from typing import Optional import cv2 @@ -6,110 +8,135 @@ from ultralytics import YOLO from ultralytics.engine.results import Results -import rospy +import rclpy +from builtin_interfaces.msg import Duration, Time from cv_bridge import CvBridge from geometry_msgs.msg import PointStamped +from rclpy.node import Node from sensor_msgs.msg import CameraInfo, CompressedImage, Image from visualization_msgs.msg import Marker from yolov8_ros import logger, get_model_download_dir +from yolov8_ros.parameters import YoloSkeletonRightWristNodeParams -class YoloSkeletonRightWristNode: +class YoloSkeletonRightWristNode(Node): + """ + This node estimates the 3D position of the right wrist of a human from RGBD + images. + + The assumption is made that the human is facing the robot and the right + wrist of the human is not occluded. + """ + def __init__(self) -> None: - self._model = YOLO(get_model_download_dir() / "yolov8x-pose.pt") + super().__init__("yolov8_right_wrist") + + self._params = YoloSkeletonRightWristNodeParams.read_from_ros(self) + self._model = YOLO(get_model_download_dir() / self._params.yolov8_model_name) ##################### # ROS-related setup # ##################### - # Subscribe to the uncompressed image since there seems to be some - # difficulty subscribing to the /compressedDepth topic. - depth_topic: str = rospy.get_param( - "depth_topic", default="/camera/aligned_depth_to_color/image_raw" + self._wrist_position_pub = self.create_publisher( + PointStamped, "/yolov8/pose/right_wrist", 1 ) - rgb_topic: str = rospy.get_param( - "rgb_topic", default="/camera/color/image_raw/compressed" + self._debug_marker_pub = self.create_publisher( + Marker, "/yolov8/markers/right_wrist", 1 ) - camera_info_topic: str = rospy.get_param( - "camera_info_topic", default="/camera/aligned_depth_to_color/camera_info" - ) - self._stretch_robot_rotate_image_90deg: bool = rospy.get_param( - "stretch_robot_rotate_image_90deg", default=True + self._debug_img_pub = self.create_publisher( + CompressedImage, "/yolov8/pose/vis/compressed", 1 ) - self._wrist_position_pub = rospy.Publisher( - "/yolov8/pose/right_wrist", PointStamped, queue_size=1 - ) - self._debug_marker_pub = rospy.Publisher( - "/yolov8/markers/right_wrist", Marker, queue_size=1 - ) - self._debug_img_pub = rospy.Publisher( - "/yolov8/pose/vis/compressed", CompressedImage, queue_size=1 - ) - - # Perform a blocking read of a single CameraInfo message instead of - # creating a persistent subscriber. - logger.info("Waiting 5s for CameraInfo...") - camera_info_msg: CameraInfo = rospy.wait_for_message( - camera_info_topic, CameraInfo, timeout=5 - ) - self._intrinsic_matrix = np.array(camera_info_msg.K).reshape((3, 3)) + # Data used by subscribers and callbacks + self._msg_lock = threading.Lock() + self._intrinsic_matrix: Optional[np.ndarray] = None self._rgb_msg: Optional[CompressedImage] = None self._depth_msg: Optional[Image] = None self._cv_bridge = CvBridge() # TODO(elvout): how to we make sure these messages are synchronized? # Maybe track timestamps and only use timestamps with both messages - self._rgb_sub = rospy.Subscriber( - rgb_topic, CompressedImage, self._rgb_callback, queue_size=1 + self._camera_info_sub = self.create_subscription( + CameraInfo, + self._params.camera_info_sub_topic, + self._camera_info_callback, + 1, ) - self._depth_sub = rospy.Subscriber( - depth_topic, Image, self._depth_callback, queue_size=1 + self._rgb_sub = self.create_subscription( + CompressedImage, self._params.rgb_sub_topic, self._rgb_callback, 1 ) + self._depth_sub = self.create_subscription( + Image, self._params.depth_sub_topic, self._depth_callback, 1 + ) + + # TODO(elvout): switch to ros service + self._timer = self.create_timer(1 / 15, self.run_inference_and_publish_pose) + + def _camera_info_callback(self, msg: CameraInfo) -> None: + with self._msg_lock: + self._intrinsic_matrix = np.array(msg.k).reshape((3, 3)) + + # Assuming that the intrinsic matrix does not change, we only need to + # receive one CameraInfo message. In ROS 1, we used + # rospy.wait_for_message to avoid keeping track of a Subscriber object. + # ROS 2 Humble does not have this feature (although it has since been + # introduced https://github.com/ros2/rclpy/pull/960), so we manually + # destroy the Subscription after receiving the CameraInfo message. + self.destroy_subscription(self._camera_info_sub) def _rgb_callback(self, msg: CompressedImage) -> None: - self._rgb_msg = msg + with self._msg_lock: + self._rgb_msg = msg def _depth_callback(self, msg: Image) -> None: - self._depth_msg = msg + with self._msg_lock: + self._depth_msg = msg def _point_to_marker(self, point: PointStamped) -> Marker: marker = Marker() marker.header.frame_id = point.header.frame_id - marker.header.stamp = rospy.Time(0) + marker.header.stamp = Time() - marker.type = Marker.ARROW - marker.scale.x = 0.25 - marker.scale.y = 0.05 - marker.scale.z = 0.05 + marker.lifetime = Duration(sec=0, nanosec=int(1e9 / 15) * 2) + marker.type = Marker.SPHERE + marker.scale.x = 0.15 + marker.scale.y = 0.15 + marker.scale.z = 0.15 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 1.0 - marker.pose.position.x = point.point.x - marker.scale.x + marker.pose.position.x = point.point.x marker.pose.position.y = point.point.y marker.pose.position.z = point.point.z - marker.pose.orientation.x = 0.7071068 - marker.pose.orientation.y = 0.0 - marker.pose.orientation.z = 0.0 - marker.pose.orientation.w = 0.7071068 - return marker def run_inference_and_publish_pose(self) -> None: - if self._rgb_msg is None or self._depth_msg is None: - return - - rgb_msg_header = self._rgb_msg.header - - bgr_image = self._cv_bridge.compressed_imgmsg_to_cv2(self._rgb_msg) - depth_image = ( - self._cv_bridge.imgmsg_to_cv2(self._depth_msg).astype(np.float32) / 1000.0 - ) - - if self._stretch_robot_rotate_image_90deg: + with self._msg_lock: + if ( + self._intrinsic_matrix is None + or self._rgb_msg is None + or self._depth_msg is None + ): + return + + rgb_msg_header = self._rgb_msg.header + + bgr_image = self._cv_bridge.compressed_imgmsg_to_cv2(self._rgb_msg) + depth_image = ( + self._cv_bridge.imgmsg_to_cv2(self._depth_msg).astype(np.float32) + / 1000.0 + ) + + # Discard existing messages so that inference is only run again once + # we have new data. + self._rgb_msg = None + self._depth_msg = None + + if self._params.stretch_robot_rotate_image_90deg: bgr_image = cv2.rotate(bgr_image, cv2.ROTATE_90_CLOCKWISE) results: list[Results] = self._model.predict(bgr_image, verbose=False) @@ -148,7 +175,7 @@ def run_inference_and_publish_pose(self) -> None: # Find the pixel coordinate in the un-rotated image so we can query the # depth image. - if self._stretch_robot_rotate_image_90deg: + if self._params.stretch_robot_rotate_image_90deg: y = bgr_image.shape[1] - right_wrist_pixel_xy[0] x = right_wrist_pixel_xy[1] else: @@ -160,15 +187,15 @@ def run_inference_and_publish_pose(self) -> None: if depth == 0: return - P_camera_rightwrist = ( + P_camera_rightwrist: np.ndarray = ( depth * np.linalg.inv(self._intrinsic_matrix) @ np.array([[x], [y], [1]]) ) point_msg = PointStamped() point_msg.header = rgb_msg_header - point_msg.point.x = P_camera_rightwrist[0] - point_msg.point.y = P_camera_rightwrist[1] - point_msg.point.z = P_camera_rightwrist[2] + point_msg.point.x = P_camera_rightwrist[0].item() + point_msg.point.y = P_camera_rightwrist[1].item() + point_msg.point.z = P_camera_rightwrist[2].item() self._wrist_position_pub.publish(point_msg) marker_msg = self._point_to_marker(point_msg) @@ -182,15 +209,12 @@ def run_inference_and_publish_pose(self) -> None: def main() -> None: - rospy.init_node("yolov8_right_wrist") - node = YoloSkeletonRightWristNode() + rclpy.init(args=sys.argv) + node = YoloSkeletonRightWristNode() logger.success("node initialized") - # TODO(elvout): switch to ros service - while not rospy.is_shutdown(): - node.run_inference_and_publish_pose() - rospy.sleep(0.05) + rclpy.spin(node) if __name__ == "__main__":