diff --git a/cgn_flexbe/.flake8 b/cgn_flexbe/.flake8 new file mode 100644 index 0000000..7fc980c --- /dev/null +++ b/cgn_flexbe/.flake8 @@ -0,0 +1,3 @@ +[flake8] +exclude = .git +max-line-length = 130 diff --git a/cgn_flexbe/.gitignore b/cgn_flexbe/.gitignore new file mode 100644 index 0000000..890dd3f --- /dev/null +++ b/cgn_flexbe/.gitignore @@ -0,0 +1,3 @@ +*~ +*.pyc +*_tmp.py diff --git a/cgn_flexbe/CONTRIBUTING.md b/cgn_flexbe/CONTRIBUTING.md new file mode 100644 index 0000000..0703ae9 --- /dev/null +++ b/cgn_flexbe/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any new contributions that you make to this repository will +be under the Apache 2.0 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0). diff --git a/cgn_flexbe/LICENSE b/cgn_flexbe/LICENSE new file mode 100644 index 0000000..d809660 --- /dev/null +++ b/cgn_flexbe/LICENSE @@ -0,0 +1,13 @@ +Copyright 2023 Philipp Schillinger, Christopher Newport University + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/cgn_flexbe/README.md b/cgn_flexbe/README.md new file mode 100644 index 0000000..fd7c3fe --- /dev/null +++ b/cgn_flexbe/README.md @@ -0,0 +1,381 @@ +# FlexBE States and Behaviors for Contact-GraspNet + +FlexBE service states and behavior pipelines for integrating **Contact-GraspNet (CGN)** into a ROS 2 manipulation workflow. + +This repository provides: + +- **FlexBE service states** for calling a ROS 2 Contact-GraspNet server +- **Behavior pipelines** that connect perception/segmentation to grasp planning and motion execution +- **FlexBE utilities** for utility scripts different from `uml-robotics/compare_flexbe` upstream +- A recommended pipeline using: + - **Unseen Object Clustering (UOC)** for segmentation + - **Contact-GraspNet with RGB-D inputs** for grasp generation + +## Overview + +This package is a **FlexBE-based perception-to-action integration layer** for Contact-GraspNet. + +It supports two CGN service states: + +1. **`cgn_grasp_rgbd_service_state.py`** (recommended) + - Uses an **RGB-D-based CGN workflow** + - Works well with **Unseen Object Clustering (UOC)** as the upstream segmentation step + - Calls the ROS 2 Contact-GraspNet server (`/get_grasps`) using a `scene_name` convention (server-side scene loading) + +2. **`cgn_grasp_cloud_service_state.py`** (less recommended) + - Uses **point cloud input** (`sensor_msgs/PointCloud2`) + - Sends flattened XYZ points + mask/indices to the CGN ROS 2 server + - Works with PointCloud-based perception modules `GetPointCloudServiceState` and `EuclideanClusteringServiceState`, but is generally less robust than the RGB-D/UOC pipeline + +## Recommended Pipeline + +**Most recommended:** +**UOC segmentation + CGN (RGB-D) + MoveIt OMPL motion planning** + +Implemented in: + +- `unseenobjclustercontactgraspnetpipeine_sm.py` + +High-level flow: + +1. RGB-D segmentation (UOC) +2. Target instance selection +3. Contact-GraspNet grasp generation (RGB-D mode) +4. MoveIt-based motion planning to candidate grasp poses + +## Repository Structure + +```text +├── cgn_flexbe +├── cgn_flexbe_behaviors +│ ├── bin +│ ├── cgn_flexbe_behaviors +│ │ ├── __init__.py +│ │ ├── euclideanclustercontactgraspnetpipeine_sm.py +│ │ ├── pointcloudcontactgraspnetpipeine_sm.py +│ │ └── unseenobjclustercontactgraspnetpipeine_sm.py +│ ├── CHANGELOG.rst +│ ├── CMakeLists.txt +│ ├── config +│ │ └── example.yaml +│ ├── manifest +│ │ ├── euclideanclustercontactgraspnetpipeine.xml +│ │ ├── pointcloudcontactgraspnetpipeine.xml +│ │ └── unseenobjclustercontactgraspnetpipeine.xml +│ ├── package.xml +│ ├── resource +│ │ └── cgn_flexbe_behaviors +│ ├── setup.cfg +│ └── setup.py +├── cgn_flexbe_states +│ ├── cgn_flexbe_states +│ │ ├── __init__.py +│ │ ├── cgn_grasp_cloud_service_state.py +│ │ ├── cgn_grasp_rgbd_service_state.py +│ │ ├── reach_to_grasp_service_state.py +│ │ └── select_instance_to_cgn_indices_state.py +│ ├── CHANGELOG.rst +│ ├── package.xml +│ ├── resource +│ │ └── cgn_flexbe_states +│ ├── setup.cfg +│ └── setup.py +``` + +## Quick Start + +This section is tailored to the service names used in the uploaded FlexBE states/behaviors. + +### 1) Build the workspace + +```bash +cd ~/your_ws +colcon build --symlink-install +source install/setup.bash +``` + +### 2) Setup required ROS 2 servers (recommended UOC + RGB-D CGN pipeline) + +The recommended behavior (`UnseenObjClusterContactGraspnetPipeine`) expects these services to be set up: + +- `/segmentation_rgbd` (UOC segmentation server): setup unseen_object_clustering_ros2 server through https://github.com/zhaohuajing/unseen_object_clustering_ros2 +- `/get_grasps` (Contact-GraspNet server): setup contact_graspnet_ros2 server through https://github.com/zhaohuajing/contact_graspnet_ros2 +- `/move_to_pose` (MoveIt/OMPL execution server): setup moveit and armada_ros2 servers through https://github.com/flynnbm/armada_ros2 + +Consider adding the following nodes to your launch file: +```text +uoc_rgbd_bringup = Node( + package="unseen_obj_clst_ros2", + executable="segmentation_rgbd_server", + name="segmentation_rgbd_server", + output="screen", +) + +cgn_rgbd_bringup = Node( + package="contact_graspnet_ros2", + executable="grasp_executor_rgbd_server", + name="grasp_executor_rgbd_server", + output="screen", +) + + +cgn_cloud_bringup = Node( + package="contact_graspnet_ros2", + executable="grasp_executor_cloud_server", + name="grasp_executor_cloud_server", + output="screen", +) +``` + +### 3) Start FlexBE and run a behavior + +Open your FlexBE app / onboard execution, then run one of: + +- `UnseenObjClusterContactGraspnetPipeine` (**recommended**) +- `EuclideanClusterContactGraspnetPipeine` +- `PointCloudContactGraspnetPipeine` + +### 4) Optional services for alternate pipelines + +Depending on the behavior you choose, additional services may be required: + +- `/get_point_cloud` +- `/euclidean_clustering` +- `/filter_by_indices` +- `/reach_to_grasp` + +You can verify everything is up with: + +```bash +ros2 service list | grep -E "segmentation_rgbd|get_grasps|move_to_pose|get_point_cloud|euclidean_clustering|filter_by_indices|reach_to_grasp" +``` + +## Provided FlexBE States + +### `CGNGraspRGBDServiceState` (recommended) +**File:** `cgn_flexbe_states/cgn_grasp_rgbd_service_state.py` + +Calls the Contact-GraspNet ROS 2 service in **RGB-D scene-name mode** (server loads data internally using `scene_name`). + +**Inputs** +- `scene_name` (`string`) — CGN scene identifier (e.g., `scene_from_ucn`) + +**Outputs** +- `grasp_target_poses` (`geometry_msgs/Pose[]`) +- `grasp_scores` (`float[]`) +- `grasp_samples` (`geometry_msgs/Point[]`) +- `grasp_object_ids` (`int[]`) + +**Default service** +- `/get_grasps` + +--- + +### `CGNGraspCloudServiceState` (point cloud mode, less recommended) +**File:** `cgn_flexbe_states/cgn_grasp_cloud_service_state.py` + +Calls the Contact-GraspNet ROS 2 service using `PointCloud2`, converting the cloud to flattened XYZ points and optionally applying index-based masking / Z filtering before the request. + +**Inputs** +- `cloud_in` (`sensor_msgs/PointCloud2`) — target/filtered point cloud +- `indices` (`list[int]`, optional) — optional point indices used for mask generation + +**Outputs** +- `grasp_target_poses` (`geometry_msgs/Pose[]`) +- `grasp_scores` (`float[]`) +- `grasp_samples` (`geometry_msgs/Point[]`) +- `grasp_object_ids` (`int[]`) + +**Default service** +- `/get_grasps` + +--- + +### Other related states (used in pipelines) +This repository also includes utility states for integration and execution, such as: + +- `reach_to_grasp_service_state.py` +- `select_instance_to_cgn_indices_state.py` + +The behaviors also rely on additional states from companion packages (e.g., `compare_flexbe_states`) such as: +- `UnseenObjSegRGBDServiceState` +- `GetPointCloudServiceState` +- `EuclideanClusteringServiceState` +- `FilterByIndicesServiceState` +- `MoveToPoseServiceState` +- `PublishPointCloudState` + +## Provided FlexBE Behaviors (Pipelines) + +### 1) `UnseenObjClusterContactGraspnetPipeine` (recommended) +**File:** `cgn_flexbe_behaviors/cgn_flexbe_behaviors/unseenobjclustercontactgraspnetpipeine_sm.py` + +Pipeline: +1. `UnseenObjSegRGBDServiceState` (`/segmentation_rgbd`) +2. `SelectInstanceToSceneNameState` +3. `CGNGraspRGBDServiceState` (`/get_grasps`) +4. `MoveToPoseServiceState` (`/move_to_pose`) + +Why recommended: +- Best compatibility in this integration +- Clean segmentation-to-scene-name handoff +- More stable grasp generation in the current setup + +--- + +### 2) `EuclideanClusterContactGraspnetPipeine` +**File:** `cgn_flexbe_behaviors/cgn_flexbe_behaviors/euclideanclustercontactgraspnetpipeine_sm.py` + +Pipeline: +1. `GetPointCloudServiceState` (`/get_point_cloud`) +2. `EuclideanClusteringServiceState` (`/euclidean_clustering`) +3. `FilterByIndicesServiceState` (`/filter_by_indices`) +4. `CGNGraspServiceState` (`/get_grasps`, point cloud mode) +5. `PublishPointCloudState` (publishes `/filtered_cloud/target_object`) +6. `MoveToPoseServiceState` (`/move_to_pose`) + +Use case: +- Full point-cloud pipeline with clustering +- Useful fallback when UOC is unavailable + +--- + +### 3) `PointCloudContactGraspnetPipeine` +**File:** `cgn_flexbe_behaviors/cgn_flexbe_behaviors/pointcloudcontactgraspnetpipeine_sm.py` + +Pipeline: +1. `GetPointCloudServiceState` (`/get_point_cloud`) +2. `CGNGraspServiceState` (`/get_grasps`, point cloud mode) +3. `MoveToPoseServiceState` (`/move_to_pose`) +4. `ReachToGraspServiceState` (`/reach_to_grasp`) + +Use case: +- Minimal direct point-cloud-to-grasp pipeline +- Good for debugging and simpler demos + +## Tables for Easier Documentation + +### State summary + +| State file | Main class | Inputs | Outputs | Service called | Notes | +|---|---|---|---|---|---| +| `cgn_grasp_rgbd_service_state.py` | `CGNGraspRGBDServiceState` | `scene_name` | `grasp_target_poses`, `grasp_scores`, `grasp_samples`, `grasp_object_ids` | `/get_grasps` | Recommended. RGB-D scene-name mode. | +| `cgn_grasp_cloud_service_state.py` | `CGNGraspServiceState` | `cloud_in`, `indices` (optional) | `grasp_target_poses`, `grasp_scores`, `grasp_samples`, `grasp_object_ids` | `/get_grasps` | Point-cloud mode, less recommended. | +| `reach_to_grasp_service_state.py` | `ReachToGraspServiceState` | typically `grasp_poses`, `grasp_index` | behavior-dependent | `/reach_to_grasp` | Used as final approach/closure in point-cloud pipeline. | +| `select_instance_to_cgn_indices_state.py` | `SelectInstanceToSceneNameState` or related selector | segmentation results (`seg_json`, ids, masks) | selected target / `scene_name` or indices | none (local mapping) | Bridges segmentation output to CGN input convention. | + +### Behavior summary + +| Behavior (FlexBE) | Main file | Pipeline type | Services used | Recommended | +|---|---|---|---|---| +| `UnseenObjClusterContactGraspnetPipeine` | `unseenobjclustercontactgraspnetpipeine_sm.py` | UOC (RGB-D) -> CGN RGB-D -> MoveIt | `/segmentation_rgbd`, `/get_grasps`, `/move_to_pose` | Yes (primary) | +| `EuclideanClusterContactGraspnetPipeine` | `euclideanclustercontactgraspnetpipeine_sm.py` | Point cloud -> Euclidean clustering -> CGN cloud -> MoveIt | `/get_point_cloud`, `/euclidean_clustering`, `/filter_by_indices`, `/get_grasps`, `/move_to_pose` | Secondary | +| `PointCloudContactGraspnetPipeine` | `pointcloudcontactgraspnetpipeine_sm.py` | Point cloud -> CGN cloud -> MoveIt -> Reach | `/get_point_cloud`, `/get_grasps`, `/move_to_pose`, `/reach_to_grasp` | Debug/fallback | + +## Architecture + +### Recommended architecture (UOC + RGB-D CGN) + +```text +RGB-D Camera + | + v +Unseen Object Clustering (ROS 2 service: /segmentation_rgbd) + | + v +Target instance selection / scene mapping + | + v +CGNGraspRGBDServiceState + | + v +Contact-GraspNet ROS 2 Server (/get_grasps) + | + v +Grasp poses (Pose[]) + | + v +MoveIt / OMPL (MoveToPoseServiceState -> /move_to_pose) + | + v +Robot motion execution +``` + +### Point-cloud architecture (less recommended) + +```text +PointCloud2 (/get_point_cloud) + | + v +(optional) Euclidean clustering / filtering + | + v +CGNGraspServiceState (cloud mode) + | + v +Contact-GraspNet ROS 2 Server (/get_grasps) + | + v +Grasp poses (Pose[]) + | + v +MoveIt / OMPL + | + v +(optional) ReachToGrasp (/reach_to_grasp) +``` + +## Dependencies + +This repository assumes you already have the following in your ROS 2 workspace: + +- **FlexBE** (core + onboard/app tooling) +- **Contact-GraspNet ROS 2 server** + - service: `/get_grasps` + - service/message types from `contact_graspnet_ros2` +- **MoveIt / motion execution service(s)** + - `/move_to_pose` + - optionally `/reach_to_grasp` +- **Perception/segmentation services** depending on pipeline: + - `/segmentation_rgbd` (UOC, recommended) + - `/get_point_cloud` + - `/euclidean_clustering` + - `/filter_by_indices` + +## Installation + +Clone into your ROS 2 workspace `src/` folder: + +```bash +cd ~/your_ws/src +git clone +``` + +Build and source: + +```bash +cd ~/your_ws +colcon build --symlink-install +source install/setup.bash +``` + +## Notes and Recommendations + +- **Use `CGNGraspRGBDServiceState` + UOC whenever possible.** + This is the most reliable configuration in the current integration. + +- The point-cloud CGN state is still useful for debugging or fallback integration, but grasp quality and robustness may be lower depending on scene preprocessing. + +- These behavior files are generated FlexBE state machines. Manual edits inside generated sections may be overwritten if the behavior is regenerated. + +- Some behavior imports reference companion states from `compare_flexbe_states` (e.g., segmentation, clustering, move-to-pose). Keep those packages in the same workspace. + +## Acknowledgments + +This repository builds on: + +- Contact-GraspNet +- FlexBE +- ROS 2 +- MoveIt +- Upstream perception modules (Unseen Object Clustering / PCL clustering) diff --git a/cgn_flexbe/cgn_flexbe/CHANGELOG.rst b/cgn_flexbe/cgn_flexbe/CHANGELOG.rst new file mode 100644 index 0000000..23b2b24 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cgn_flexbe +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE project \ No newline at end of file diff --git a/cgn_flexbe/cgn_flexbe/CMakeLists.txt b/cgn_flexbe/cgn_flexbe/CMakeLists.txt new file mode 100644 index 0000000..18529ad --- /dev/null +++ b/cgn_flexbe/cgn_flexbe/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.10) +project(cgn_flexbe) + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/cgn_flexbe/cgn_flexbe/package.xml b/cgn_flexbe/cgn_flexbe/package.xml new file mode 100644 index 0000000..e441c69 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe/package.xml @@ -0,0 +1,24 @@ + + cgn_flexbe + 0.0.1 + + Meta-package for FlexBE enabled cgn + + TODO + TODO + BSD + TODO/issues + TODO + TODO + + ament_cmake + + flexbe_core + cgn_flexbe_behaviors + cgn_flexbe_states + + + ament_cmake + + + diff --git a/cgn_flexbe/cgn_flexbe_behaviors/CHANGELOG.rst b/cgn_flexbe/cgn_flexbe_behaviors/CHANGELOG.rst new file mode 100644 index 0000000..388ec91 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cgn_flexbe_behaviors +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE behaviors \ No newline at end of file diff --git a/cgn_flexbe/cgn_flexbe_behaviors/CMakeLists.txt b/cgn_flexbe/cgn_flexbe_behaviors/CMakeLists.txt new file mode 100644 index 0000000..50b1279 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.10) +project(cgn_flexbe_behaviors) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) + +install(DIRECTORY + manifest + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + config + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS + bin/copy_behavior + DESTINATION lib/${PROJECT_NAME} +) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +ament_package() diff --git a/cgn_flexbe/cgn_flexbe_behaviors/bin/copy_behavior b/cgn_flexbe/cgn_flexbe_behaviors/bin/copy_behavior new file mode 100755 index 0000000..0469953 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/bin/copy_behavior @@ -0,0 +1,102 @@ +#!/bin/bash + +# Copyright 2025 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +# WARNING: Use at your own risk, this script does not protect against overwriting files, or mis-naming + +if [ $# -lt 1 ]; then + echo -e "\e[93mThis is an extremely simple script designed to copy behaviors\033[0m" + echo -e "\e[93mfrom the install folder to the src folder assumed to be a git repo\033[0m" + echo -e "\e[93mfor storage.\033[0m" + echo "" + echo -e "\e[93mIt requires a WORKSPACE_ROOT environment variable assuming a standard \033[0m" + echo -e "\e[93m \${WORKSPACE_ROOT}/install and \${WORKSPACE_ROOT}/src layout.\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/cgn\033[0m" + echo "" + echo -e "\e[93m Usage: ros2 run cgn_flexbe_behaviors copy_behavior BEHAVIOR_FILE_BASE \033[0m" + echo -e "\e[93m where BEHAVIOR_FILE_BASE_NAME.xml is the saved manifest name\033[0m" + echo -e "\e[93m OPTIONAL_BEHAVIOR_PACKAGE defaults to 'cgn_flexbe_behaviors'\033[0m" + echo "" + echo -e "\e[93m WARNING: Use at your own risk, this script does not protect against overwriting files or mis-naming\033[0m" + exit 2 +fi + + +beh=$(echo "$1" | cut -f 1 -d '.') # include only the base file name is someone pastes .xml name +pack="cgn_flexbe_behaviors" # Default name uses this behaviors package +if [ $# -eq 2 ]; then + pack="$2" + echo "Using specified package '${pack}'!" +fi + +# A few basic checks before attempting to copy +if [ ! -d "${pack}" ]; then + echo "" + echo -e "\e[91m Package '${pack}' does not exist under current directory '${PWD}' !\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/cgn\033[0m" + exit +fi + +if [ ! -d "${pack}/${pack}" ]; then + echo "" + echo -e "\e[91m Behavior implementation folder '${pack}/${pack}' does not exist under current directory '${PWD}' !\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/cgn\033[0m" + exit +fi + +PYVER=$(python3 -c "import sys; print(f'{sys.version_info[0]}.{sys.version_info[1]}')") + +if [ ! -f "${WORKSPACE_ROOT}/install/${pack}/lib/${PYVER}/site-packages/${pack}/${beh}_sm.py" ]; then + echo "" + echo -e "\e[91m Behavior '${beh}' implementation does not exist in install folder for '${pack}' package!\033[0m" + echo "" + echo "Available behavior manifests in '${pack}' :" + ls ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest + echo "" + echo -e "\e[93mConfirm behavior and package names!\033[0m" + exit +fi + + +# Begin the work +echo "Copying '${beh}' implementation to '${pack}' under '${PWD}' ..." +cp ${WORKSPACE_ROOT}/install/${pack}/local/lib/${PYVER}/site-packages/${pack}/${beh}_sm.py ./${pack}/${pack}/${beh}_sm.py + +if test $? -eq 0; then + echo "Copying '${beh}.xml' manifest to '${pack}' ..." + + cp ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest/${beh}.xml ./${pack}/manifest/${beh}.xml + if test $? -eq 0; then + echo "" + echo -e "\e[92mDone copying behavior '${beh}' to '${pack}'!\033[0m" + echo "Do not forget to git commit and git push any changes." + exit + else + echo "" + echo -e "\e[91m Failed to copy behavior '${beh}.xml' manifest after copying behavior implementation!\033[0m" + exit + fi +else + echo "" + echo -e "\e[91m Failed to copy behavior '${beh}'!\033[0m" + exit +fi \ No newline at end of file diff --git a/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/__init__.py b/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/euclideanclustercontactgraspnetpipeine_sm.py b/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/euclideanclustercontactgraspnetpipeine_sm.py new file mode 100644 index 0000000..5324c2a --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/euclideanclustercontactgraspnetpipeine_sm.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2025 Huajing Zhao +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +# TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define EuclideanClusterContactGraspnetPipeine. + +A perception-to-action pipeline which employs cluster extraction in order to +cluster pickable objects from a +scene and choose a target for grasping, then contact-graspnet for grasp +planning and OMPL for manipulation via MoveIt + +Created on Oct 22 2025 +@author: Huajing Zhao +""" + + +from cgn_flexbe_states.cgn_grasp_cloud_service_state import CGNGraspCloudServiceState +from compare_flexbe_states.euclidean_clustering_service_state import EuclideanClusteringServiceState +from compare_flexbe_states.filter_by_indices_service_state import FilterByIndicesServiceState +from compare_flexbe_states.get_point_cloud_service_state import GetPointCloudServiceState +from compare_flexbe_states.move_to_pose_service_state import MoveToPoseServiceState +from compare_flexbe_states.publish_point_cloud_state import PublishPointCloudState +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class EuclideanClusterContactGraspnetPipeineSM(Behavior): + """ + Define EuclideanClusterContactGraspnetPipeine. + + A perception-to-action pipeline which employs cluster extraction in order to + cluster pickable objects from a + scene and choose a target for grasping, then contact-graspnet for grasp + planning and OMPL for manipulation via MoveIt + """ + + def __init__(self, node): + super().__init__() + self.name = 'EuclideanClusterContactGraspnetPipeine' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:1576 y:418, x:251 y:389 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['target_cluster_indexed', 'scene_pointcloud']) + _state_machine.userdata.scene_pointcloud = 0 + _state_machine.userdata.camera_pose = 0 + _state_machine.userdata.target_cluster_indexed = 0 + _state_machine.userdata.obstacle_clusters_indexed = 0 + _state_machine.userdata.cluster_count = 0 + _state_machine.userdata.point_cloud_visual = 0 + _state_machine.userdata.camera_source = 0 + _state_machine.userdata.grasp_poses = [] + _state_machine.userdata.test_indices = [] + _state_machine.userdata.grasp_waypoints = [] + _state_machine.userdata.waypoint_index = 0 + _state_machine.userdata.grasp_target_poses = [] + _state_machine.userdata.grasp_index = 0 + _state_machine.userdata.ready_pose = 'ready' + _state_machine.userdata.scene_id = 0 + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:119 y:63 + OperatableStateMachine.add('GetPointCloud', + GetPointCloudServiceState(service_timeout=5.0, + service_name='/get_point_cloud', + camera_topic='/rgbd_camera/points', + target_frame='simple_pedestal'), + transitions={'finished': 'EuclideanClustering' # 306 84 -1 -1 -1 -1 + , 'failed': 'failed' # 236 225 228 116 -1 -1 + }, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'camera_pose': 'camera_pose', + 'cloud_out': 'scene_pointcloud', + 'cloud_frame': 'cloud_frame'}) + + # x:831 y:53 + OperatableStateMachine.add('CgnGrasp', + CGNGraspCloudServiceState(service_name='/get_grasps', + use_scene_id=False, + service_timeout=5.0, + field_names=None), + transitions={'done': 'PublishPointCloud', + 'failed': 'failed' # 688 269 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'cloud_in': 'point_cloud_visual', + 'indices': 'test_indices', + 'scene_id': 'scene_id', + 'grasp_target_poses': 'grasp_target_poses', + 'grasp_scores': 'grasp_scores', + 'grasp_samples': 'grasp_samples', + 'grasp_object_ids': 'grasp_object_ids'}) + + # x:344 y:61 + OperatableStateMachine.add('EuclideanClustering', + EuclideanClusteringServiceState(service_timeout=5.0, + service_name='/euclidean_clustering', + cluster_tolerance=0.02, + min_cluster_size=100, + max_cluster_size=25000), + transitions={'finished': 'FilterByIndices' # 557 80 -1 -1 -1 -1 + , 'failed': 'failed' # 400 213 406 114 -1 -1 + }, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'cloud_in': 'scene_pointcloud', + 'camera_pose': 'camera_pose', + 'target_cluster_indices': 'target_cluster_indices', + 'obstacle_cluster_indices': 'obstacle_cluster_indices'}) + + # x:600 y:56 + OperatableStateMachine.add('FilterByIndices', + FilterByIndicesServiceState(service_timeout=5.0, + service_name='/filter_by_indices'), + transitions={'finished': 'CgnGrasp', + 'failed': 'failed' # 648 229 -1 -1 -1 -1 + }, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'cloud_in': 'scene_pointcloud', + 'target_indices': 'target_cluster_indices', + 'cloud_out': 'point_cloud_visual'}) + + # x:1316 y:56 + OperatableStateMachine.add('MoveOMPL', + MoveToPoseServiceState(timeout_sec=5.0, + service_name='/move_to_pose'), + transitions={'done': 'finished' # 1455 349 -1 -1 -1 -1 + , 'next': 'MoveOMPL' # 1376 147 -1 -1 -1 -1 + , 'failed': 'failed' # 1142 394 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, + 'next': Autonomy.Off, + 'failed': Autonomy.Off}, + remapping={'grasp_poses': 'grasp_target_poses', + 'grasp_index': 'grasp_index'}) + + # x:1055 y:53 + OperatableStateMachine.add('PublishPointCloud', + PublishPointCloudState(pub_topic='/filtered_cloud/target_object'), + transitions={'done': 'MoveOMPL', + 'failed': 'failed' # 1099 242 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'cloud_in': 'point_cloud_visual'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/pointcloudcontactgraspnetpipeine_sm.py b/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/pointcloudcontactgraspnetpipeine_sm.py new file mode 100644 index 0000000..cc57f21 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/pointcloudcontactgraspnetpipeine_sm.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2025 Huajing Zhao +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +# TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define PointCloudContactGraspnetPipeine. + +A perception-to-action pipeline which employs that directly input point clouds +to contact-graspnet, which masks objects above the table for grasp +planning and OMPL for manipulation via MoveIt + +Created on Oct 22 2025 +@author: Huajing Zhao +""" + + +from cgn_flexbe_states.cgn_grasp_cloud_service_state import CGNGraspCloudServiceState +from compare_flexbe_states.get_point_cloud_service_state import GetPointCloudServiceState +from compare_flexbe_states.move_to_pose_service_state import MoveToPoseServiceState +from cgn_flexbe_states.reach_to_grasp_service_state import ReachToGraspServiceState +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class PointCloudContactGraspnetPipeineSM(Behavior): + """ + Define PointCloudContactGraspnetPipeine. + + A perception-to-action pipeline which employs that directly input point clouds + to contact-graspnet, which masks objects above the table for grasp + planning and OMPL for manipulation via MoveIt + """ + + def __init__(self, node): + super().__init__() + self.name = 'PointCloudContactGraspnetPipeine' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:841 y:381, x:251 y:389 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['target_cluster_indexed', 'scene_pointcloud']) + _state_machine.userdata.scene_pointcloud = 0 + _state_machine.userdata.camera_pose = 0 + _state_machine.userdata.target_cluster_indexed = 0 + _state_machine.userdata.obstacle_clusters_indexed = 0 + _state_machine.userdata.cluster_count = 0 + _state_machine.userdata.point_cloud_visual = 0 + _state_machine.userdata.camera_source = 0 + _state_machine.userdata.grasp_poses = [] + _state_machine.userdata.test_indices = [] + _state_machine.userdata.grasp_waypoints = [] + _state_machine.userdata.waypoint_index = 0 + _state_machine.userdata.grasp_target_poses = [] + _state_machine.userdata.grasp_index = 0 + _state_machine.userdata.ready_pose = 'ready' + _state_machine.userdata.scene_id = 0 + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:119 y:63 + OperatableStateMachine.add('GetPointCloud', + GetPointCloudServiceState(service_timeout=5.0, + service_name='/get_point_cloud', + camera_topic='/rgbd_camera/points', + target_frame='simple_pedestal'), + transitions={'finished': 'CgnGrasp', + 'failed': 'failed' # 236 225 228 116 -1 -1 + }, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'camera_pose': 'camera_pose', + 'cloud_out': 'scene_pointcloud', + 'cloud_frame': 'cloud_frame'}) + + # x:419 y:65 + OperatableStateMachine.add('CgnGrasp', + CGNGraspCloudServiceState(service_timeout=5.0, + service_name='/get_grasps', + use_scene_id=False, + field_names=None, + z_min=0.28), + transitions={'done': 'MoveOMPL', + 'failed': 'failed' # 378 273 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'cloud_in': 'scene_pointcloud', + 'indices': 'test_indices', + 'grasp_target_poses': 'grasp_target_poses'}) + + # x:680 y:61 + OperatableStateMachine.add('MoveOMPL', + MoveToPoseServiceState(timeout_sec=5.0, + service_name='/move_to_pose'), + transitions={'done': 'ReachToGrasp', + 'next': 'MoveOMPL' # 747 24 -1 -1 -1 -1 + , 'failed': 'failed' # 538 304 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, + 'next': Autonomy.Off, + 'failed': Autonomy.Off}, + remapping={'grasp_poses': 'grasp_target_poses', + 'grasp_index': 'grasp_index'}) + + # x:919 y:55 + OperatableStateMachine.add('ReachToGrasp', + ReachToGraspServiceState(service_name='/reach_to_grasp'), + transitions={'done': 'finished', + 'failed': 'failed' # 641 334 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'grasp_poses': 'grasp_target_poses', + 'grasp_index': 'grasp_index'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/unseenobjclustercontactgraspnetpipeine_sm.py b/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/unseenobjclustercontactgraspnetpipeine_sm.py new file mode 100644 index 0000000..97e0e56 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/cgn_flexbe_behaviors/unseenobjclustercontactgraspnetpipeine_sm.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2025 Huajing Zhao +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +# TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define UnseenObjClusterContactGraspnetPipeine. + +A perception-to-action pipeline which employs unseen-object-clustering for +object segmentation from a +scene and choose a target for grasping, then contact-graspnet for grasp +planning and OMPL for manipulation via MoveIt + +Created on Dec 07 2025 +@author: Huajing Zhao +""" + + +from cgn_flexbe_states.cgn_grasp_rgbd_service_state import CGNGraspRGBDServiceState +from cgn_flexbe_states.move_to_pose_service_state import MoveToPoseServiceState +from cgn_flexbe_states.select_instance_to_cgn_indices_state import SelectInstanceToSceneNameState +from uoc_flexbe_states.unseen_obj_seg_rgbd_service_state import UnseenObjSegRGBDServiceState +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class UnseenObjClusterContactGraspnetPipeineSM(Behavior): + """ + Define UnseenObjClusterContactGraspnetPipeine. + + A perception-to-action pipeline which employs unseen-object-clustering for + object segmentation from a + scene and choose a target for grasping, then contact-graspnet for grasp + planning and OMPL for manipulation via MoveIt + """ + + def __init__(self, node): + super().__init__() + self.name = 'UnseenObjClusterContactGraspnetPipeine' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:1154 y:332, x:141 y:356 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['im_name']) + _state_machine.userdata.im_name = 'from_rgbd' + _state_machine.userdata.seg_json = {} + _state_machine.userdata.result_dir = '' + _state_machine.userdata.instance_ids_2d = [] + _state_machine.userdata.instance_id_list = [] + _state_machine.userdata.target_instance_id = 0 + _state_machine.userdata.scene_name = 'scene_from_ucn' + _state_machine.userdata.message = '' + _state_machine.userdata.grasp_target_poses = [] + _state_machine.userdata.grasp_scores = [] + _state_machine.userdata.grasp_samples = [] + _state_machine.userdata.grasp_object_ids = [] + _state_machine.userdata.grasp_index = 0 + _state_machine.userdata.manual_target_instance_id = -1 + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:30 y:40 + OperatableStateMachine.add('UnseenObjSegRGBD', + UnseenObjSegRGBDServiceState(service_name='/segmentation_rgbd', + service_timeout=5.0, + default_im_name='from_rgbd', + background_id=0), + transitions={'finished': 'SelectInstanceToScene', + 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'im_name': 'im_name', + 'seg_json': 'seg_json', + 'result_dir': 'result_dir', + 'instance_ids_2d': 'instance_ids_2d', + 'instance_id_list': 'instance_id_list', + 'instance_masks': 'instance_masks', + 'message': 'message'}) + + # x:762 y:41 + OperatableStateMachine.add('CgnGraspRGBD', + CGNGraspRGBDServiceState(service_timeout=20.0, + service_name='/get_grasps_rgbd'), + transitions={'done': 'MoveOMPL', 'failed': 'failed'}, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'scene_name': 'scene_name', + 'grasp_target_poses': 'grasp_target_poses', + 'grasp_scores': 'grasp_scores', + 'grasp_samples': 'grasp_samples', + 'grasp_object_ids': 'grasp_object_ids'}) + + # x:1087 y:38 + OperatableStateMachine.add('MoveOMPL', + MoveToPoseServiceState(timeout_sec=5.0, + service_name='/move_to_pose'), + transitions={'done': 'finished', + 'next': 'MoveOMPL', + 'failed': 'failed'}, + autonomy={'done': Autonomy.Off, + 'next': Autonomy.Off, + 'failed': Autonomy.Off}, + remapping={'grasp_poses': 'grasp_target_poses', + 'grasp_index': 'grasp_index'}) + + # x:419 y:38 + OperatableStateMachine.add('SelectInstanceToScene', + SelectInstanceToSceneNameState(default_scene_name='scene_from_ucn', + selection_mode='manual'), + transitions={'finished': 'CgnGraspRGBD', 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'seg_json': 'seg_json', + 'result_dir': 'result_dir', + 'instance_ids_2d': 'instance_ids_2d', + 'instance_id_list': 'instance_id_list', + 'im_name': 'im_name', + 'target_instance_id': 'target_instance_id', + 'scene_name': 'scene_name', + 'manual_target_instance_id': 'manual_target_instance_id', + 'message': 'message'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/cgn_flexbe/cgn_flexbe_behaviors/config/example.yaml b/cgn_flexbe/cgn_flexbe_behaviors/config/example.yaml new file mode 100644 index 0000000..e3f24de --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/config/example.yaml @@ -0,0 +1 @@ +# You can use this folder to place general config files diff --git a/cgn_flexbe/cgn_flexbe_behaviors/manifest/euclideanclustercontactgraspnetpipeine.xml b/cgn_flexbe/cgn_flexbe_behaviors/manifest/euclideanclustercontactgraspnetpipeine.xml new file mode 100644 index 0000000..808a896 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/manifest/euclideanclustercontactgraspnetpipeine.xml @@ -0,0 +1,21 @@ + + + + + + + Huajing Zhao + Oct 22 2025 + + Using contact_graspnet_flexbe package: A perception-to-action pipeline which employs cluster extraction in order to + cluster pickable objects from a + scene and choose a target for grasping, then contact-graspnet for grasp + planning and OMPL for manipulation via MoveIt + + + + + + + + diff --git a/cgn_flexbe/cgn_flexbe_behaviors/manifest/pointcloudcontactgraspnetpipeine.xml b/cgn_flexbe/cgn_flexbe_behaviors/manifest/pointcloudcontactgraspnetpipeine.xml new file mode 100644 index 0000000..58f7f62 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/manifest/pointcloudcontactgraspnetpipeine.xml @@ -0,0 +1,20 @@ + + + + + + + Huajing Zhao + Oct 22 2025 + + A perception-to-action pipeline which employs that directly input point clouds + to contact-graspnet, which masks objects above the table for grasp + planning and OMPL for manipulation via MoveIt + + + + + + + + diff --git a/cgn_flexbe/cgn_flexbe_behaviors/manifest/unseenobjclustercontactgraspnetpipeine.xml b/cgn_flexbe/cgn_flexbe_behaviors/manifest/unseenobjclustercontactgraspnetpipeine.xml new file mode 100644 index 0000000..58c0e7f --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/manifest/unseenobjclustercontactgraspnetpipeine.xml @@ -0,0 +1,21 @@ + + + + + + + Huajing Zhao + Dec 07 2025 + + Using contact_graspnet_flexbe package: A perception-to-action pipeline which employs unseen-object-clustering for + object segmentation from a + scene and choose a target for grasping, then contact-graspnet for grasp + planning and OMPL for manipulation via MoveIt + + + + + + + + diff --git a/cgn_flexbe/cgn_flexbe_behaviors/package.xml b/cgn_flexbe/cgn_flexbe_behaviors/package.xml new file mode 100644 index 0000000..edb36de --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/package.xml @@ -0,0 +1,30 @@ + + + + cgn_flexbe_behaviors + 0.0.1 + + cgn_flexbe_behaviors references all implemented behaviors. + + TODO + TODO + BSD + + rclpy + flexbe_core + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_cmake + ament_cmake_python + + + + ament_cmake + + diff --git a/cgn_flexbe/cgn_flexbe_behaviors/resource/cgn_flexbe_behaviors b/cgn_flexbe/cgn_flexbe_behaviors/resource/cgn_flexbe_behaviors new file mode 100644 index 0000000..e69de29 diff --git a/cgn_flexbe/cgn_flexbe_behaviors/setup.cfg b/cgn_flexbe/cgn_flexbe_behaviors/setup.cfg new file mode 100644 index 0000000..978b940 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/cgn_flexbe_behaviors +[install] +install_scripts=$base/lib/cgn_flexbe_behaviors diff --git a/cgn_flexbe/cgn_flexbe_behaviors/setup.py b/cgn_flexbe/cgn_flexbe_behaviors/setup.py new file mode 100644 index 0000000..0d575ef --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_behaviors/setup.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python +from setuptools import setup + +package_name = 'cgn_flexbe_behaviors' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='phil', + maintainer_email='philsplus@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_behavior_sm = cgn_flexbe_behaviors.example_behavior_sm', + ], + }, +) diff --git a/cgn_flexbe/cgn_flexbe_states/CHANGELOG.rst b/cgn_flexbe/cgn_flexbe_states/CHANGELOG.rst new file mode 100644 index 0000000..01ab893 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cgn_flexbe_states +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE states \ No newline at end of file diff --git a/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/__init__.py b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/cgn_grasp_cloud_service_state.py b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/cgn_grasp_cloud_service_state.py new file mode 100644 index 0000000..ca1c106 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/cgn_grasp_cloud_service_state.py @@ -0,0 +1,237 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import rclpy +from typing import Iterable, Optional + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyServiceCaller + +from geometry_msgs.msg import Point, Pose +from sensor_msgs.msg import PointCloud2 +from std_msgs.msg import Int64 + +# Read PCL points from PointCloud2 +from sensor_msgs_py import point_cloud2 as pc2 + +# Contact-GraspNet ROS2 service & msg +from contact_graspnet_ros2.srv import GetGrasps as SrvType +from contact_graspnet_ros2.msg import Grasps + + +class CGNGraspCloudServiceState(EventState): + """ + Calls the Contact-GraspNet `get_grasps` service and returns grasp poses. + + -- service_timeout float Timeout for service discovery (sec, default: 5.0) + -- service_name str Service name (default: '/get_grasps') + -- use_scene_name bool If True, only send `scene_name` (server loads NPZ/JSON internally) + -- field_names list[str] Point fields to read from PointCloud2 (default: ['x','y','z']) + + ># cloud_in sensor_msgs/PointCloud2 Input (filtered) cloud of the target object + ># scene_name int Optional: server-side dataset id (if using offline results) + ># indices list[int] or None Optional: indices mask (1 = keep) to match CGN 'mask' + <# grasp_target_poses geometry_msgs/Pose[] Target pose list from CGN + <# grasp_scores float[] Confidence scores + <# grasp_samples geometry_msgs/Point[] Contact points + <# grasp_object_ids int[] Object ids (per-grasp) + + <= done Service call succeeded + <= failed Service call failed or timed out + """ + + def __init__(self, + service_timeout: float = 5.0, + service_name: str = '/get_grasps', + use_scene_name: bool = False, + field_names: Optional[list] = None, + z_min: float = 0.28): + super().__init__( + outcomes=['done', 'failed'], + input_keys=['cloud_in', 'indices'], #, 'scene_name'], + output_keys=['grasp_target_poses', 'grasp_scores', 'grasp_samples', 'grasp_object_ids'] + ) + self._service_timeout = float(service_timeout) + self._service_name = str(service_name) + self._use_scene_name = bool(use_scene_name) + self._fields = field_names if field_names is not None else ['x', 'y', 'z'] + self._z_min = float(z_min) + + # Proxy service + self._srv = ProxyServiceCaller({self._service_name: SrvType}) + + # Result bookkeeping + self._res = None + self._had_error = False + + # ---------------------------- helpers ---------------------------- + + @staticmethod + def _ensure_int_list(seq: Iterable) -> list[int]: + return [int(v) for v in seq] + + @staticmethod + def _ensure_uint32_list(seq: Iterable) -> list[int]: + # Python int is unbounded, service will coerce to uint32 + return [int(v) & 0xFFFFFFFF for v in seq] + + @staticmethod + def _pc2_to_xyz_list(cloud: PointCloud2, fields: list[str]) -> list[float]: + """Returns flattened [x0, y0, z0, x1, y1, z1, ...] in float32-like Python floats.""" + # Read generator of tuples (x,y,z,...) using only requested fields + pts_iter = pc2.read_points(cloud, field_names=fields, skip_nans=True) + flat = [] + for t in pts_iter: + # t is (x, y, z) when fields=['x','y','z'] + flat.extend((float(t[0]), float(t[1]), float(t[2]))) + return flat + + # ---------------------------- lifecycle ---------------------------- + + def execute(self, userdata): + # Execute this method periodically while the state is active. + # Main purpose is to check state conditions and trigger a corresponding outcome. + # If no outcome is returned, the state will stay active. + + # Check for error or no response + if self._had_error or self._res is None: + return 'failed' + + try: + grasps = self._res.grasps + userdata.grasp_target_poses = list(grasps.poses) + userdata.grasp_scores = list(grasps.scores) + userdata.grasp_samples = list(grasps.samples) + userdata.grasp_object_ids = list(grasps.object_ids) + Logger.loginfo(f"[{type(self).__name__}] Received {len(grasps.poses)} CGN grasp poses.") + except Exception as e: + Logger.logerr(f"[{type(self).__name__}] Failed to copy result to userdata: {e}") + return 'failed' + + return 'done' + + def on_enter(self, userdata): + self._res = None + self._had_error = False + + # Build request + request = SrvType.Request() + + # If the server supports loading precomputed NPZ/JSON by scene id, + # you can skip sending points/mask and just pass scene_name. + # if self._use_scene_name: + # try: + # request.scene_name = int(userdata.scene_name) + # except Exception: + # request.scene_name = 0 # default fallback + # else: + # Prepare points from PointCloud2 + try: + cloud = userdata.cloud_in + request.points = self._pc2_to_xyz_list(cloud, self._fields) + + # Optional indices -> boolean-ish mask (uint32). If no indices are given, keep-all. + if userdata.indices: + # Make a dense keep-mask of size N; indices mark the ones to keep. + # Since we used skip_nans=True and projected to a *filtered* cloud already, + # we simply mark all as 1 unless a sparse index list is explicitly provided. + # If you want exact alignment with original cloud, pre-filter before this state. + idx = self._ensure_int_list(userdata.indices) + # We'll just pass a trivial same-length mask of ones; the server ignores 0s anyway + # if you pre-filtered the cloud to target object. + # (If you truly need sparse selection, pass a binary mask the same length as points/3.) + # Default: "keep all" + # # (mask length in server is expected to match number of points; 1 = keep) + # num_pts = len(request.points) // 3 + # request.mask = [1] * num_pts + + # Build Z > z_min mask (1 = keep) + num_pts = len(request.points) // 3 + if num_pts == 0: + raise RuntimeError("Input cloud produced zero points") + Logger.loginfo(f"[{type(self).__name__}] num_pts = {num_pts}") + + # z values live at indices 2, 5, 8, ... + z_vals = (request.points[2::3]) + z_mask = [1 if float(z) > self._z_min else 0 for z in z_vals] + + # Optional indices -> combine with z_mask (AND) + # If indices is a list of positions to keep, turn into a dense mask. + if hasattr(userdata, 'indices') and userdata.indices: + idx_list = self._ensure_int_list(userdata.indices) + idx_mask = [0] * num_pts + for i in idx_list: + if 0 <= i < num_pts: + idx_mask[i] = 1 + # AND-combine: keep only those above the table AND in indices + request.mask = [1 if (z_mask[i] and idx_mask[i]) else 0 for i in range(num_pts)] + else: + # Only Z-filter + request.mask = z_mask + + kept = sum(request.mask) + Logger.loginfo(f"[{type(self).__name__}] Z-filter: z_min={self._z_min:.3f}m " + f"→ kept {kept}/{num_pts} points") + + if kept == 0: + Logger.logwarn(f"[{type(self).__name__}] No points above z_min={self._z_min:.3f}m; aborting.") + self._had_error = True + return + + except Exception as e: + Logger.logerr(f"[{type(self).__name__}] Failed to prepare request from cloud: {e}") + self._had_error = True + return + + # # Optional scene id (used by your server for bookkeeping) + # try: + # request.scene_name = int(userdata.scene_name) + # except Exception: + # request.scene_name = 0 + + + # inside on_enter(), before using self._srv: + import time + deadline = time.time() + self._service_timeout + while time.time() < deadline: + if self._srv.is_available(self._service_name): + break + time.sleep(0.2) + + # Wait for service + if not self._srv.is_available(self._service_name): + Logger.logerr(f"[{type(self).__name__}] Service '{self._service_name}' not available after {self._service_timeout}s.") + self._had_error = True + return + + # Call service + try: + self._res = self._srv.call(self._service_name, request) + Logger.loginfo(f"[{type(self).__name__}] Called service '{self._service_name}'.") + except Exception as e: + Logger.logerr(f"[{type(self).__name__}] Service call failed: {e}") + self._had_error = True + self._res = None + + + def on_exit(self, userdata): + # Call this method when an outcome is returned and another state gets active. + # It can be used to stop possibly running processes started by on_enter. + + # No-op: template hook + pass + + def on_start(self): + # Call this method when the behavior is instantiated on board. + # If possible, it is generally better to initialize used resources in the constructor + # because if anything failed, the behavior would not even be started. + + # No-op: template hook + pass + + def on_stop(self): + # Call this method whenever the behavior stops execution, also if it is cancelled. + # Use this event to clean up things like claimed resources. + + # No-op: template hook + pass diff --git a/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/cgn_grasp_rgbd_service_state.py b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/cgn_grasp_rgbd_service_state.py new file mode 100644 index 0000000..603177b --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/cgn_grasp_rgbd_service_state.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import time +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyServiceCaller + +from contact_graspnet_ros2.srv import GetGrasps as SrvType +from contact_graspnet_ros2.msg import Grasps + +import subprocess, os + + + +class CGNGraspRGBDServiceState(EventState): + """ + Calls the Contact-GraspNet `get_grasps` service using only a precomputed + scene name (RGBD pipeline). + + Assumes the server: + - Expects `scene_name` to select test_data/.npy + - Ignores `points` and `mask` fields (or they can be left empty) + + This matches your `grasp_executor_rgbd_server.py`, which uses: + self.scene_name = request.scene_name + np_path = f"test_data/{scene_name}.npy" + and loads 'results/predictions_.npz'. :contentReference[oaicite:1]{index=1} + + ># scene_name string CGN scene name (without .npy) + <# grasp_target_poses geometry_msgs/Pose[] Grasp poses + <# grasp_scores float[] Grasp scores + <# grasp_samples geometry_msgs/Point[] Contact points + <# grasp_object_ids int[] Object IDs per grasp + + <= done Service call succeeded + <= failed Service call failed or no grasps received + """ + + def __init__(self, + service_timeout: float = 10.0, + service_name: str = '/get_grasps'): + super().__init__( + outcomes=['done', 'failed'], + input_keys=['scene_name'], + output_keys=['grasp_target_poses', 'grasp_scores', 'grasp_samples', 'grasp_object_ids'] + ) + + self._service_timeout = float(service_timeout) + self._service_name = str(service_name) + + self._srv = ProxyServiceCaller({self._service_name: SrvType}) + self._res = None + self._had_error = False + + def on_enter(self, userdata): + self._res = None + self._had_error = False + + try: + scene_name = str(userdata.scene_name) + except Exception: + scene_name = "scene_from_ucn" + + Logger.loginfo(f"[CGNGraspRGBDServiceState] Requesting grasps for scene '{scene_name}'") + + request = SrvType.Request() + # Leave points/mask empty; server loads data from scene_name internally. + request.points = [] + request.mask = [] + request.scene_name = scene_name + + # Wait for service to be available + deadline = time.time() + self._service_timeout + while time.time() < deadline: + if self._srv.is_available(self._service_name): + break + time.sleep(0.2) + + if not self._srv.is_available(self._service_name): + Logger.logerr(f"[CGNGraspRGBDServiceState] Service '{self._service_name}' not available after " + f"{self._service_timeout}s.") + self._had_error = True + return + + + # Call service + try: + self._res = self._srv.call(self._service_name, request) + + cmd = [ + "python3", + "/home/csrobot/graspnet_ws/src/contact_graspnet_ros2/contact_graspnet/result_plotter.py", + ] + subprocess.check_call(cmd) + + Logger.loginfo(f"[CGNGraspRGBDServiceState] Called service '{self._service_name}'.") + except Exception as e: + Logger.logerr(f"[CGNGraspRGBDServiceState] Service call failed: {e}") + self._had_error = True + self._res = None + + def execute(self, userdata): + if self._had_error or self._res is None: + return 'failed' + + try: + grasps = self._res.grasps # type: Grasps + userdata.grasp_target_poses = list(grasps.poses) + userdata.grasp_scores = list(grasps.scores) + userdata.grasp_samples = list(grasps.samples) + userdata.grasp_object_ids = list(grasps.object_ids) + + Logger.loginfo(f"[CGNGraspRGBDServiceState] Received {len(grasps.poses)} grasp poses.") + except Exception as e: + Logger.logerr(f"[CGNGraspRGBDServiceState] Failed to copy result to userdata: {e}") + return 'failed' + + if len(userdata.grasp_target_poses) == 0: + Logger.logwarn("[CGNGraspRGBDServiceState] No grasps in response.") + return 'failed' + + return 'done' + + def on_exit(self, userdata): + pass + + def on_start(self): + pass + + def on_stop(self): + pass diff --git a/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/move_to_pose_service_state.py b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/move_to_pose_service_state.py new file mode 100644 index 0000000..8cbf6be --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/move_to_pose_service_state.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 + +# Copyright 2023 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.duration import Duration + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyServiceCaller + +from compare_flexbe_utilities.srv import MoveToPose as SrvType +from geometry_msgs.msg import Pose + +class MoveToPoseServiceState(EventState): + """ + Calls a service to move the robot to a specific pose using the he setPoseTarget, plan and execute C++ functions wrapped into a service server. + + -- timeout_sec float Timeout for waiting for service (default: 5.0) + -- service_name str Service name (default: '/move_to_pose') + + ># grasp_poses list A list of geometry_msgs/Pose messages to choose from (uses the last one) + + <= finished Service call succeeded + <= failure Service call failed or pose was invalid + """ + + def __init__(self, timeout_sec=5.0, service_name='/move_to_pose'): + super().__init__(outcomes=['done', 'next', 'failed'], + input_keys=['grasp_poses', 'grasp_index'], + output_keys=['grasp_index'] + ) + self._timeout_sec = timeout_sec + self._service_name = service_name + self._client = None + + # Create proxy service caller to handle rclpy node + self._srv = ProxyServiceCaller({self._service_name: SrvType}) + + # result storage + self._res = None + self._had_error = False + + def execute(self, userdata): + # Execute this method periodically while the state is active. + # Main purpose is to check state conditions and trigger a corresponding outcome. + # If no outcome is returned, the state will stay active. + + # Check for error or no response + if self._had_error or self._res is None: + return 'failed' + + try: + Logger.loginfo(f"[{type(self).__name__}] Finished plan with result: {self._res.success}.") + # if self._res.success == 1: + if self._res.success == 1 and userdata.grasp_index >= 10: + return 'done' + if (userdata.grasp_index + 1) < len(userdata.grasp_poses): + userdata.grasp_index = userdata.grasp_index + 1 + return 'next' + else: + # This was the last set of waypoints + return 'failed' + except Exception as e: + Logger.logerr(f"[{type(self).__name__}] Service call failed: {str(e)}") + return 'failed' + + # Return outcome failed if no other outcomes occur + return 'failed' + + def on_enter(self, userdata): + # Call this method a single time when the state becomes active, when a transition from another state to this one is taken. + # It is primarily used to start actions which are associated with this state. + + # reset state + self._res = None + self._had_error = False + + # check for correct data + grasp_poses = userdata.grasp_poses + idx = userdata.grasp_index + + # check for correct data + if not isinstance(grasp_poses, list) or not isinstance(idx, int) or idx < 0 or idx >= len(grasp_poses): + Logger.logerr(f"[{type(self).__name__}] grasp_index {idx} out of range for {len(grasp_poses)} sets or invalid or missing data type in userdata.") + return + + # construct request + request = SrvType.Request() + request.target_pose = grasp_poses[idx] + + # wait for availability (once per entry) + if not self._srv.is_available(self._service_name): + Logger.logerr(f"[{type(self).__name__}] Service '{self._service_name}' not available after {self._service_timeout}s.") + self._had_error = True + return + + # send request + try: + self._res = self._srv.call(self._service_name, request) + Logger.loginfo(f"[{type(self).__name__}] Called service '{self._service_name}'.") + except Exception as e: + Logger.logerr(f"[{type(self).__name__}] Service call failed: {e}") + + def on_exit(self, userdata): + # Call this method when an outcome is returned and another state gets active. + # It can be used to stop possibly running processes started by on_enter. + + # No-op: template hook + pass + + def on_start(self): + # Call this method when the behavior is instantiated on board. + # If possible, it is generally better to initialize used resources in the constructor + # because if anything failed, the behavior would not even be started. + + # No-op: template hook + pass + + def on_stop(self): + # Call this method whenever the behavior stops execution, also if it is cancelled. + # Use this event to clean up things like claimed resources. + + # No-op: template hook + pass \ No newline at end of file diff --git a/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/reach_to_grasp_service_state.py b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/reach_to_grasp_service_state.py new file mode 100644 index 0000000..1e98916 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/reach_to_grasp_service_state.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 + +import rclpy + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyServiceCaller + +# Use the same srv type as move_to_pose +from compare_flexbe_utilities.srv import MoveToPose as SrvType +from geometry_msgs.msg import Pose + + +class ReachToGraspServiceState(EventState): + """ + Call /reach_to_grasp (MoveToPose.srv) to execute the grasp sequence: + - open gripper + - move along EE-Z + - close gripper + - lift in base-Z + - (optionally) open again, etc. + + We pass the selected grasp pose in the request as `target_pose`. + + -- service_name string Absolute service name (default: '/reach_to_grasp') + -- timeout_sec float [currently not used by ProxyServiceCaller] (default: 10.0) + + ># grasp_poses list List[geometry_msgs/Pose] of candidate grasp poses + ># grasp_index int Index of the grasp pose to use + + <= done Sequence executed successfully + <= failed Service call failed or service reported failure + """ + + def __init__(self, + service_name='/reach_to_grasp', + timeout_sec=10.0): + super().__init__( + outcomes=['done', 'failed'], + input_keys=['grasp_poses', 'grasp_index'], + output_keys=['grasp_index']) + + self._service_name = service_name + self._timeout_sec = timeout_sec + + # Do not block at construction time; service may not exist yet + self._srv = ProxyServiceCaller({self._service_name: SrvType}, + wait_duration=0.0) + + self._res = None + self._failed = False + + def on_enter(self, userdata): + self._failed = False + self._res = None + + Logger.loginfo(f"[{type(self).__name__}] Preparing to call service " + f"'{self._service_name}'") + + # Validate userdata + grasp_poses = userdata.grasp_poses + idx = userdata.grasp_index + + if (not isinstance(grasp_poses, list) or + not isinstance(idx, int) or + idx < 0 or + idx >= len(grasp_poses)): + Logger.logerr( + f"[{type(self).__name__}] grasp_index {idx} out of range for " + f"{len(grasp_poses)} poses or invalid userdata." + ) + self._failed = True + return + + request = SrvType.Request() + request.target_pose = grasp_poses[idx] + + if not self._srv.is_available(self._service_name): + Logger.logwarn( + f"[{type(self).__name__}] Service '{self._service_name}' not " + f"reported available yet, calling anyway." + ) + + try: + self._res = self._srv.call(self._service_name, request) + Logger.loginfo( + f"[{type(self).__name__}] Called service '{self._service_name}'.") + except Exception as e: + Logger.logerr( + f"[{type(self).__name__}] Service call to " + f"'{self._service_name}' failed: {e}" + ) + self._failed = True + + def execute(self, userdata): + if self._failed or self._res is None: + return 'failed' + + # MoveToPose.srv uses an integer success flag (usually 1 == success) + try: + if getattr(self._res, 'success', 0) == 1: + Logger.loginfo( + f"[{type(self).__name__}] Reach-to-grasp OK: " + f"{getattr(self._res, 'message', '')}" + ) + return 'done' + else: + Logger.logwarn( + f"[{type(self).__name__}] Reach-to-grasp failed: " + f"{getattr(self._res, 'message', '')}" + ) + return 'failed' + except Exception as e: + Logger.logerr( + f"[{type(self).__name__}] Error checking response: {e}") + return 'failed' + + def on_exit(self, userdata): + pass + + def on_start(self): + pass + + def on_stop(self): + pass diff --git a/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/select_instance_to_cgn_indices_state.py b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/select_instance_to_cgn_indices_state.py new file mode 100644 index 0000000..88232e6 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/cgn_flexbe_states/select_instance_to_cgn_indices_state.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import json +from flexbe_core import EventState, Logger +import subprocess +import numpy as np + +class SelectInstanceToSceneNameState(EventState): + def __init__(self, + default_scene_name: str = 'scene_from_ucn', + selection_mode: str = 'manual', # 'largest' | 'manual' | 'largest_or_manual' + allow_background: bool = False, + manual_sentinel: int = -1): + super().__init__( + outcomes=['finished', 'failed'], + input_keys=[ + 'seg_json', 'result_dir', 'instance_ids_2d', 'instance_id_list', 'im_name', + 'manual_target_instance_id' # NEW + ], + output_keys=['target_instance_id', 'scene_name', 'message'] + ) + self._default_scene_name = str(default_scene_name) + self._selection_mode = str(selection_mode).lower().strip() + self._allow_background = bool(allow_background) + self._manual_sentinel = int(manual_sentinel) + + self._had_error = False + self._target_id = None + self._msg = "" + + def _pick_largest(self, instance_ids, instance_ids_2d): + best_id = None + best_area = -1 + areas = {} + + for inst_id in instance_ids: + mask = (instance_ids_2d == inst_id) + area = int(mask.sum()) + areas[int(inst_id)] = area + Logger.loginfo(f"[SelectInstanceToSceneNameState] Instance {inst_id} has area {area}.") + if area > best_area: + best_area = area + best_id = inst_id + + return (None, None, areas) if best_id is None else (int(best_id), int(best_area), areas) + + def _get_manual_id(self, userdata): + # Accept None, missing, sentinel => "not provided" + if not hasattr(userdata, 'manual_target_instance_id'): + return None + v = userdata.manual_target_instance_id + if v is None: + return None + try: + v = int(v) + except Exception: + return None + if v == self._manual_sentinel: + return None + return v + + def on_enter(self, userdata): + self._had_error = False + self._target_id = None + self._msg = "" + + try: + seg = userdata.seg_json + if isinstance(seg, str): + seg = json.loads(seg) + + instance_ids = list(userdata.instance_id_list) if userdata.instance_id_list else [] + if not self._allow_background: + instance_ids = [i for i in instance_ids if int(i) != 0] + + if not instance_ids: + self._msg = "[SelectInstanceToSceneNameState] No instance ids found." + Logger.logwarn(self._msg) + self._had_error = True + return + + instance_ids_2d = np.array(userdata.instance_ids_2d, dtype=np.int32) + + best_id, best_area, areas = self._pick_largest(instance_ids, instance_ids_2d) + if best_id is None or best_area is None or best_area <= 0: + self._msg = "[SelectInstanceToSceneNameState] Failed to find a non-empty instance mask." + Logger.logwarn(self._msg) + self._had_error = True + return + + # NEW: manual id from userdata + manual_id = self._get_manual_id(userdata) + + # Decide + if self._selection_mode == 'largest': + chosen_id = best_id + elif self._selection_mode == 'manual': + if manual_id is None: + self._msg = ("[SelectInstanceToSceneNameState] selection_mode='manual' but " + "manual_target_instance_id was not provided.") + Logger.logwarn(self._msg) + self._had_error = True + return + chosen_id = manual_id + else: # 'largest_or_manual' default + chosen_id = manual_id if manual_id is not None else best_id + + # Validate chosen_id + valid_ids = [int(x) for x in instance_ids] + if int(chosen_id) not in valid_ids: + self._msg = (f"[SelectInstanceToSceneNameState] Chosen instance id {chosen_id} " + f"is not in instance_id_list {valid_ids}.") + Logger.logwarn(self._msg) + self._had_error = True + return + + chosen_area = areas.get(int(chosen_id), -1) + self._target_id = int(chosen_id) + self._msg = (f"[SelectInstanceToSceneNameState] Selected instance {self._target_id} " + f"(area={chosen_area}) → scene_name='{self._default_scene_name}'") + Logger.loginfo(self._msg) + + cmd = [ + "python3", + "/home/csrobot/graspnet_ws/src/contact_graspnet_ros2/contact_graspnet/test_data/ucn_to_cgn_scene.py", + # IMPORTANT: if your script supports args, you should pass them here: + # "--im_name", userdata.im_name, + # "--seg_dir", userdata.result_dir, + # "--scene_name", self._default_scene_name, + # "--target_id", str(self._target_id), + ] + subprocess.check_call(cmd) + + Logger.loginfo("[SelectInstanceToSceneNameState] Generated contact_graspnet/test_data/scene_from_ucn.npy using ucn_to_cgn_scene.py") + + except Exception as e: + self._msg = f"[SelectInstanceToSceneNameState] Exception: {e}" + Logger.logerr(self._msg) + self._had_error = True + + def execute(self, userdata): + if self._had_error: + userdata.message = self._msg + return 'failed' + + userdata.target_instance_id = self._target_id + userdata.scene_name = self._default_scene_name + userdata.message = self._msg + return 'finished' \ No newline at end of file diff --git a/cgn_flexbe/cgn_flexbe_states/package.xml b/cgn_flexbe/cgn_flexbe_states/package.xml new file mode 100644 index 0000000..62c02b3 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/package.xml @@ -0,0 +1,38 @@ + + + + cgn_flexbe_states + 0.0.1 + + cgn_flexbe_states provides a collection of custom states. + Feel free to add new states. + + TODO + TODO + BSD + + rclpy + flexbe_core + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + turtlesim + + ament_cmake + ament_cmake_python + + + + ament_python + + + diff --git a/cgn_flexbe/cgn_flexbe_states/resource/cgn_flexbe_states b/cgn_flexbe/cgn_flexbe_states/resource/cgn_flexbe_states new file mode 100644 index 0000000..e69de29 diff --git a/cgn_flexbe/cgn_flexbe_states/setup.cfg b/cgn_flexbe/cgn_flexbe_states/setup.cfg new file mode 100644 index 0000000..df5562b --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/cgn_flexbe_states +[install] +install_scripts=$base/lib/cgn_flexbe_states diff --git a/cgn_flexbe/cgn_flexbe_states/setup.py b/cgn_flexbe/cgn_flexbe_states/setup.py new file mode 100644 index 0000000..9b632b2 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/setup.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python + +from glob import glob +from setuptools import setup +from setuptools import find_packages + +PACKAGE_NAME = 'cgn_flexbe_states' + +setup( + name=PACKAGE_NAME, + version='0.0.1', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + ('share/' + PACKAGE_NAME + "/tests", glob('tests/*.test')), + ('share/' + PACKAGE_NAME + "/launch", glob('tests/*.launch.py')), + ], + + install_requires=['setuptools'], + zip_safe=True, + maintainer='TODO', + maintainer_email='TODO@TODO.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_action_state = cgn_flexbe_states.example_action_state', + 'example_state = cgn_flexbe_states.example_state', + ], + }, +) diff --git a/cgn_flexbe/cgn_flexbe_states/tests/example_action_state.test b/cgn_flexbe/cgn_flexbe_states/tests/example_action_state.test new file mode 100644 index 0000000..25aec12 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/tests/example_action_state.test @@ -0,0 +1,14 @@ +path: cgn_flexbe_states.example_action_state +class: ExampleActionState + +import_only: false + +params: + action_topic: 'action_is_not_available' + timeout: 0.5 + +input: + angle: 2. + +outcome: + failed diff --git a/cgn_flexbe/cgn_flexbe_states/tests/example_state.test b/cgn_flexbe/cgn_flexbe_states/tests/example_state.test new file mode 100644 index 0000000..40a0f09 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/tests/example_state.test @@ -0,0 +1,10 @@ +path: cgn_flexbe_states.example_state +class: ExampleState + +import_only: true + +params: + target_time: 2.0 + +outcome: + done diff --git a/cgn_flexbe/cgn_flexbe_states/tests/run_colcon_test.py b/cgn_flexbe/cgn_flexbe_states/tests/run_colcon_test.py new file mode 100644 index 0000000..0387d96 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/tests/run_colcon_test.py @@ -0,0 +1,62 @@ +# Copyright 2023 Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +"""Pytest testing for cgn_flexbe_states.""" + + +from flexbe_testing.py_tester import PyTester + + +class TestFlexBEStates(PyTester): + """Pytest testing for cgn_flexbe_states.""" + + # def __init__(self, *args, **kwargs): + # """Initialize unit test.""" + # super().__init__(*args, **kwargs) + + @classmethod + def setUpClass(cls): + + PyTester._package = "cgn_flexbe_states" + PyTester._tests_folder = "tests" + + PyTester.setUpClass() # Do this last after setting package and tests folder + + # The tests + def test_example_state(self): + """Run FlexBE unit test given .test file.""" + self.run_test("example_state") + + def test_example_action_state(self): + """ + Run FlexBE unit test given .test file. + + This test requires longer wait than normal + """ + self.run_test("example_action_state", timeout_sec=2.0, max_cnt=5000) diff --git a/cgn_flexbe/cgn_flexbe_states/tests/run_tests.launch.py b/cgn_flexbe/cgn_flexbe_states/tests/run_tests.launch.py new file mode 100644 index 0000000..29961ec --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_states/tests/run_tests.launch.py @@ -0,0 +1,64 @@ +# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""cgn_flexbe_states testing.""" + +from os.path import join + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Flexbe_states testing.""" + flexbe_testing_dir = get_package_share_directory('flexbe_testing') + flexbe_states_test_dir = get_package_share_directory('cgn_flexbe_states') + + path = join(flexbe_states_test_dir, "tests") + + # The tests + testcases = "" + testcases += join(path, "example_state.test") + "\n" + testcases += join(path, "example_action_state.test") + "\n" + + return LaunchDescription([ + DeclareLaunchArgument("pkg", default_value="cgn_flexbe_states"), + DeclareLaunchArgument("testcases", default_value=testcases), + DeclareLaunchArgument("compact_format", default_value='true'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(join(flexbe_testing_dir, "launch", "flexbe_testing.launch.py")), + launch_arguments={ + 'package': LaunchConfiguration("pkg"), + 'compact_format': LaunchConfiguration("compact_format"), + 'testcases': LaunchConfiguration("testcases"), + }.items() + ) + ]) diff --git a/cgn_flexbe/cgn_flexbe_utilities/CMakeLists.txt b/cgn_flexbe/cgn_flexbe_utilities/CMakeLists.txt new file mode 100644 index 0000000..c81ec48 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_utilities/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.8) +project(cgn_flexbe_utilities) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(pcl_msgs REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(PCL REQUIRED) +find_package(gpd_ros REQUIRED) +find_package(builtin_interfaces REQUIRED) + +# Generate services (⚠️ gpd_ros REMOVED from DEPENDENCIES) +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/MoveToPose.srv" + DEPENDENCIES + sensor_msgs + pcl_msgs + geometry_msgs + std_msgs +) + +include_directories( + ${PCL_INCLUDE_DIRS} +) + + +add_executable(move_to_pose_service src/move_to_pose_service.cpp) +ament_target_dependencies(move_to_pose_service + rclcpp + geometry_msgs + moveit_ros_planning_interface + moveit_msgs +) + + +add_executable(reach_to_grasp_service src/reach_to_grasp_service.cpp) +ament_target_dependencies(reach_to_grasp_service + rclcpp + geometry_msgs + visualization_msgs + moveit_ros_planning_interface + moveit_msgs + tf2 + tf2_geometry_msgs + std_srvs +) +target_link_libraries(reach_to_grasp_service ${PCL_LIBRARIES}) + +install(TARGETS + move_to_pose_service + reach_to_grasp_service + DESTINATION lib/${PROJECT_NAME} +) + +# Link generated service headers +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(move_to_pose_service ${cpp_typesupport_target}) +target_link_libraries(reach_to_grasp_service ${cpp_typesupport_target}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/cgn_flexbe/cgn_flexbe_utilities/package.xml b/cgn_flexbe/cgn_flexbe_utilities/package.xml new file mode 100644 index 0000000..0f738d1 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_utilities/package.xml @@ -0,0 +1,38 @@ + + + + cgn_flexbe_utilities + 0.0.0 + TODO: Package description + csrobot + TODO: License declaration + + ament_cmake + rosidl_default_generators + + sensor_msgs + geometry_msgs + std_msgs + rclcpp + tf2 + tf2_ros + tf2_sensor_msgs + tf2_geometry_msgs + pcl_msgs + pcl_conversions + moveit_ros_planning_interface + moveit_msgs + gpd_ros + + + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/cgn_flexbe/cgn_flexbe_utilities/src/move_to_pose_service.cpp b/cgn_flexbe/cgn_flexbe_utilities/src/move_to_pose_service.cpp new file mode 100644 index 0000000..e529776 --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_utilities/src/move_to_pose_service.cpp @@ -0,0 +1,144 @@ +#include +#include +#include +#include "cgn_flexbe_utilities/srv/move_to_pose.hpp" + +#include + +class PosePlanner : public rclcpp::Node +{ +public: + PosePlanner() + : Node("move_to_pose_service") + { + this->declare_parameter("planning_group", "arm"); + + std::string group_name; + this->get_parameter("planning_group", group_name); + + move_group_ = std::make_shared( + std::shared_ptr(this, [](rclcpp::Node*){}), group_name); + + service_ = this->create_service( + "move_to_pose", + std::bind(&PosePlanner::handle_request, this, std::placeholders::_1, std::placeholders::_2) + ); + + RCLCPP_INFO(this->get_logger(), "MoveToPose service ready (group: %s)", group_name.c_str()); + } + +private: + rclcpp::Service::SharedPtr service_; + std::shared_ptr move_group_; + + void handle_request( + const std::shared_ptr req, + std::shared_ptr res) + { + try + { + move_group_->setPoseTarget(req->target_pose); + + auto marker_pub_ = this->create_publisher("debug_goal_marker", 1); + + visualization_msgs::msg::Marker m; + m.header.stamp = this->now(); + m.header.frame_id = "simple_pedestal"; + // m.header.frame_id = "panda_link0"; + m.ns = "move_to_pose_goal"; + m.id = 0; + m.type = visualization_msgs::msg::Marker::SPHERE; + m.action = visualization_msgs::msg::Marker::ADD; + m.pose = req->target_pose; // exactly what you're planning to + m.scale.x = m.scale.y = m.scale.z = 0.03; + m.color.a = 1.0; m.color.r = 0.1; m.color.g = 0.8; m.color.b = 0.2; + marker_pub_->publish(m); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + moveit::core::MoveItErrorCode planning_result = move_group_->plan(plan); + + if (planning_result != moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_WARN(this->get_logger(), "Planning failed."); + res->success = false; + return; + } + + RCLCPP_INFO(this->get_logger(), "Planning succeeded. Executing..."); + + moveit::core::MoveItErrorCode exec_result = move_group_->execute(plan); + + if (exec_result == moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Motion to pose succeeded."); + res->success = true; // comment out if adding additional pick and place later in this script + } + else + { + RCLCPP_ERROR(this->get_logger(), "Motion execution failed."); + res->success = false; + } + + // Additional step test: Lift vertically in base frame (planning frame) + // bool flg_liftUp = moveInBaseZ(req, 0.05); + // if (flg_liftUp) + // { + // res->success = true; + // RCLCPP_INFO(this->get_logger(), "Motion to lift object succeeded."); + // return; + // } + // else + // { + // res->success = false; + // RCLCPP_ERROR(this->get_logger(),"Failed to lift object."); + // return; + // } + + } + catch (const std::exception &e) + { + RCLCPP_ERROR(this->get_logger(), "Exception during planning or execution: %s", e.what()); + res->success = false; + } + } + + bool moveInBaseZ(const std::shared_ptr req, double dz) + { + const std::string ee_link = move_group_->getEndEffectorLink(); + // geometry_msgs::msg::PoseStamped current = move_group_->getCurrentPose(ee_link); + + geometry_msgs::msg::Pose target = req->target_pose; // current.pose; + target.position.z += dz; // base frame Z + + RCLCPP_INFO(this->get_logger(), "target.position.z = %f", target.position.z); + + move_group_->setPoseTarget(target, ee_link); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto result = move_group_->plan(plan); + if (result != moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_WARN(this->get_logger(), "Planning base-Z lift failed."); + return false; + } + + auto exec = move_group_->execute(plan); + if (exec != moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Execution of base-Z lift failed."); + return false; + } + + return true; + } + +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/cgn_flexbe/cgn_flexbe_utilities/src/reach_to_grasp_service.cpp b/cgn_flexbe/cgn_flexbe_utilities/src/reach_to_grasp_service.cpp new file mode 100644 index 0000000..afd4bdd --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_utilities/src/reach_to_grasp_service.cpp @@ -0,0 +1,319 @@ +#include +#include + +#include +#include + +#include +#include +#include + +#include "cgn_flexbe_utilities/srv/move_to_pose.hpp" +using MoveToPoseSrv = cgn_flexbe_utilities::srv::MoveToPose; + + +#include + +class ReachToGraspNode : public rclcpp::Node +{ +public: + ReachToGraspNode() + : Node("reach_to_grasp_service") + { + this->declare_parameter("planning_group", "panda_arm"); + this->declare_parameter("gripper_group", "panda_hand"); + + std::string arm_group_name, gripper_group_name; + this->get_parameter("planning_group", arm_group_name); + this->get_parameter("gripper_group", gripper_group_name); + + RCLCPP_INFO(this->get_logger(), "ReachToGraspNode using arm group: %s, hand group: %s", + arm_group_name.c_str(), gripper_group_name.c_str()); + + arm_group_ = std::make_shared( + std::shared_ptr(this, [](rclcpp::Node*){}), arm_group_name); + + gripper_group_ = std::make_shared( + std::shared_ptr(this, [](rclcpp::Node*){}), gripper_group_name); + + // For Panda hand (two finger joints, mirrored with single value): directly set open/close values + open_gripper_joint_values_.clear(); + close_gripper_joint_values_.clear(); + + // Configure gripper open/close joint targets for Panda hand + // const std::vector& joint_names = gripper_group_->getJointNames(); + const auto& joint_names = gripper_group_->getJointNames(); + // std::size_t nj = joint_names.size(); + + open_gripper_joint_values_.assign(joint_names.size(), 0.04); + close_gripper_joint_values_.assign(joint_names.size(), 0.015); + + + // // Make sure we have the right number of joints + // // NOTE: replace with gripper_command action server - 40 for effort, position for 0.04 + + service_ = this->create_service( + "/reach_to_grasp", + std::bind(&ReachToGraspNode::handle_request, this, + std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "ReachToGrasp service '/reach_to_grasp' is ready."); + } + +private: + // rclcpp::Service::SharedPtr service_; + rclcpp::Service::SharedPtr service_; + std::shared_ptr arm_group_; + std::shared_ptr gripper_group_; + + std::vector open_gripper_joint_values_; + std::vector close_gripper_joint_values_; + + + geometry_msgs::msg::PoseStamped target_ps; + + void handle_request(const std::shared_ptr req, + std::shared_ptr res) + { + try + { + + // Use the requested pose as grasp pose in base frame + const geometry_msgs::msg::Pose& grasp_pose = req->target_pose; + + geometry_msgs::msg::PoseStamped grasp_ps; + // grasp_ps.header.frame_id = "panda_link0"; + grasp_ps.header.frame_id = arm_group_->getPlanningFrame(); + grasp_ps.pose = grasp_pose; + + // 1) Open gripper + if (!setGripper(open_gripper_joint_values_)) + { + res->success = false; + // res->message = "Failed to open gripper."; + return; + } + + // (Temp) Move down vertically in base frame (planning frame) + if (!moveInBaseZ(grasp_ps, -0.05)) + { + res->success = false; + // res->message = "Failed to lift object."; + return; + } + + // 2) Move EEF along its own +Z axis by 0.01m + if (!moveAlongEndEffectorZ(target_ps, 0.12)) + { + res->success = false; + // res->message = "Failed to move in base-Y."; + return; + } + + // 3) Close gripper + if (!setGripper(close_gripper_joint_values_)) + { + res->success = false; + // res->message = "Failed to close gripper."; + return; + } + + // 4) Lift vertically in base frame (planning frame) + if (!moveInBaseZ(target_ps, 0.1)) + { + res->success = false; + // res->message = "Failed to lift object."; + return; + } + + // 5) Open gripper to drop + if (!setGripper(open_gripper_joint_values_)) + { + res->success = false; + // res->message = "Failed to reopen gripper."; + return; + } + + res->success = true; + // res->message = "Grasp sequence executed successfully."; + } + catch (const std::exception& e) + { + RCLCPP_ERROR(this->get_logger(), "Exception in grasp sequence: %s", e.what()); + res->success = false; + // res->message = std::string("Exception: ") + e.what(); + } + } + + bool setGripper(const std::vector& joint_values) + { + + // 1) Get current state with a small timeout + moveit::core::RobotStatePtr current_state = gripper_group_->getCurrentState(1.0); + + if (current_state) + { + // Enforce bounds in case of tiny numerical violations + current_state->enforceBounds(); + gripper_group_->setStartState(*current_state); + RCLCPP_INFO(this->get_logger(), + "openGripper: Using current robot state as start state."); + } + else + { + // Fallback: construct a default state from the robot model + RCLCPP_WARN(this->get_logger(), + "openGripper: No current robot state available within 1.0s, " + "falling back to default robot state."); + + moveit::core::RobotState default_state(gripper_group_->getRobotModel()); + default_state.setToDefaultValues(); + default_state.enforceBounds(); + gripper_group_->setStartState(default_state); + } + + // 2) set execution tolerances for gripper closing + gripper_group_->setPlanningTime(5.0); + gripper_group_->setMaxVelocityScalingFactor(0.5); + gripper_group_->setMaxAccelerationScalingFactor(0.5); + + // 3) Set target joint values for the open posture + gripper_group_->setJointValueTarget(joint_values); + + // 4) Plan and execute with time-out failure control to accomodate gripper closing on object + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto result = gripper_group_->plan(plan); + if (result != moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_WARN(this->get_logger(), "Gripper planning failed."); + return false; + } + auto exec = gripper_group_->execute(plan); + if (exec == moveit::core::MoveItErrorCode::SUCCESS) + { + // RCLCPP_ERROR(this->get_logger(), "Gripper execution failed."); + // return false; + RCLCPP_INFO(this->get_logger(), "Gripper execution succeeded."); + return true; + } + + // 5) Change to accomodating gripper closing on objects + if (exec == moveit::core::MoveItErrorCode::TIMED_OUT || + exec == moveit::core::MoveItErrorCode::CONTROL_FAILED) + { + RCLCPP_WARN(this->get_logger(), + "Gripper: execution returned %d (TIMED_OUT / CONTROL_FAILED). " + "Assuming object contact and treating as success.", + static_cast(exec.val)); + return true; + } + + return false; + } + + bool moveAlongEndEffectorZ( + const geometry_msgs::msg::PoseStamped& ref_pose, + double distance) + { + // geometry_msgs::msg::PoseStamped target = ref_pose; + target_ps = ref_pose; + + // Compute world direction of EE Z axis + tf2::Quaternion q; + tf2::fromMsg(target_ps.pose.orientation, q); + tf2::Matrix3x3 R(q); + + // Move along +Z_EE (or -Z_EE depending on how you set distance sign) + tf2::Vector3 dz_ee(0.0, 0.0, distance); + tf2::Vector3 dz_world = R * dz_ee; + + target_ps.pose.position.x += dz_world.x(); + target_ps.pose.position.y += dz_world.y(); + target_ps.pose.position.z += dz_world.z(); + + arm_group_->setPoseTarget(target_ps); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto code = arm_group_->plan(plan); + if (code != moveit::core::MoveItErrorCode::SUCCESS) + return false; + + code = arm_group_->execute(plan); + return code == moveit::core::MoveItErrorCode::SUCCESS; + } + + bool moveInBaseZ(const geometry_msgs::msg::PoseStamped& ref_pose, double dz) // const std::shared_ptr req, + { + const std::string ee_link = arm_group_->getEndEffectorLink(); + // geometry_msgs::msg::PoseStamped current = arm_group_->getCurrentPose(ee_link); + + // geometry_msgs::msg::Pose target = current.pose; // req->target_pose; // + + target_ps = ref_pose; + target_ps.pose.position.z += dz; // base frame Z + + RCLCPP_INFO(this->get_logger(), "target_ps.pose.position.z = %f", target_ps.pose.position.z); + + arm_group_->setPoseTarget(target_ps, ee_link); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto result = arm_group_->plan(plan); + if (result != moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_WARN(this->get_logger(), "Planning base-Z lift failed."); + return false; + } + + auto exec = arm_group_->execute(plan); + if (exec != moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Execution of base-Z lift failed."); + return false; + } + + return true; + } + + + bool moveInBaseY(const geometry_msgs::msg::PoseStamped& ref_pose, double dy) // const std::shared_ptr req, + { + const std::string ee_link = arm_group_->getEndEffectorLink(); + // geometry_msgs::msg::PoseStamped current = arm_group_->getCurrentPose(ee_link); + + // geometry_msgs::msg::Pose target = current.pose; // req->target_pose; // + // geometry_msgs::msg::PoseStamped target = ref_pose; + target_ps = ref_pose; + target_ps.pose.position.y += dy; // base frame y + + RCLCPP_INFO(this->get_logger(), "target_ps.pose.position.y = %f", target_ps.pose.position.y); + + arm_group_->setPoseTarget(target_ps, ee_link); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto result = arm_group_->plan(plan); + if (result != moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_WARN(this->get_logger(), "Planning base-Y reach failed."); + return false; + } + + auto exec = arm_group_->execute(plan); + if (exec != moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Execution of base-Y reach failed."); + return false; + } + + return true; + } +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; + +} diff --git a/cgn_flexbe/cgn_flexbe_utilities/srv/EuclideanClustering.srv b/cgn_flexbe/cgn_flexbe_utilities/srv/EuclideanClustering.srv new file mode 100644 index 0000000..199f79c --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_utilities/srv/EuclideanClustering.srv @@ -0,0 +1,8 @@ +sensor_msgs/PointCloud2 input +geometry_msgs/PoseStamped camera_pose +float32 cluster_tolerance # meters (e.g., 0.015–0.03 typical) +int32 min_cluster_size # e.g., 100 +int32 max_cluster_size # e.g., 25000 +--- +pcl_msgs/PointIndices target_cluster_indices +pcl_msgs/PointIndices[] obstacle_cluster_indices \ No newline at end of file diff --git a/cgn_flexbe/cgn_flexbe_utilities/srv/MoveToPose.srv b/cgn_flexbe/cgn_flexbe_utilities/srv/MoveToPose.srv new file mode 100644 index 0000000..7344e2a --- /dev/null +++ b/cgn_flexbe/cgn_flexbe_utilities/srv/MoveToPose.srv @@ -0,0 +1,3 @@ +geometry_msgs/Pose target_pose +--- +bool success \ No newline at end of file diff --git a/gsam_flexbe/.flake8 b/gsam_flexbe/.flake8 new file mode 100644 index 0000000..7fc980c --- /dev/null +++ b/gsam_flexbe/.flake8 @@ -0,0 +1,3 @@ +[flake8] +exclude = .git +max-line-length = 130 diff --git a/gsam_flexbe/.gitignore b/gsam_flexbe/.gitignore new file mode 100644 index 0000000..890dd3f --- /dev/null +++ b/gsam_flexbe/.gitignore @@ -0,0 +1,3 @@ +*~ +*.pyc +*_tmp.py diff --git a/gsam_flexbe/CONTRIBUTING.md b/gsam_flexbe/CONTRIBUTING.md new file mode 100644 index 0000000..0703ae9 --- /dev/null +++ b/gsam_flexbe/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any new contributions that you make to this repository will +be under the Apache 2.0 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0). diff --git a/gsam_flexbe/LICENSE b/gsam_flexbe/LICENSE new file mode 100644 index 0000000..d809660 --- /dev/null +++ b/gsam_flexbe/LICENSE @@ -0,0 +1,13 @@ +Copyright 2023 Philipp Schillinger, Christopher Newport University + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/gsam_flexbe/README.md b/gsam_flexbe/README.md new file mode 100644 index 0000000..84d093b --- /dev/null +++ b/gsam_flexbe/README.md @@ -0,0 +1,96 @@ +# FlexBE States and Behaviors for gsam + +Generic template for a behaviors repository to be used for new projects + +Modify this README as needed for your specific project details. + +Below we provide basic details, but you are free to delete or modify this README as you wish. + +---- + +This raw repository has several folders and files with the generic name `gsam`. + + +This repository is used by the FlexBE widget +[`create_repo`](https://github.com/FlexBE/flexbe_behavior_engine/blob/ros2-devel/flexbe_widget/bin/create_repo) +script to create an example project that you can build off of to add your own states and behaviors. + +Using `ros2 run flexbe_widget create_repo ` will clone this repository, +and change the relevant `gsam` text to `my_new_project_name` as needed. + +It sets up the `package.xml` files with proper FlexBE export tags. +It is maintained at version `0.0.1` as the starting point for your work. + +We have provided a license file to conform to ROS guidelines; however, you are free to replace the +`LICENSE` file, and apply whatever license you choose to states and behaviors that you create. + +This repository contains an example behavior and examples for writing your own state implementations. + +## Example States in `gsam_flexbe_states` + +Packages providing FlexBE states are identified by an export tag in the `package.xml`: + +```xml + + + ament_cmake + +``` + +* `example_state.py ` + * Example state implementation with extra console logging to show the state life cycle. + +* `example_action_state.py` + +> Note: These example states are defined with extra console logging that is useful when learning FlexBE, +> but you will typically not include so much of the `Logger.info` commands as in these examples. + +> Note: You are free to copy and modify these files to create your own files and publish under your own license terms. +> As per the existing licenses, no warranty is implied. + +## Example Behaviors in `gsam_flexbe_behaviors` + +Packages providing FlexBE behaviors are identified by an export tag in the `package.xml`: + +```xml + + + ament_cmake + +``` + + * `example_behavior_sm.py` + * Most basic example state machine + + * `example_action_behavior_sm.py` + * Uses the `ExampleActionState` with the standard action tutorials + + [Understanding ROS2 Actions](https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html) + + [Introducing Turtlesim](https://docs.ros.org/en/iron/Tutorials/Beginner-CLI-Tools/Introducing-Turtlesim/Introducing-Turtlesim.html) + + To execute the associated behavior in FlexBE, you need to first run the turtlesim node that provdes the action server + + `ros2 run turtlesim turtlesim_node` + + To display the available actions: + + `ros2 action list` + + The action is defined by: + + `/turtle1/rotate_absolute:` [`turtlesim/action/RotateAbsolute`](https://docs.ros2.org/latest/api/turtlesim/action/RotateAbsolute.html) + +Behaviors typically edited and generated by the FlexBE UI. +These generated files are stored in the root workspace `install` folder. +Presuming a `WORKSPACE_ROOT` environment variable exists, we provide a simple +[`copy_behavior`](gsam_flexbe_behaviors/bin/copy_behavior) script to copy a saved behavior +— both the Python implementation and manifest `.xml` file — +to the project source folder for long term storage. +Use `ros2 run gsam_flexbe_behavior copy_behavior` to see the usage guide. +The script should be run from this repository's base folder. + +For a Quick-start and more comprehensive introduction to FlexBE, +see the [FlexBE Turtlesim Demonstrations](https://github.com/FlexBE/flexbe_turtlesim_demo). + +# gsam_flexbe diff --git a/gsam_flexbe/gsam_flexbe/CHANGELOG.rst b/gsam_flexbe/gsam_flexbe/CHANGELOG.rst new file mode 100644 index 0000000..5e13b5b --- /dev/null +++ b/gsam_flexbe/gsam_flexbe/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gsam_flexbe +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE project \ No newline at end of file diff --git a/gsam_flexbe/gsam_flexbe/CMakeLists.txt b/gsam_flexbe/gsam_flexbe/CMakeLists.txt new file mode 100644 index 0000000..3584578 --- /dev/null +++ b/gsam_flexbe/gsam_flexbe/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.10) +project(gsam_flexbe) + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/gsam_flexbe/gsam_flexbe/package.xml b/gsam_flexbe/gsam_flexbe/package.xml new file mode 100644 index 0000000..cd5541e --- /dev/null +++ b/gsam_flexbe/gsam_flexbe/package.xml @@ -0,0 +1,24 @@ + + gsam_flexbe + 0.0.1 + + Meta-package for FlexBE enabled gsam + + TODO + TODO + BSD + TODO/issues + TODO + TODO + + ament_cmake + + flexbe_core + gsam_flexbe_behaviors + gsam_flexbe_states + + + ament_cmake + + + diff --git a/gsam_flexbe/gsam_flexbe_behaviors/CHANGELOG.rst b/gsam_flexbe/gsam_flexbe_behaviors/CHANGELOG.rst new file mode 100644 index 0000000..523867c --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gsam_flexbe_behaviors +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE behaviors \ No newline at end of file diff --git a/gsam_flexbe/gsam_flexbe_behaviors/CMakeLists.txt b/gsam_flexbe/gsam_flexbe_behaviors/CMakeLists.txt new file mode 100644 index 0000000..fcda57d --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.10) +project(gsam_flexbe_behaviors) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) + +install(DIRECTORY + manifest + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + config + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS + bin/copy_behavior + DESTINATION lib/${PROJECT_NAME} +) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +ament_package() diff --git a/gsam_flexbe/gsam_flexbe_behaviors/bin/copy_behavior b/gsam_flexbe/gsam_flexbe_behaviors/bin/copy_behavior new file mode 100755 index 0000000..551726f --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/bin/copy_behavior @@ -0,0 +1,102 @@ +#!/bin/bash + +# Copyright 2025 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +# WARNING: Use at your own risk, this script does not protect against overwriting files, or mis-naming + +if [ $# -lt 1 ]; then + echo -e "\e[93mThis is an extremely simple script designed to copy behaviors\033[0m" + echo -e "\e[93mfrom the install folder to the src folder assumed to be a git repo\033[0m" + echo -e "\e[93mfor storage.\033[0m" + echo "" + echo -e "\e[93mIt requires a WORKSPACE_ROOT environment variable assuming a standard \033[0m" + echo -e "\e[93m \${WORKSPACE_ROOT}/install and \${WORKSPACE_ROOT}/src layout.\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/gsam\033[0m" + echo "" + echo -e "\e[93m Usage: ros2 run gsam_flexbe_behaviors copy_behavior BEHAVIOR_FILE_BASE \033[0m" + echo -e "\e[93m where BEHAVIOR_FILE_BASE_NAME.xml is the saved manifest name\033[0m" + echo -e "\e[93m OPTIONAL_BEHAVIOR_PACKAGE defaults to 'gsam_flexbe_behaviors'\033[0m" + echo "" + echo -e "\e[93m WARNING: Use at your own risk, this script does not protect against overwriting files or mis-naming\033[0m" + exit 2 +fi + + +beh=$(echo "$1" | cut -f 1 -d '.') # include only the base file name is someone pastes .xml name +pack="gsam_flexbe_behaviors" # Default name uses this behaviors package +if [ $# -eq 2 ]; then + pack="$2" + echo "Using specified package '${pack}'!" +fi + +# A few basic checks before attempting to copy +if [ ! -d "${pack}" ]; then + echo "" + echo -e "\e[91m Package '${pack}' does not exist under current directory '${PWD}' !\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/gsam\033[0m" + exit +fi + +if [ ! -d "${pack}/${pack}" ]; then + echo "" + echo -e "\e[91m Behavior implementation folder '${pack}/${pack}' does not exist under current directory '${PWD}' !\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/gsam\033[0m" + exit +fi + +PYVER=$(python3 -c "import sys; print(f'{sys.version_info[0]}.{sys.version_info[1]}')") + +if [ ! -f "${WORKSPACE_ROOT}/install/${pack}/lib/${PYVER}/site-packages/${pack}/${beh}_sm.py" ]; then + echo "" + echo -e "\e[91m Behavior '${beh}' implementation does not exist in install folder for '${pack}' package!\033[0m" + echo "" + echo "Available behavior manifests in '${pack}' :" + ls ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest + echo "" + echo -e "\e[93mConfirm behavior and package names!\033[0m" + exit +fi + + +# Begin the work +echo "Copying '${beh}' implementation to '${pack}' under '${PWD}' ..." +cp ${WORKSPACE_ROOT}/install/${pack}/local/lib/${PYVER}/site-packages/${pack}/${beh}_sm.py ./${pack}/${pack}/${beh}_sm.py + +if test $? -eq 0; then + echo "Copying '${beh}.xml' manifest to '${pack}' ..." + + cp ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest/${beh}.xml ./${pack}/manifest/${beh}.xml + if test $? -eq 0; then + echo "" + echo -e "\e[92mDone copying behavior '${beh}' to '${pack}'!\033[0m" + echo "Do not forget to git commit and git push any changes." + exit + else + echo "" + echo -e "\e[91m Failed to copy behavior '${beh}.xml' manifest after copying behavior implementation!\033[0m" + exit + fi +else + echo "" + echo -e "\e[91m Failed to copy behavior '${beh}'!\033[0m" + exit +fi \ No newline at end of file diff --git a/gsam_flexbe/gsam_flexbe_behaviors/config/example.yaml b/gsam_flexbe/gsam_flexbe_behaviors/config/example.yaml new file mode 100644 index 0000000..e3f24de --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/config/example.yaml @@ -0,0 +1 @@ +# You can use this folder to place general config files diff --git a/gsam_flexbe/gsam_flexbe_behaviors/gsam_flexbe_behaviors/__init__.py b/gsam_flexbe/gsam_flexbe_behaviors/gsam_flexbe_behaviors/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gsam_flexbe/gsam_flexbe_behaviors/gsam_flexbe_behaviors/unseenobjclustergraspsampipeine_sm.py b/gsam_flexbe/gsam_flexbe_behaviors/gsam_flexbe_behaviors/unseenobjclustergraspsampipeine_sm.py new file mode 100644 index 0000000..a97cb40 --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/gsam_flexbe_behaviors/unseenobjclustergraspsampipeine_sm.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2026 Huajing Zhao +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +# TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define UnseenObjClusterGraspSamPipeine. + +A perception-to-action pipeline which employs unseen-object-clustering for +object segmentation from a +scene and choose a target for grasping, then GraspSAM for grasp +planning and OMPL for manipulation via MoveIt + +Created on Feb 16 2026 +@author: Huajing Zhao +""" + + +from gsam_flexbe_states.graspsam_service_state import GraspSAMServiceState +from compare_flexbe_states.move_to_pose_service_state import MoveToPoseServiceState +from compare_flexbe_states.select_instance_to_cgn_indices_state import SelectInstanceToSceneNameState +from uoc_flexbe_states.unseen_obj_seg_rgbd_service_state import UnseenObjSegRGBDServiceState +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class UnseenObjClusterGraspSamPipeineSM(Behavior): + """ + Define UnseenObjClusterGraspSamPipeine. + + A perception-to-action pipeline which employs unseen-object-clustering for + object segmentation from a + scene and choose a target for grasping, then GraspSAM for grasp + planning and OMPL for manipulation via MoveIt + """ + + def __init__(self, node): + super().__init__() + self.name = 'UnseenObjClusterGraspSamPipeine' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:1154 y:332, x:141 y:356 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['im_name']) + _state_machine.userdata.im_name = 'from_rgbd' + _state_machine.userdata.seg_json = {} + _state_machine.userdata.result_dir = '' + _state_machine.userdata.instance_ids_2d = [] + _state_machine.userdata.instance_id_list = [] + _state_machine.userdata.target_instance_id = 0 + _state_machine.userdata.scene_name = 'scene_from_ucn' + _state_machine.userdata.message = '' + _state_machine.userdata.grasp_target_poses = [] + _state_machine.userdata.grasp_scores = [] + _state_machine.userdata.grasp_samples = [] + _state_machine.userdata.grasp_object_ids = [] + _state_machine.userdata.grasp_index = 0 + _state_machine.userdata.dataset_name = 'from_rgbd' + _state_machine.userdata.checkpoint_path = 'pretrained_checkpoint/mobile_sam.pt' + _state_machine.userdata.dataset_root = './datasets/sample_scene_ucn' + _state_machine.userdata.sam_encoder_type = 'vit_t' + _state_machine.userdata.no_grasps = 10 + _state_machine.userdata.seen_set = False + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:30 y:40 + OperatableStateMachine.add('UnseenObjSegRGBD', + UnseenObjSegRGBDServiceState(service_name='/segmentation_rgbd', + service_timeout=5.0, + default_im_name='from_rgbd', + background_id=0), + transitions={'finished': 'SelectInstanceToScene', + 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'im_name': 'im_name', + 'seg_json': 'seg_json', + 'result_dir': 'result_dir', + 'instance_ids_2d': 'instance_ids_2d', + 'instance_id_list': 'instance_id_list', + 'instance_masks': 'instance_masks', + 'message': 'message'}) + + # x:812 y:38 + OperatableStateMachine.add('GraspSAM', + GraspSAMServiceState(service_name='/run_graspsam', + dataset_root='./datasets/sample_scene_ucn', + dataset_name='from_rgbd', + checkpoint_path='pretrained_checkpoint/mobile_sam.pt', + sam_encoder_type='vit_t', + no_grasps=10, + timeout=2.0, + seen_set=False, + seen_set_default=False), + transitions={'done': 'MoveOMPL', 'failed': 'failed'}, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'dataset_root': 'dataset_root', + 'dataset_name': 'dataset_name', + 'checkpoint_path': 'checkpoint_path', + 'sam_encoder_type': 'sam_encoder_type', + 'no_grasps': 'no_grasps', + 'seen_set': 'seen_set', + 'output_dir': 'output_dir', + 'grasps': 'grasps', + 'grasp_target_poses': 'grasp_target_poses', + 'message': 'message'}) + + # x:1087 y:38 + OperatableStateMachine.add('MoveOMPL', + MoveToPoseServiceState(timeout_sec=5.0, + service_name='/move_to_pose'), + transitions={'done': 'finished', + 'next': 'MoveOMPL', + 'failed': 'failed' # 666 281 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, + 'next': Autonomy.Off, + 'failed': Autonomy.Off}, + remapping={'grasp_poses': 'grasp_target_poses', + 'grasp_index': 'grasp_index'}) + + # x:419 y:38 + OperatableStateMachine.add('SelectInstanceToScene', + SelectInstanceToSceneNameState(default_scene_name='scene_from_ucn'), + transitions={'finished': 'GraspSAM', 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'seg_json': 'seg_json', + 'result_dir': 'result_dir', + 'instance_ids_2d': 'instance_ids_2d', + 'instance_id_list': 'instance_id_list', + 'im_name': 'im_name', + 'target_instance_id': 'target_instance_id', + 'scene_name': 'scene_name', + 'message': 'message'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/gsam_flexbe/gsam_flexbe_behaviors/manifest/unseenobjclustergraspsampipeine.xml b/gsam_flexbe/gsam_flexbe_behaviors/manifest/unseenobjclustergraspsampipeine.xml new file mode 100644 index 0000000..090d46b --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/manifest/unseenobjclustergraspsampipeine.xml @@ -0,0 +1,21 @@ + + + + + + + Huajing Zhao + Feb 16 2026 + + A perception-to-action pipeline which employs unseen-object-clustering for + object segmentation from a + scene and choose a target for grasping, then GraspSAM for grasp + planning and OMPL for manipulation via MoveIt + + + + + + + + diff --git a/gsam_flexbe/gsam_flexbe_behaviors/package.xml b/gsam_flexbe/gsam_flexbe_behaviors/package.xml new file mode 100644 index 0000000..5d02aa2 --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/package.xml @@ -0,0 +1,30 @@ + + + + gsam_flexbe_behaviors + 0.0.1 + + gsam_flexbe_behaviors references all implemented behaviors. + + TODO + TODO + BSD + + rclpy + flexbe_core + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_cmake + ament_cmake_python + + + + ament_cmake + + diff --git a/gsam_flexbe/gsam_flexbe_behaviors/resource/gsam_flexbe_behaviors b/gsam_flexbe/gsam_flexbe_behaviors/resource/gsam_flexbe_behaviors new file mode 100644 index 0000000..e69de29 diff --git a/gsam_flexbe/gsam_flexbe_behaviors/setup.cfg b/gsam_flexbe/gsam_flexbe_behaviors/setup.cfg new file mode 100644 index 0000000..c1901e7 --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/gsam_flexbe_behaviors +[install] +install_scripts=$base/lib/gsam_flexbe_behaviors diff --git a/gsam_flexbe/gsam_flexbe_behaviors/setup.py b/gsam_flexbe/gsam_flexbe_behaviors/setup.py new file mode 100644 index 0000000..2377b99 --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_behaviors/setup.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python +from setuptools import setup + +package_name = 'gsam_flexbe_behaviors' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='phil', + maintainer_email='philsplus@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_behavior_sm = gsam_flexbe_behaviors.example_behavior_sm', + ], + }, +) diff --git a/gsam_flexbe/gsam_flexbe_states/CHANGELOG.rst b/gsam_flexbe/gsam_flexbe_states/CHANGELOG.rst new file mode 100644 index 0000000..818b7ba --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gsam_flexbe_states +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE states \ No newline at end of file diff --git a/gsam_flexbe/gsam_flexbe_states/gsam_flexbe_states/__init__.py b/gsam_flexbe/gsam_flexbe_states/gsam_flexbe_states/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gsam_flexbe/gsam_flexbe_states/gsam_flexbe_states/graspsam_service_state.py b/gsam_flexbe/gsam_flexbe_states/gsam_flexbe_states/graspsam_service_state.py new file mode 100644 index 0000000..ff7991e --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/gsam_flexbe_states/graspsam_service_state.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyServiceCaller + +from graspsam_ros2.srv import RunGraspSAM + + +class GraspSAMServiceState(EventState): + """ + Calls GraspSAM ROS2 service /run_graspsam and waits for response (synchronous call). + + -- service_name string Service name (default: '/run_graspsam') + -- timeout float Seconds to wait for service availability (default: 2.0) + -- seen_set_default bool Default if userdata.seen_set is missing/None (default: False) + + ># dataset_root string + ># dataset_name string + ># checkpoint_path string + ># sam_encoder_type string + ># no_grasps int + ># seen_set bool (optional; falls back to seen_set_default) + + #> output_dir string + #> grasps graspsam_ros2/Grasp[] + #> message string + + <= done + <= failed + """ + + def __init__(self, + service_name='/run_graspsam', + dataset_root='', + dataset_name='', + checkpoint_path='', + sam_encoder_type='vit_t', + no_grasps=10, + timeout=2.0, + seen_set=False, + seen_set_default = False, + **_ignored_kwargs): + super().__init__( + outcomes=['done', 'failed'], + input_keys=[ + 'dataset_root', + 'dataset_name', + 'checkpoint_path', + 'sam_encoder_type', + 'no_grasps', + 'seen_set', + ], + output_keys=['output_dir', 'grasps', 'grasp_target_poses', 'message'] + ) + + self._service_name = service_name + self._timeout = float(timeout) + self._seen_set_default = bool(seen_set_default) + + self._srv = ProxyServiceCaller({self._service_name: RunGraspSAM}) + + self._sent = False + self._success = False + + def on_enter(self, userdata): + self._sent = False + self._success = False + userdata.output_dir = '' + userdata.grasps = [] + userdata.grasp_target_poses = [] + userdata.message = '' + + # FlexBE/ProxyServiceCaller expects a FLOAT seconds wait_duration + if not self._srv.is_available(self._service_name): + Logger.loginfo( + f"[GraspSAMServiceState] Waiting for service '{self._service_name}' (timeout={self._timeout}s)..." + ) + if not self._srv.wait_for_service(self._service_name, wait_duration=self._timeout): + userdata.message = f"Service '{self._service_name}' not available after {self._timeout}s." + Logger.logerr(f"[GraspSAMServiceState] {userdata.message}") + return + + # Build request + request = RunGraspSAM.Request() + request.dataset_root = str(userdata.dataset_root) + request.dataset_name = str(userdata.dataset_name) + request.checkpoint_path = str(userdata.checkpoint_path) + request.sam_encoder_type = str(userdata.sam_encoder_type) + request.no_grasps = int(userdata.no_grasps) + + # default seen_set=False if missing/None + seen = getattr(userdata, 'seen_set', None) + request.seen_set = self._seen_set_default if seen is None else bool(seen) + + Logger.loginfo( + "[GraspSAMServiceState] Calling {} with dataset_root={}, dataset_name={}, checkpoint={}, encoder={}, no_grasps={}, seen_set={}".format( + self._service_name, + request.dataset_root, + request.dataset_name, + request.checkpoint_path, + request.sam_encoder_type, + request.no_grasps, + request.seen_set + ) + ) + + # Synchronous call (CGN-style) + try: + resp = self._srv.call(self._service_name, request) + self._sent = True + except Exception as e: + userdata.message = f"Service call exception: {e}" + Logger.logerr(f"[GraspSAMServiceState] {userdata.message}") + return + + if resp is None: + userdata.message = "Service call returned None." + Logger.logerr(f"[GraspSAMServiceState] {userdata.message}") + return + + userdata.message = resp.message + userdata.output_dir = resp.output_dir + userdata.grasps = list(resp.grasps) if resp.grasps is not None else [] + + # Extract base-frame target poses (Pose list) from grasps + poses = [] + for g in userdata.grasps: + if hasattr(g, 'pose_base'): + pb = g.pose_base + # defensive: skip uninitialized/default poses if needed + poses.append(pb) + userdata.grasp_target_poses = poses + + self._success = bool(resp.success) + + if self._success: + Logger.loginfo( + f"[GraspSAMServiceState] Success. Parsed grasps: {len(userdata.grasps)} output_dir: {userdata.output_dir}" + ) + else: + Logger.logerr(f"[GraspSAMServiceState] Failure: {userdata.message}") + + def execute(self, userdata): + # If we never even sent successfully, fail + if not self._sent: + return 'failed' + return 'done' if self._success else 'failed' diff --git a/gsam_flexbe/gsam_flexbe_states/package.xml b/gsam_flexbe/gsam_flexbe_states/package.xml new file mode 100644 index 0000000..bdd786a --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/package.xml @@ -0,0 +1,38 @@ + + + + gsam_flexbe_states + 0.0.1 + + gsam_flexbe_states provides a collection of custom states. + Feel free to add new states. + + TODO + TODO + BSD + + rclpy + flexbe_core + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + turtlesim + + ament_cmake + ament_cmake_python + + + + ament_python + + + diff --git a/gsam_flexbe/gsam_flexbe_states/resource/gsam_flexbe_states b/gsam_flexbe/gsam_flexbe_states/resource/gsam_flexbe_states new file mode 100644 index 0000000..e69de29 diff --git a/gsam_flexbe/gsam_flexbe_states/setup.cfg b/gsam_flexbe/gsam_flexbe_states/setup.cfg new file mode 100644 index 0000000..643e73f --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/gsam_flexbe_states +[install] +install_scripts=$base/lib/gsam_flexbe_states diff --git a/gsam_flexbe/gsam_flexbe_states/setup.py b/gsam_flexbe/gsam_flexbe_states/setup.py new file mode 100644 index 0000000..d21d782 --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/setup.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python + +from glob import glob +from setuptools import setup +from setuptools import find_packages + +PACKAGE_NAME = 'gsam_flexbe_states' + +setup( + name=PACKAGE_NAME, + version='0.0.1', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + ('share/' + PACKAGE_NAME + "/tests", glob('tests/*.test')), + ('share/' + PACKAGE_NAME + "/launch", glob('tests/*.launch.py')), + ], + + install_requires=['setuptools'], + zip_safe=True, + maintainer='TODO', + maintainer_email='TODO@TODO.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_action_state = gsam_flexbe_states.example_action_state', + 'example_state = gsam_flexbe_states.example_state', + ], + }, +) diff --git a/gsam_flexbe/gsam_flexbe_states/tests/example_action_state.test b/gsam_flexbe/gsam_flexbe_states/tests/example_action_state.test new file mode 100644 index 0000000..66a7817 --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/tests/example_action_state.test @@ -0,0 +1,14 @@ +path: gsam_flexbe_states.example_action_state +class: ExampleActionState + +import_only: false + +params: + action_topic: 'action_is_not_available' + timeout: 0.5 + +input: + angle: 2. + +outcome: + failed diff --git a/gsam_flexbe/gsam_flexbe_states/tests/example_state.test b/gsam_flexbe/gsam_flexbe_states/tests/example_state.test new file mode 100644 index 0000000..b0274fc --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/tests/example_state.test @@ -0,0 +1,10 @@ +path: gsam_flexbe_states.example_state +class: ExampleState + +import_only: true + +params: + target_time: 2.0 + +outcome: + done diff --git a/gsam_flexbe/gsam_flexbe_states/tests/run_colcon_test.py b/gsam_flexbe/gsam_flexbe_states/tests/run_colcon_test.py new file mode 100644 index 0000000..c916b5e --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/tests/run_colcon_test.py @@ -0,0 +1,62 @@ +# Copyright 2023 Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +"""Pytest testing for gsam_flexbe_states.""" + + +from flexbe_testing.py_tester import PyTester + + +class TestFlexBEStates(PyTester): + """Pytest testing for gsam_flexbe_states.""" + + # def __init__(self, *args, **kwargs): + # """Initialize unit test.""" + # super().__init__(*args, **kwargs) + + @classmethod + def setUpClass(cls): + + PyTester._package = "gsam_flexbe_states" + PyTester._tests_folder = "tests" + + PyTester.setUpClass() # Do this last after setting package and tests folder + + # The tests + def test_example_state(self): + """Run FlexBE unit test given .test file.""" + self.run_test("example_state") + + def test_example_action_state(self): + """ + Run FlexBE unit test given .test file. + + This test requires longer wait than normal + """ + self.run_test("example_action_state", timeout_sec=2.0, max_cnt=5000) diff --git a/gsam_flexbe/gsam_flexbe_states/tests/run_tests.launch.py b/gsam_flexbe/gsam_flexbe_states/tests/run_tests.launch.py new file mode 100644 index 0000000..ff76838 --- /dev/null +++ b/gsam_flexbe/gsam_flexbe_states/tests/run_tests.launch.py @@ -0,0 +1,64 @@ +# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""gsam_flexbe_states testing.""" + +from os.path import join + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Flexbe_states testing.""" + flexbe_testing_dir = get_package_share_directory('flexbe_testing') + flexbe_states_test_dir = get_package_share_directory('gsam_flexbe_states') + + path = join(flexbe_states_test_dir, "tests") + + # The tests + testcases = "" + testcases += join(path, "example_state.test") + "\n" + testcases += join(path, "example_action_state.test") + "\n" + + return LaunchDescription([ + DeclareLaunchArgument("pkg", default_value="gsam_flexbe_states"), + DeclareLaunchArgument("testcases", default_value=testcases), + DeclareLaunchArgument("compact_format", default_value='true'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(join(flexbe_testing_dir, "launch", "flexbe_testing.launch.py")), + launch_arguments={ + 'package': LaunchConfiguration("pkg"), + 'compact_format': LaunchConfiguration("compact_format"), + 'testcases': LaunchConfiguration("testcases"), + }.items() + ) + ]) diff --git a/uoc_flexbe/.flake8 b/uoc_flexbe/.flake8 new file mode 100644 index 0000000..7fc980c --- /dev/null +++ b/uoc_flexbe/.flake8 @@ -0,0 +1,3 @@ +[flake8] +exclude = .git +max-line-length = 130 diff --git a/uoc_flexbe/.gitignore b/uoc_flexbe/.gitignore new file mode 100644 index 0000000..890dd3f --- /dev/null +++ b/uoc_flexbe/.gitignore @@ -0,0 +1,3 @@ +*~ +*.pyc +*_tmp.py diff --git a/uoc_flexbe/CONTRIBUTING.md b/uoc_flexbe/CONTRIBUTING.md new file mode 100644 index 0000000..0703ae9 --- /dev/null +++ b/uoc_flexbe/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any new contributions that you make to this repository will +be under the Apache 2.0 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0). diff --git a/uoc_flexbe/LICENSE b/uoc_flexbe/LICENSE new file mode 100644 index 0000000..d809660 --- /dev/null +++ b/uoc_flexbe/LICENSE @@ -0,0 +1,13 @@ +Copyright 2023 Philipp Schillinger, Christopher Newport University + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/uoc_flexbe/README.md b/uoc_flexbe/README.md new file mode 100644 index 0000000..1230a56 --- /dev/null +++ b/uoc_flexbe/README.md @@ -0,0 +1,317 @@ +# FlexBE States and Behaviors for Unseen Object Clustering + +FlexBE service states and behavior pipelines for integrating **Unseen Object Clustering (UOC)** into ROS 2 manipulation workflows. + +This repository provides: + +- **FlexBE service states** for calling UOC ROS 2 segmentation servers +- **FlexBE behavior pipelines** that use UOC as the perception front-end before grasp planning +- Integrated pipelines for: + - **UOC + Contact-GraspNet (RGB-D)** (recommended) + - **UOC + GraspSAM (RGB-D)** (recommended) +- An experimental/test state for **UOC with point cloud input** (not recommended) + +## Overview + +This package is a **FlexBE-based perception integration layer** centered on UOC. + +It includes two UOC segmentation service states: + +1. **`unseen_obj_seg_rgbd_service_state.py`** (recommended) + - Calls the UOC ROS 2 server using **RGB-D inputs** + - This is the intended and reliable usage mode for UOC in our pipeline + - Produces segmentation outputs used by downstream target selection and grasp modules + +2. **`unseen_obj_seg_cloud_service_state.py`** (experimental, not recommended) + - Calls UOC with **point cloud inputs** + - Included mainly for testing / experimentation + - In our setup, UOC performs poorly with point-cloud-only inputs + +This repository also includes two FlexBE behaviors that connect UOC to downstream grasp planners: + +- **UOC -> Contact-GraspNet (RGB-D)** +- **UOC -> GraspSAM (RGB-D)** + +## Important Recommendation + +Use **UOC with RGB-D inputs**. + +- UOC is designed to work well with RGB-D segmentation. +- The point-cloud UOC state is kept for testing, but segmentation quality is generally poor. + +Also note: + +- **Contact-GraspNet can still be used with point cloud inputs** through other perception/filtering modules (e.g., Euclidean clustering / point cloud filtering), but that is a different integration path and is generally less robust than RGB-D. +- The weak performance is specifically about **UOC + point cloud**, not all point-cloud-based pipelines in general. + +## Repository Structure + +```text +├── uoc_flexbe +│ ├── CHANGELOG.rst +│ ├── CMakeLists.txt +│ └── package.xml +├── uoc_flexbe_behaviors +│ ├── bin +│ │ └── copy_behavior +│ ├── CHANGELOG.rst +│ ├── CMakeLists.txt +│ ├── config +│ │ └── example.yaml +│ ├── manifest +│ │ ├── unseenobjclustercontactgraspnetpipeine.xml +│ │ └── unseenobjclustergraspsampipeine.xml +│ ├── package.xml +│ ├── resource +│ │ └── uoc_flexbe_behaviors +│ ├── setup.cfg +│ ├── setup.py +│ └── uoc_flexbe_behaviors +│ ├── __init__.py +│ ├── unseenobjclustercontactgraspnetpipeine_sm.py +│ └── unseenobjclustergraspsampipeine_sm.py +└── uoc_flexbe_states + ├── CHANGELOG.rst + ├── package.xml + ├── resource + │ └── uoc_flexbe_states + ├── setup.cfg + ├── setup.py + └── uoc_flexbe_states + ├── __init__.py + ├── unseen_obj_seg_cloud_service_state.py + └── unseen_obj_seg_rgbd_service_state.py +``` + +## Quick Start + +This section is tailored to the service names used by the uploaded UOC states/behaviors and the downstream integration behaviors. + +### 1) Build the workspace + +```bash +cd ~/your_ws +colcon build --symlink-install +source install/setup.bash +``` + +### 2) Setup required ROS 2 servers + +For the UOC-based pipelines, you will typically need: + +- `/segmentation_rgbd` (UOC segmentation server, recommended) +- `/get_grasps_rgbd` (Contact-GraspNet server) for the CGN pipeline +- `/run_graspsam` (GraspSAM server) for the GraspSAM pipeline +- `/move_to_pose` (MoveIt / OMPL motion execution service) + +### 3) Start FlexBE and run a behavior + +Open FlexBE App / onboard execution and run one of: + +- `UnseenObjClusterContactGraspnetPipeine` (UOC + CGN, recommended) +- `UnseenObjClusterGraspSamPipeine` (UOC + GraspSAM, recommended) + +### 4) Verify services + +```bash +ros2 service list | grep -E "segmentation_rgbd|get_grasps_rgbd|run_graspsam|move_to_pose" +``` + +## Provided FlexBE States + +### `UnseenObjSegRGBDServiceState` (recommended) +**File:** `uoc_flexbe_states/unseen_obj_seg_rgbd_service_state.py` + +Calls the UOC ROS 2 segmentation service in **RGB-D mode**. + +This is the primary and recommended UOC state for real use. + +**Typical outputs** +- Segmentation results / instance metadata (e.g., masks, selected instance candidates, scene mapping info) +- JSON/path/string metadata for downstream state selection (depending on your UOC server interface) + +**Default service** +- `/segmentation_rgbd` + +**Notes** +- Intended to be used before target selection and downstream grasp planning +- Works well with both the Contact-GraspNet RGB-D pipeline and the GraspSAM pipeline + +--- + +### `UnseenObjSegCloudServiceState` (experimental, not recommended) +**File:** `uoc_flexbe_states/unseen_obj_seg_cloud_service_state.py` + +Calls the UOC segmentation service using **point cloud inputs**. + +**Default service** +- (typically a cloud segmentation service, depending on your server setup) + +**Notes** +- Included for testing / experimentation +- UOC generally performs poorly in this mode in our setup +- Prefer `UnseenObjSegRGBDServiceState` unless you are explicitly testing point-cloud UOC behavior + +## Provided FlexBE Behaviors (Pipelines) + +### 1) `UnseenObjClusterContactGraspnetPipeine` (recommended) +**File:** `uoc_flexbe_behaviors/uoc_flexbe_behaviors/unseenobjclustercontactgraspnetpipeine_sm.py` + +Pipeline: +1. `UnseenObjSegRGBDServiceState` (`/segmentation_rgbd`) +2. `SelectInstanceToSceneNameState` (map selected target to CGN scene naming convention) +3. `CGNGraspRGBDServiceState` (`/get_grasps_rgbd`) +4. `MoveToPoseServiceState` (`/move_to_pose`) + +Why recommended: +- Strongest integration path for UOC-based grasping +- UOC segmentation pairs well with CGN RGB-D grasp generation +- Clean and reliable segmentation-to-grasp handoff + +--- + +### 2) `UnseenObjClusterGraspSamPipeine` (recommended) +**File:** `uoc_flexbe_behaviors/uoc_flexbe_behaviors/unseenobjclustergraspsampipeine_sm.py` + +Pipeline: +1. `UnseenObjSegRGBDServiceState` (`/segmentation_rgbd`) +2. `SelectInstanceToSceneNameState` (map selected target to GraspSAM scene convention) +3. `GraspSAMServiceState` (`/run_graspsam`) +4. `MoveToPoseServiceState` (`/move_to_pose`) + +Why recommended: +- Reuses the same UOC RGB-D segmentation front-end +- Clean integration into GraspSAM grasp generation +- Outputs base-frame grasp poses for direct motion planning + +## Tables for Easier Documentation + +### State summary + +| State file | Main class | Inputs | Outputs | Service called | Notes | +|---|---|---|---|---|---| +| `unseen_obj_seg_rgbd_service_state.py` | `UnseenObjSegRGBDServiceState` | RGB-D scene inputs / request config | segmentation outputs (instances, masks, metadata) | `/segmentation_rgbd` | Recommended UOC state. | +| `unseen_obj_seg_cloud_service_state.py` | `UnseenObjSegCloudServiceState` | PointCloud2 / cloud-based request | segmentation outputs (cloud mode) | cloud segmentation service (setup-dependent) | Experimental only; poor performance in our setup. | + +### Behavior summary + +| Behavior (FlexBE) | Main file | Pipeline type | Services used | Recommended | +|---|---|---|---|---| +| `UnseenObjClusterContactGraspnetPipeine` | `unseenobjclustercontactgraspnetpipeine_sm.py` | UOC (RGB-D) -> CGN (RGB-D) -> MoveIt | `/segmentation_rgbd`, `/get_grasps_rgbd`, `/move_to_pose` | Yes (primary) | +| `UnseenObjClusterGraspSamPipeine` | `unseenobjclustergraspsampipeine_sm.py` | UOC (RGB-D) -> GraspSAM -> MoveIt | `/segmentation_rgbd`, `/run_graspsam`, `/move_to_pose` | Yes (primary) | + +## Architecture + +### UOC + Contact-GraspNet (RGB-D) pipeline + +```text +RGB-D Camera + | + v +UOC Segmentation (ROS 2 service: /segmentation_rgbd) + | + v +Target instance selection / scene mapping + | + v +CGNGraspRGBDServiceState + | + v +Contact-GraspNet ROS 2 Server (/get_grasps_rgbd) + | + v +Grasp poses (Pose[]) + | + v +MoveIt / OMPL (/move_to_pose) + | + v +Robot motion execution +``` + +### UOC + GraspSAM (RGB-D) pipeline + +```text +RGB-D Camera + | + v +UOC Segmentation (ROS 2 service: /segmentation_rgbd) + | + v +Target instance selection / scene mapping + | + v +GraspSAMServiceState + | + v +GraspSAM ROS 2 Server (/run_graspsam) + | + v +Grasp results -> pose_base extraction + | + v +MoveIt / OMPL (/move_to_pose) + | + v +Robot motion execution +``` + +## Dependencies + +This repository assumes the following are available in your ROS 2 workspace: + +- **FlexBE** (core + onboard/app tooling) +- **UOC ROS 2 server** + - `/segmentation_rgbd` (recommended) + - Optional cloud segmentation service (if testing cloud mode) +- **Contact-GraspNet ROS 2 server** (for the CGN behavior) + - `/get_grasps_rgbd` +- **GraspSAM ROS 2 server** (for the GraspSAM behavior) + - `/run_graspsam` +- **MoveIt / motion execution service** + - `/move_to_pose` + +Companion FlexBE state packages used by the behaviors may include: + +- `compare_flexbe_states` (e.g., `MoveToPoseServiceState`, target selection utilities) +- `cgn_flexbe_states` (for `CGNGraspRGBDServiceState`) +- `gsam_flexbe_states` (for `GraspSAMServiceState`) + +## Installation + +Clone into your ROS 2 workspace `src/` folder: + +```bash +cd ~/your_ws/src +git clone +``` + +Build and source: + +```bash +cd ~/your_ws +colcon build --symlink-install +source install/setup.bash +``` + +## Notes and Recommendations + +- **Use `UnseenObjSegRGBDServiceState` for production use.** + This is the intended and reliable segmentation mode for UOC in these pipelines. + +- The cloud-based UOC state is kept as an experimental/test path. It is not recommended for normal use due to poor segmentation quality. + +- If your goal is point-cloud-based grasping with Contact-GraspNet, consider using other point-cloud perception/filtering modules (e.g., Euclidean clustering) instead of UOC cloud mode. + +- These behavior files are generated by FlexBE. Manual edits outside `[MANUAL]` regions may be overwritten if regenerated. + +## Acknowledgments + +This repository builds on: + +- Unseen Object Clustering (UOC) +- FlexBE +- ROS 2 +- Contact-GraspNet +- GraspSAM +- MoveIt diff --git a/uoc_flexbe/uoc_flexbe/CHANGELOG.rst b/uoc_flexbe/uoc_flexbe/CHANGELOG.rst new file mode 100644 index 0000000..e755418 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package uoc_flexbe +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE project \ No newline at end of file diff --git a/uoc_flexbe/uoc_flexbe/CMakeLists.txt b/uoc_flexbe/uoc_flexbe/CMakeLists.txt new file mode 100644 index 0000000..92eb5dd --- /dev/null +++ b/uoc_flexbe/uoc_flexbe/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.10) +project(uoc_flexbe) + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/uoc_flexbe/uoc_flexbe/package.xml b/uoc_flexbe/uoc_flexbe/package.xml new file mode 100644 index 0000000..7c58605 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe/package.xml @@ -0,0 +1,24 @@ + + uoc_flexbe + 0.0.1 + + Meta-package for FlexBE enabled uoc + + TODO + TODO + BSD + TODO/issues + TODO + TODO + + ament_cmake + + flexbe_core + uoc_flexbe_behaviors + uoc_flexbe_states + + + ament_cmake + + + diff --git a/uoc_flexbe/uoc_flexbe_behaviors/CHANGELOG.rst b/uoc_flexbe/uoc_flexbe_behaviors/CHANGELOG.rst new file mode 100644 index 0000000..9357d07 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package uoc_flexbe_behaviors +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE behaviors \ No newline at end of file diff --git a/uoc_flexbe/uoc_flexbe_behaviors/CMakeLists.txt b/uoc_flexbe/uoc_flexbe_behaviors/CMakeLists.txt new file mode 100644 index 0000000..5495cc3 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.10) +project(uoc_flexbe_behaviors) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) + +install(DIRECTORY + manifest + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + config + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS + bin/copy_behavior + DESTINATION lib/${PROJECT_NAME} +) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +ament_package() diff --git a/uoc_flexbe/uoc_flexbe_behaviors/bin/copy_behavior b/uoc_flexbe/uoc_flexbe_behaviors/bin/copy_behavior new file mode 100755 index 0000000..8ed6373 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/bin/copy_behavior @@ -0,0 +1,102 @@ +#!/bin/bash + +# Copyright 2025 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +# WARNING: Use at your own risk, this script does not protect against overwriting files, or mis-naming + +if [ $# -lt 1 ]; then + echo -e "\e[93mThis is an extremely simple script designed to copy behaviors\033[0m" + echo -e "\e[93mfrom the install folder to the src folder assumed to be a git repo\033[0m" + echo -e "\e[93mfor storage.\033[0m" + echo "" + echo -e "\e[93mIt requires a WORKSPACE_ROOT environment variable assuming a standard \033[0m" + echo -e "\e[93m \${WORKSPACE_ROOT}/install and \${WORKSPACE_ROOT}/src layout.\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/uoc\033[0m" + echo "" + echo -e "\e[93m Usage: ros2 run uoc_flexbe_behaviors copy_behavior BEHAVIOR_FILE_BASE \033[0m" + echo -e "\e[93m where BEHAVIOR_FILE_BASE_NAME.xml is the saved manifest name\033[0m" + echo -e "\e[93m OPTIONAL_BEHAVIOR_PACKAGE defaults to 'uoc_flexbe_behaviors'\033[0m" + echo "" + echo -e "\e[93m WARNING: Use at your own risk, this script does not protect against overwriting files or mis-naming\033[0m" + exit 2 +fi + + +beh=$(echo "$1" | cut -f 1 -d '.') # include only the base file name is someone pastes .xml name +pack="uoc_flexbe_behaviors" # Default name uses this behaviors package +if [ $# -eq 2 ]; then + pack="$2" + echo "Using specified package '${pack}'!" +fi + +# A few basic checks before attempting to copy +if [ ! -d "${pack}" ]; then + echo "" + echo -e "\e[91m Package '${pack}' does not exist under current directory '${PWD}' !\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/uoc\033[0m" + exit +fi + +if [ ! -d "${pack}/${pack}" ]; then + echo "" + echo -e "\e[91m Behavior implementation folder '${pack}/${pack}' does not exist under current directory '${PWD}' !\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/uoc\033[0m" + exit +fi + +PYVER=$(python3 -c "import sys; print(f'{sys.version_info[0]}.{sys.version_info[1]}')") + +if [ ! -f "${WORKSPACE_ROOT}/install/${pack}/lib/${PYVER}/site-packages/${pack}/${beh}_sm.py" ]; then + echo "" + echo -e "\e[91m Behavior '${beh}' implementation does not exist in install folder for '${pack}' package!\033[0m" + echo "" + echo "Available behavior manifests in '${pack}' :" + ls ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest + echo "" + echo -e "\e[93mConfirm behavior and package names!\033[0m" + exit +fi + + +# Begin the work +echo "Copying '${beh}' implementation to '${pack}' under '${PWD}' ..." +cp ${WORKSPACE_ROOT}/install/${pack}/local/lib/${PYVER}/site-packages/${pack}/${beh}_sm.py ./${pack}/${pack}/${beh}_sm.py + +if test $? -eq 0; then + echo "Copying '${beh}.xml' manifest to '${pack}' ..." + + cp ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest/${beh}.xml ./${pack}/manifest/${beh}.xml + if test $? -eq 0; then + echo "" + echo -e "\e[92mDone copying behavior '${beh}' to '${pack}'!\033[0m" + echo "Do not forget to git commit and git push any changes." + exit + else + echo "" + echo -e "\e[91m Failed to copy behavior '${beh}.xml' manifest after copying behavior implementation!\033[0m" + exit + fi +else + echo "" + echo -e "\e[91m Failed to copy behavior '${beh}'!\033[0m" + exit +fi \ No newline at end of file diff --git a/uoc_flexbe/uoc_flexbe_behaviors/config/example.yaml b/uoc_flexbe/uoc_flexbe_behaviors/config/example.yaml new file mode 100644 index 0000000..e3f24de --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/config/example.yaml @@ -0,0 +1 @@ +# You can use this folder to place general config files diff --git a/uoc_flexbe/uoc_flexbe_behaviors/manifest/unseenobjclustercontactgraspnetpipeine.xml b/uoc_flexbe/uoc_flexbe_behaviors/manifest/unseenobjclustercontactgraspnetpipeine.xml new file mode 100644 index 0000000..fc19b4a --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/manifest/unseenobjclustercontactgraspnetpipeine.xml @@ -0,0 +1,21 @@ + + + + + + + Huajing Zhao + Dec 07 2025 + + A perception-to-action pipeline which employs unseen-object-clustering for + object segmentation from a + scene and choose a target for grasping, then contact-graspnet for grasp + planning and OMPL for manipulation via MoveIt + + + + + + + + diff --git a/uoc_flexbe/uoc_flexbe_behaviors/manifest/unseenobjclustergraspsampipeine.xml b/uoc_flexbe/uoc_flexbe_behaviors/manifest/unseenobjclustergraspsampipeine.xml new file mode 100644 index 0000000..3e7614c --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/manifest/unseenobjclustergraspsampipeine.xml @@ -0,0 +1,20 @@ + + + + + + + Huajing Zhao + Feb 16 2026 + + Use uoc_flexbe and cgn_flexbe: A perception-to-action pipeline which employs unseen-object-clustering for + object segmentation from a scene and choose a target for grasping, then GraspSAM for grasp + planning and OMPL for manipulation via MoveIt + + + + + + + + diff --git a/uoc_flexbe/uoc_flexbe_behaviors/package.xml b/uoc_flexbe/uoc_flexbe_behaviors/package.xml new file mode 100644 index 0000000..60b6821 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/package.xml @@ -0,0 +1,30 @@ + + + + uoc_flexbe_behaviors + 0.0.1 + + uoc_flexbe_behaviors references all implemented behaviors. + + TODO + TODO + BSD + + rclpy + flexbe_core + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_cmake + ament_cmake_python + + + + ament_cmake + + diff --git a/uoc_flexbe/uoc_flexbe_behaviors/resource/uoc_flexbe_behaviors b/uoc_flexbe/uoc_flexbe_behaviors/resource/uoc_flexbe_behaviors new file mode 100644 index 0000000..e69de29 diff --git a/uoc_flexbe/uoc_flexbe_behaviors/setup.cfg b/uoc_flexbe/uoc_flexbe_behaviors/setup.cfg new file mode 100644 index 0000000..88aebd1 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/uoc_flexbe_behaviors +[install] +install_scripts=$base/lib/uoc_flexbe_behaviors diff --git a/uoc_flexbe/uoc_flexbe_behaviors/setup.py b/uoc_flexbe/uoc_flexbe_behaviors/setup.py new file mode 100644 index 0000000..153f661 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/setup.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python +from setuptools import setup + +package_name = 'uoc_flexbe_behaviors' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='phil', + maintainer_email='philsplus@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_behavior_sm = uoc_flexbe_behaviors.example_behavior_sm', + ], + }, +) diff --git a/uoc_flexbe/uoc_flexbe_behaviors/uoc_flexbe_behaviors/__init__.py b/uoc_flexbe/uoc_flexbe_behaviors/uoc_flexbe_behaviors/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/uoc_flexbe/uoc_flexbe_behaviors/uoc_flexbe_behaviors/unseenobjclustercontactgraspnetpipeine_sm.py b/uoc_flexbe/uoc_flexbe_behaviors/uoc_flexbe_behaviors/unseenobjclustercontactgraspnetpipeine_sm.py new file mode 100644 index 0000000..30de75c --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/uoc_flexbe_behaviors/unseenobjclustercontactgraspnetpipeine_sm.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2025 Huajing Zhao +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +# TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define UnseenObjClusterContactGraspnetPipeine. + +A perception-to-action pipeline which employs unseen-object-clustering for +object segmentation from a +scene and choose a target for grasping, then contact-graspnet for grasp +planning and OMPL for manipulation via MoveIt + +Created on Dec 07 2025 +@author: Huajing Zhao +""" + + +from cgn_flexbe_states.cgn_grasp_rgbd_service_state import CGNGraspRGBDServiceState +from cgn_flexbe_states.move_to_pose_service_state import MoveToPoseServiceState +from uoc_flexbe_states.select_instance_to_cgn_indices_state import SelectInstanceToSceneNameState +from uoc_flexbe_states.unseen_obj_seg_rgbd_service_state import UnseenObjSegRGBDServiceState +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class UnseenObjClusterContactGraspnetPipeineSM(Behavior): + """ + Define UnseenObjClusterContactGraspnetPipeine. + + A perception-to-action pipeline which employs unseen-object-clustering for + object segmentation from a + scene and choose a target for grasping, then contact-graspnet for grasp + planning and OMPL for manipulation via MoveIt + """ + + def __init__(self, node): + super().__init__() + self.name = 'UnseenObjClusterContactGraspnetPipeine' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:1154 y:332, x:141 y:356 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['im_name']) + _state_machine.userdata.im_name = 'from_rgbd' + _state_machine.userdata.seg_json = {} + _state_machine.userdata.result_dir = '' + _state_machine.userdata.instance_ids_2d = [] + _state_machine.userdata.instance_id_list = [] + _state_machine.userdata.target_instance_id = 0 + _state_machine.userdata.scene_name = 'scene_from_ucn' + _state_machine.userdata.message = '' + _state_machine.userdata.grasp_target_poses = [] + _state_machine.userdata.grasp_scores = [] + _state_machine.userdata.grasp_samples = [] + _state_machine.userdata.grasp_object_ids = [] + _state_machine.userdata.grasp_index = 0 + _state_machine.userdata.manual_target_instance_id = -1 + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:30 y:40 + OperatableStateMachine.add('UnseenObjSegRGBD', + UnseenObjSegRGBDServiceState(service_name='/segmentation_rgbd', + service_timeout=5.0, + default_im_name='from_rgbd', + background_id=0), + transitions={'finished': 'SelectInstanceToScene', + 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'im_name': 'im_name', + 'seg_json': 'seg_json', + 'result_dir': 'result_dir', + 'instance_ids_2d': 'instance_ids_2d', + 'instance_id_list': 'instance_id_list', + 'instance_masks': 'instance_masks', + 'message': 'message'}) + + # x:762 y:41 + OperatableStateMachine.add('CgnGraspRGBD', + CGNGraspRGBDServiceState(service_timeout=20.0, + service_name='/get_grasps_rgbd'), + transitions={'done': 'MoveOMPL', 'failed': 'failed'}, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'scene_name': 'scene_name', + 'grasp_target_poses': 'grasp_target_poses', + 'grasp_scores': 'grasp_scores', + 'grasp_samples': 'grasp_samples', + 'grasp_object_ids': 'grasp_object_ids'}) + + # x:1087 y:38 + OperatableStateMachine.add('MoveOMPL', + MoveToPoseServiceState(timeout_sec=5.0, + service_name='/move_to_pose'), + transitions={'done': 'finished', + 'next': 'MoveOMPL', + 'failed': 'failed'}, + autonomy={'done': Autonomy.Off, + 'next': Autonomy.Off, + 'failed': Autonomy.Off}, + remapping={'grasp_poses': 'grasp_target_poses', + 'grasp_index': 'grasp_index'}) + + # x:419 y:38 + OperatableStateMachine.add('SelectInstanceToScene', + SelectInstanceToSceneNameState(default_scene_name='scene_from_ucn', + selection_mode='manual'), + transitions={'finished': 'CgnGraspRGBD', 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'seg_json': 'seg_json', + 'result_dir': 'result_dir', + 'instance_ids_2d': 'instance_ids_2d', + 'instance_id_list': 'instance_id_list', + 'im_name': 'im_name', + 'target_instance_id': 'target_instance_id', + 'scene_name': 'scene_name', + 'manual_target_instance_id': 'manual_target_instance_id', + 'message': 'message'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/uoc_flexbe/uoc_flexbe_behaviors/uoc_flexbe_behaviors/unseenobjclustergraspsampipeine_sm.py b/uoc_flexbe/uoc_flexbe_behaviors/uoc_flexbe_behaviors/unseenobjclustergraspsampipeine_sm.py new file mode 100644 index 0000000..2cbb4c8 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_behaviors/uoc_flexbe_behaviors/unseenobjclustergraspsampipeine_sm.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2026 Huajing Zhao +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +# TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define UnseenObjClusterGraspSamPipeine. + +A perception-to-action pipeline which employs unseen-object-clustering for +object segmentation from a +scene and choose a target for grasping, then GraspSAM for grasp +planning and OMPL for manipulation via MoveIt + +Created on Feb 16 2026 +@author: Huajing Zhao +""" + + +from gsam_flexbe_states.graspsam_service_state import GraspSAMServiceState +from cgn_flexbe_states.move_to_pose_service_state import MoveToPoseServiceState +from uoc_flexbe_states.select_instance_to_cgn_indices_state import SelectInstanceToSceneNameState +from uoc_flexbe_states.unseen_obj_seg_rgbd_service_state import UnseenObjSegRGBDServiceState +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class UnseenObjClusterGraspSamPipeineSM(Behavior): + """ + Define UnseenObjClusterGraspSamPipeine. + + A perception-to-action pipeline which employs unseen-object-clustering for + object segmentation from a + scene and choose a target for grasping, then GraspSAM for grasp + planning and OMPL for manipulation via MoveIt + """ + + def __init__(self, node): + super().__init__() + self.name = 'UnseenObjClusterGraspSamPipeine' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:1154 y:332, x:141 y:356 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['im_name']) + _state_machine.userdata.im_name = 'from_rgbd' + _state_machine.userdata.seg_json = {} + _state_machine.userdata.result_dir = '' + _state_machine.userdata.instance_ids_2d = [] + _state_machine.userdata.instance_id_list = [] + _state_machine.userdata.target_instance_id = 0 + _state_machine.userdata.scene_name = 'scene_from_ucn' + _state_machine.userdata.message = '' + _state_machine.userdata.grasp_target_poses = [] + _state_machine.userdata.grasp_scores = [] + _state_machine.userdata.grasp_samples = [] + _state_machine.userdata.grasp_object_ids = [] + _state_machine.userdata.grasp_index = 0 + _state_machine.userdata.dataset_name = 'from_rgbd' + _state_machine.userdata.checkpoint_path = 'pretrained_checkpoint/mobile_sam.pt' + _state_machine.userdata.dataset_root = './datasets/sample_scene_ucn' + _state_machine.userdata.sam_encoder_type = 'vit_t' + _state_machine.userdata.no_grasps = 10 + _state_machine.userdata.seen_set = False + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:30 y:40 + OperatableStateMachine.add('UnseenObjSegRGBD', + UnseenObjSegRGBDServiceState(service_name='/segmentation_rgbd', + service_timeout=5.0, + default_im_name='from_rgbd', + background_id=0), + transitions={'finished': 'SelectInstanceToScene', + 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'im_name': 'im_name', + 'seg_json': 'seg_json', + 'result_dir': 'result_dir', + 'instance_ids_2d': 'instance_ids_2d', + 'instance_id_list': 'instance_id_list', + 'instance_masks': 'instance_masks', + 'message': 'message'}) + + # x:812 y:38 + OperatableStateMachine.add('GraspSAM', + GraspSAMServiceState(service_name='/run_graspsam', + dataset_root='./datasets/sample_scene_ucn', + dataset_name='from_rgbd', + checkpoint_path='pretrained_checkpoint/mobile_sam.pt', + sam_encoder_type='vit_t', + no_grasps=10, + timeout=2.0, + seen_set=False, + seen_set_default=False), + transitions={'done': 'MoveOMPL', 'failed': 'failed'}, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'dataset_root': 'dataset_root', + 'dataset_name': 'dataset_name', + 'checkpoint_path': 'checkpoint_path', + 'sam_encoder_type': 'sam_encoder_type', + 'no_grasps': 'no_grasps', + 'seen_set': 'seen_set', + 'output_dir': 'output_dir', + 'grasps': 'grasps', + 'grasp_target_poses': 'grasp_target_poses', + 'message': 'message'}) + + # x:1087 y:38 + OperatableStateMachine.add('MoveOMPL', + MoveToPoseServiceState(timeout_sec=5.0, + service_name='/move_to_pose'), + transitions={'done': 'finished', + 'next': 'MoveOMPL', + 'failed': 'failed' # 666 281 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, + 'next': Autonomy.Off, + 'failed': Autonomy.Off}, + remapping={'grasp_poses': 'grasp_target_poses', + 'grasp_index': 'grasp_index'}) + + # x:419 y:38 + OperatableStateMachine.add('SelectInstanceToScene', + SelectInstanceToSceneNameState(default_scene_name='scene_from_ucn'), + transitions={'finished': 'GraspSAM', 'failed': 'failed'}, + autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'seg_json': 'seg_json', + 'result_dir': 'result_dir', + 'instance_ids_2d': 'instance_ids_2d', + 'instance_id_list': 'instance_id_list', + 'im_name': 'im_name', + 'target_instance_id': 'target_instance_id', + 'scene_name': 'scene_name', + 'message': 'message'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/uoc_flexbe/uoc_flexbe_states/CHANGELOG.rst b/uoc_flexbe/uoc_flexbe_states/CHANGELOG.rst new file mode 100644 index 0000000..294b859 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package uoc_flexbe_states +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE states \ No newline at end of file diff --git a/uoc_flexbe/uoc_flexbe_states/package.xml b/uoc_flexbe/uoc_flexbe_states/package.xml new file mode 100644 index 0000000..754e942 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/package.xml @@ -0,0 +1,38 @@ + + + + uoc_flexbe_states + 0.0.1 + + uoc_flexbe_states provides a collection of custom states. + Feel free to add new states. + + TODO + TODO + BSD + + rclpy + flexbe_core + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + turtlesim + + ament_cmake + ament_cmake_python + + + + ament_python + + + diff --git a/uoc_flexbe/uoc_flexbe_states/resource/uoc_flexbe_states b/uoc_flexbe/uoc_flexbe_states/resource/uoc_flexbe_states new file mode 100644 index 0000000..e69de29 diff --git a/uoc_flexbe/uoc_flexbe_states/setup.cfg b/uoc_flexbe/uoc_flexbe_states/setup.cfg new file mode 100644 index 0000000..2b79a81 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/uoc_flexbe_states +[install] +install_scripts=$base/lib/uoc_flexbe_states diff --git a/uoc_flexbe/uoc_flexbe_states/setup.py b/uoc_flexbe/uoc_flexbe_states/setup.py new file mode 100644 index 0000000..28e62d7 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/setup.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python + +from glob import glob +from setuptools import setup +from setuptools import find_packages + +PACKAGE_NAME = 'uoc_flexbe_states' + +setup( + name=PACKAGE_NAME, + version='0.0.1', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + ('share/' + PACKAGE_NAME + "/tests", glob('tests/*.test')), + ('share/' + PACKAGE_NAME + "/launch", glob('tests/*.launch.py')), + ], + + install_requires=['setuptools'], + zip_safe=True, + maintainer='TODO', + maintainer_email='TODO@TODO.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_action_state = uoc_flexbe_states.example_action_state', + 'example_state = uoc_flexbe_states.example_state', + ], + }, +) diff --git a/uoc_flexbe/uoc_flexbe_states/tests/example_action_state.test b/uoc_flexbe/uoc_flexbe_states/tests/example_action_state.test new file mode 100644 index 0000000..793bea8 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/tests/example_action_state.test @@ -0,0 +1,14 @@ +path: uoc_flexbe_states.example_action_state +class: ExampleActionState + +import_only: false + +params: + action_topic: 'action_is_not_available' + timeout: 0.5 + +input: + angle: 2. + +outcome: + failed diff --git a/uoc_flexbe/uoc_flexbe_states/tests/example_state.test b/uoc_flexbe/uoc_flexbe_states/tests/example_state.test new file mode 100644 index 0000000..8102b10 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/tests/example_state.test @@ -0,0 +1,10 @@ +path: uoc_flexbe_states.example_state +class: ExampleState + +import_only: true + +params: + target_time: 2.0 + +outcome: + done diff --git a/uoc_flexbe/uoc_flexbe_states/tests/run_colcon_test.py b/uoc_flexbe/uoc_flexbe_states/tests/run_colcon_test.py new file mode 100644 index 0000000..47258d2 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/tests/run_colcon_test.py @@ -0,0 +1,62 @@ +# Copyright 2023 Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +"""Pytest testing for uoc_flexbe_states.""" + + +from flexbe_testing.py_tester import PyTester + + +class TestFlexBEStates(PyTester): + """Pytest testing for uoc_flexbe_states.""" + + # def __init__(self, *args, **kwargs): + # """Initialize unit test.""" + # super().__init__(*args, **kwargs) + + @classmethod + def setUpClass(cls): + + PyTester._package = "uoc_flexbe_states" + PyTester._tests_folder = "tests" + + PyTester.setUpClass() # Do this last after setting package and tests folder + + # The tests + def test_example_state(self): + """Run FlexBE unit test given .test file.""" + self.run_test("example_state") + + def test_example_action_state(self): + """ + Run FlexBE unit test given .test file. + + This test requires longer wait than normal + """ + self.run_test("example_action_state", timeout_sec=2.0, max_cnt=5000) diff --git a/uoc_flexbe/uoc_flexbe_states/tests/run_tests.launch.py b/uoc_flexbe/uoc_flexbe_states/tests/run_tests.launch.py new file mode 100644 index 0000000..916a8f3 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/tests/run_tests.launch.py @@ -0,0 +1,64 @@ +# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""uoc_flexbe_states testing.""" + +from os.path import join + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Flexbe_states testing.""" + flexbe_testing_dir = get_package_share_directory('flexbe_testing') + flexbe_states_test_dir = get_package_share_directory('uoc_flexbe_states') + + path = join(flexbe_states_test_dir, "tests") + + # The tests + testcases = "" + testcases += join(path, "example_state.test") + "\n" + testcases += join(path, "example_action_state.test") + "\n" + + return LaunchDescription([ + DeclareLaunchArgument("pkg", default_value="uoc_flexbe_states"), + DeclareLaunchArgument("testcases", default_value=testcases), + DeclareLaunchArgument("compact_format", default_value='true'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(join(flexbe_testing_dir, "launch", "flexbe_testing.launch.py")), + launch_arguments={ + 'package': LaunchConfiguration("pkg"), + 'compact_format': LaunchConfiguration("compact_format"), + 'testcases': LaunchConfiguration("testcases"), + }.items() + ) + ]) diff --git a/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/__init__.py b/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/select_instance_to_cgn_indices_state.py b/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/select_instance_to_cgn_indices_state.py new file mode 100644 index 0000000..88232e6 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/select_instance_to_cgn_indices_state.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import json +from flexbe_core import EventState, Logger +import subprocess +import numpy as np + +class SelectInstanceToSceneNameState(EventState): + def __init__(self, + default_scene_name: str = 'scene_from_ucn', + selection_mode: str = 'manual', # 'largest' | 'manual' | 'largest_or_manual' + allow_background: bool = False, + manual_sentinel: int = -1): + super().__init__( + outcomes=['finished', 'failed'], + input_keys=[ + 'seg_json', 'result_dir', 'instance_ids_2d', 'instance_id_list', 'im_name', + 'manual_target_instance_id' # NEW + ], + output_keys=['target_instance_id', 'scene_name', 'message'] + ) + self._default_scene_name = str(default_scene_name) + self._selection_mode = str(selection_mode).lower().strip() + self._allow_background = bool(allow_background) + self._manual_sentinel = int(manual_sentinel) + + self._had_error = False + self._target_id = None + self._msg = "" + + def _pick_largest(self, instance_ids, instance_ids_2d): + best_id = None + best_area = -1 + areas = {} + + for inst_id in instance_ids: + mask = (instance_ids_2d == inst_id) + area = int(mask.sum()) + areas[int(inst_id)] = area + Logger.loginfo(f"[SelectInstanceToSceneNameState] Instance {inst_id} has area {area}.") + if area > best_area: + best_area = area + best_id = inst_id + + return (None, None, areas) if best_id is None else (int(best_id), int(best_area), areas) + + def _get_manual_id(self, userdata): + # Accept None, missing, sentinel => "not provided" + if not hasattr(userdata, 'manual_target_instance_id'): + return None + v = userdata.manual_target_instance_id + if v is None: + return None + try: + v = int(v) + except Exception: + return None + if v == self._manual_sentinel: + return None + return v + + def on_enter(self, userdata): + self._had_error = False + self._target_id = None + self._msg = "" + + try: + seg = userdata.seg_json + if isinstance(seg, str): + seg = json.loads(seg) + + instance_ids = list(userdata.instance_id_list) if userdata.instance_id_list else [] + if not self._allow_background: + instance_ids = [i for i in instance_ids if int(i) != 0] + + if not instance_ids: + self._msg = "[SelectInstanceToSceneNameState] No instance ids found." + Logger.logwarn(self._msg) + self._had_error = True + return + + instance_ids_2d = np.array(userdata.instance_ids_2d, dtype=np.int32) + + best_id, best_area, areas = self._pick_largest(instance_ids, instance_ids_2d) + if best_id is None or best_area is None or best_area <= 0: + self._msg = "[SelectInstanceToSceneNameState] Failed to find a non-empty instance mask." + Logger.logwarn(self._msg) + self._had_error = True + return + + # NEW: manual id from userdata + manual_id = self._get_manual_id(userdata) + + # Decide + if self._selection_mode == 'largest': + chosen_id = best_id + elif self._selection_mode == 'manual': + if manual_id is None: + self._msg = ("[SelectInstanceToSceneNameState] selection_mode='manual' but " + "manual_target_instance_id was not provided.") + Logger.logwarn(self._msg) + self._had_error = True + return + chosen_id = manual_id + else: # 'largest_or_manual' default + chosen_id = manual_id if manual_id is not None else best_id + + # Validate chosen_id + valid_ids = [int(x) for x in instance_ids] + if int(chosen_id) not in valid_ids: + self._msg = (f"[SelectInstanceToSceneNameState] Chosen instance id {chosen_id} " + f"is not in instance_id_list {valid_ids}.") + Logger.logwarn(self._msg) + self._had_error = True + return + + chosen_area = areas.get(int(chosen_id), -1) + self._target_id = int(chosen_id) + self._msg = (f"[SelectInstanceToSceneNameState] Selected instance {self._target_id} " + f"(area={chosen_area}) → scene_name='{self._default_scene_name}'") + Logger.loginfo(self._msg) + + cmd = [ + "python3", + "/home/csrobot/graspnet_ws/src/contact_graspnet_ros2/contact_graspnet/test_data/ucn_to_cgn_scene.py", + # IMPORTANT: if your script supports args, you should pass them here: + # "--im_name", userdata.im_name, + # "--seg_dir", userdata.result_dir, + # "--scene_name", self._default_scene_name, + # "--target_id", str(self._target_id), + ] + subprocess.check_call(cmd) + + Logger.loginfo("[SelectInstanceToSceneNameState] Generated contact_graspnet/test_data/scene_from_ucn.npy using ucn_to_cgn_scene.py") + + except Exception as e: + self._msg = f"[SelectInstanceToSceneNameState] Exception: {e}" + Logger.logerr(self._msg) + self._had_error = True + + def execute(self, userdata): + if self._had_error: + userdata.message = self._msg + return 'failed' + + userdata.target_instance_id = self._target_id + userdata.scene_name = self._default_scene_name + userdata.message = self._msg + return 'finished' \ No newline at end of file diff --git a/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/unseen_obj_seg_cloud_service_state.py b/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/unseen_obj_seg_cloud_service_state.py new file mode 100644 index 0000000..46423c6 --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/unseen_obj_seg_cloud_service_state.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +import json, os +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyServiceCaller +from unseen_obj_clst_ros2.srv import SegCloud +from sensor_msgs.msg import PointCloud2, CameraInfo + +class UnseenObjSegCloudServiceState(EventState): + """ + Calls 'run_segmentation_cloud' with a PointCloud2 (and optional CameraInfo). + Outputs: + seg_json (dict), result_dir (str), instance_ids (list), classes (list), bboxes (list), message (str) + """ + def __init__(self, + cloud_service='run_segmentation_cloud', + service_timeout=5.0, + default_im_name='from_cloud'): + super().__init__( + outcomes=['finished', 'failed'], + input_keys=['cloud_in', 'camera_info'], #, 'image_name'], + output_keys=['seg_json','result_dir','instance_ids','classes','bboxes'] #,'message'] + ) + self._timeout = float(service_timeout) + self._default_im_name = default_im_name + self._cloud_srv_name = cloud_service + self._srv = ProxyServiceCaller({ self._cloud_srv_name: SegCloud }) + self._res = None + self._err = False + + def on_enter(self, userdata): + self._res, self._err = None, False + if not isinstance(getattr(userdata, 'cloud_in', None), PointCloud2): + Logger.logerr("[SegCloudServiceState] Missing or invalid 'cloud_in' PointCloud2.") + self._err = True + return + + if not self._srv.is_available(self._cloud_srv_name, timeout=self._timeout): + Logger.logerr(f"[SegCloudServiceState] Service '{self._cloud_srv_name}' not available after {self._timeout}s.") + self._err = True + return + + try: + req = SegCloud.Request() + req.cloud = userdata.cloud_in + if isinstance(getattr(userdata, 'camera_info', None), CameraInfo): + req.cam_info = userdata.camera_info + # # optional im_name if your .srv includes it + # try: + # im_name = getattr(userdata, 'image_name', None) or self._default_im_name + # req.im_name = im_name + # except Exception: + # pass + + self._res = self._srv.call(self._cloud_srv_name, req) + + except Exception as e: + Logger.logerr(f"[SegCloudServiceState] Service call failed: {e}") + self._err = True + + def execute(self, userdata): + if self._err or self._res is None: + return 'failed' + if not self._res.success: + userdata.message = self._res.log_output or "Segmentation failed." + return 'failed' + + try: + seg_json = json.loads(self._res.json_result) + classes = seg_json.get('classes', []) + instance_ids = seg_json.get('instance_ids', []) + bboxes = seg_json.get('bboxes', []) + + result_dir = seg_json.get('result_dir', '') + if not result_dir: + base_output_dir = seg_json.get('base_output_dir', "/tmp/ucn_io/out") + # im_name = getattr(userdata, 'image_name', self._default_im_name) + result_dir = os.path.join(base_output_dir, f"segmentation_output") #f"segmentation_{im_name}") + + userdata.seg_json = seg_json + userdata.result_dir = result_dir + userdata.classes = classes + userdata.instance_ids = instance_ids + userdata.bboxes = bboxes + # userdata.message = self._res.log_output or "" + return 'finished' + + except Exception as e: + Logger.logerr(f"[SegCloudServiceState] Parse error: {e}") + return 'failed' diff --git a/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/unseen_obj_seg_rgbd_service_state.py b/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/unseen_obj_seg_rgbd_service_state.py new file mode 100644 index 0000000..8a90f5c --- /dev/null +++ b/uoc_flexbe/uoc_flexbe_states/uoc_flexbe_states/unseen_obj_seg_rgbd_service_state.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import os +import json +import numpy as np + +import time + + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyServiceCaller + +from unseen_obj_clst_ros2.srv import SegImage + +import subprocess, os + + +# Note: equivalent to running "ros2 service call /segmentation_rgbd unseen_obj_clst_ros2/srv/SegImage "{im_name: 'from_rgbd'}" in another terminal + + + +class UnseenObjSegRGBDServiceState(EventState): + """ + Call the /segmentation_rgbd SegImage service and expose instance IDs + masks. + + The RGBD segmentation server saves the latest RGB + depth, runs the UCN + segmentation pipeline, and returns a JSON string (segmentation.json) plus + a result directory. This state: + + 1) Calls /segmentation_rgbd (SegImage) with a chosen im_name. + 2) Parses the JSON returned in `json_result`. + 3) Extracts the 2D instance-id map and builds per-instance binary masks. + 4) Publishes everything on userdata for downstream states (e.g. CGN). + + -- service_name string Service name (default: '/segmentation_rgbd') + -- service_timeout float Timeout for service discovery (sec) + -- default_im_name string Fallback im_name if userdata.im_name is empty + -- background_id int Label to treat as background (default: 0) + + ># im_name string Optional override for im_name + <# seg_json dict Full segmentation JSON + <# result_dir string Output directory (as provided by server/JSON) + <# instance_ids_2d object HxW np.ndarray of instance IDs (int32) + <# instance_id_list list Sorted unique non-background IDs + <# instance_masks list List of HxW np.uint8 masks (one per instance) + <# message string Log / debug text from server + + <= finished Segmentation succeeded and userdata filled + <= failed Service call or JSON parsing failed + """ + + def __init__(self, + service_name: str = '/segmentation_rgbd', + service_timeout: float = 10.0, + default_im_name: str = 'from_rgbd', + background_id: int = 0): + + super(UnseenObjSegRGBDServiceState, self).__init__( + outcomes=['finished', 'failed'], + input_keys=['im_name'], + output_keys=[ + 'seg_json', + 'result_dir', + 'instance_ids_2d', + 'instance_id_list', + 'instance_masks', + 'message' + ] + ) + + self._service_name = service_name + self._timeout = float(service_timeout) + self._default_im_name = str(default_im_name) + self._background_id = int(background_id) + + # Proxy to the SegImage service + self._srv = ProxyServiceCaller({self._service_name: SegImage}) + + self._res = None + self._had_error = False + self._im_name_used = self._default_im_name + + # ------------------------------------------------------------------ + # FlexBE lifecycle + # ------------------------------------------------------------------ + + def on_enter(self, userdata): + """Send the SegImage request when we enter the state.""" + self._res = None + self._had_error = False + + # # Check service availability + # if not self._srv.is_available(self._service_name, timeout=self._timeout): + # Logger.logerr( + # f"[{type(self).__name__}] Service '{self._service_name}' " + # f"not available after {self._timeout:.1f}s." + # ) + # self._had_error = True + # return + + + # Check service availability (manual timeout loop) + start = time.time() + while not self._srv.is_available(self._service_name): + if time.time() - start > self._timeout: + Logger.logerr( + f"[{type(self).__name__}] Service '{self._service_name}' " + f"not available after {self._timeout:.1f}s." + ) + self._had_error = True + return + Logger.loginfo( + f"[{type(self).__name__}] Waiting for service '{self._service_name}'..." + ) + time.sleep(0.2) + + # Choose im_name: userdata.im_name or default + im_name = getattr(userdata, 'im_name', None) or self._default_im_name + self._im_name_used = im_name + + # Build request + req = SegImage.Request() + # SegImage server expects `im_name` as the field + req.im_name = im_name + + try: + self._res = self._srv.call(self._service_name, req) + Logger.loginfo( + f"[{type(self).__name__}] Called {self._service_name} " + f"with im_name='{im_name}'." + ) + + cmd = [ + "python3", + "/home/csrobot/graspnet_ws/src/unseen_obj_clst_ros2/compare_UnseenObjectClustering/segmentation_rgbd/visualize_segmentation.py", + ] + subprocess.check_call(cmd) + + except Exception as e: + Logger.logerr(f"[{type(self).__name__}] Service call failed: {e}") + self._had_error = True + + def execute(self, userdata): + """Parse the response and fill userdata.""" + if self._had_error or self._res is None: + return 'failed' + + # Check success flag from server + if not getattr(self._res, 'success', False): + msg = getattr(self._res, 'log_output', 'Segmentation failed.') + Logger.logerr(f"[{type(self).__name__}] Segmentation reported failure: {msg}") + userdata.message = msg + return 'failed' + + # Parse JSON + try: + json_str = self._res.json_result + seg_json = json.loads(json_str) + except Exception as e: + Logger.logerr(f"[{type(self).__name__}] Failed to parse json_result: {e}") + userdata.message = f"JSON parse error: {e}" + return 'failed' + + # Extract instance-id map + instance_ids = seg_json.get('instance_ids', None) + if instance_ids is None: + Logger.logerr(f"[{type(self).__name__}] 'instance_ids' missing in JSON.") + userdata.message = "Segmentation JSON missing 'instance_ids'." + return 'failed' + + # Convert to numpy array for easier mask construction + arr = np.asarray(instance_ids, dtype=np.int32) + h, w = arr.shape + Logger.loginfo( + f"[{type(self).__name__}] Received instance_ids map of shape {h}x{w}." + ) + + # Unique instance labels (excluding background) + unique_ids = sorted(int(v) for v in np.unique(arr) if v != self._background_id) + Logger.loginfo( + f"[{type(self).__name__}] Unique instance IDs (no background): {unique_ids}" + ) + + # Build binary masks (HxW np.uint8, 1 where pixel == instance_id) + masks = [] + for inst_id in unique_ids: + mask = (arr == inst_id).astype(np.uint8) + masks.append(mask) + + # Result directory: prefer JSON's 'result_dir', fall back to response field + result_dir = seg_json.get('result_dir', '') or getattr(self._res, 'result_dir', '') + if not result_dir: + # As absolute last resort, try base_output_dir + im_name + base_output_dir = seg_json.get('base_output_dir', '') + if base_output_dir: + result_dir = os.path.join(base_output_dir, f"segmentation_{self._im_name_used}") + + # Fill userdata + userdata.seg_json = seg_json + userdata.result_dir = result_dir + userdata.instance_ids_2d = arr + userdata.instance_id_list = unique_ids + userdata.instance_masks = masks + userdata.message = getattr(self._res, 'log_output', '') + + return 'finished' + + def on_exit(self, userdata): + """No special cleanup needed.""" + pass