diff --git a/.gitignore b/.gitignore index ba0430d2..29928740 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1 @@ -__pycache__/ \ No newline at end of file +__pycache__/examples/overarching_tutorial/.docker/build/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..eb5c8f54 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "examples/overarching_tutorial/simulation/dependencies/examples/overarching_tutorial/simulation/dependencies/BehaviorTree.ROS2"] + path = examples/overarching_tutorial/simulation/dependencies/examples/overarching_tutorial/simulation/dependencies/BehaviorTree.ROS2 + url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git +[submodule "examples/overarching_tutorial/simulation/dependencies/BehaviorTree.ROS2"] + path = examples/overarching_tutorial/simulation/dependencies/BehaviorTree.ROS2 + url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git diff --git a/examples/overarching_tutorial/simulation/.docker/Dockerfile b/examples/overarching_tutorial/.docker/Dockerfile similarity index 62% rename from examples/overarching_tutorial/simulation/.docker/Dockerfile rename to examples/overarching_tutorial/.docker/Dockerfile index 97f6522d..f65c61ea 100644 --- a/examples/overarching_tutorial/simulation/.docker/Dockerfile +++ b/examples/overarching_tutorial/.docker/Dockerfile @@ -1,31 +1,54 @@ FROM ros:jazzy-ros-base SHELL ["/bin/bash", "-o", "pipefail", "-c"] +USER root + + # Install apt dependencies. ARG DEBIAN_FRONTEND=noninteractive RUN apt-get update && \ apt-get upgrade -y && \ apt-get install -y \ apt-utils curl fuse3 libfuse2 gdb nano \ + terminator dbus-x11 \ libegl1 libgl1-mesa-dev libglu1-mesa-dev '^libxcb.*-dev' libx11-xcb-dev \ libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev \ libnss3 libasound2t64 libxkbfile1 \ python3-pip python3-tk python3-wrapt python3-inflection \ - ros-jazzy-example-interfaces ros-jazzy-rqt-service-caller + ros-jazzy-example-interfaces ros-jazzy-rqt-service-caller \ + libczmq-dev ros-jazzy-zmqpp-vendor-dbgsym ros-jazzy-zmqpp-vendor libczmq-dev libczmq4 curl # Create a ROS 2 workspace. RUN mkdir -p /convince_ws/src/ WORKDIR /convince_ws + +RUN /bin/bash -c "git clone --recurse-submodules https://github.com/BehaviorTree/BehaviorTree.CPP.git -b v3.8 && \ + cd BehaviorTree.CPP && \ + cmake -S . -B build && \ + cmake --build build && \ + cmake --install build" + + + # Install external dependencies. # (pip) -COPY requirements.txt /convince_ws +COPY simulation/requirements.txt /convince_ws RUN pip3 install --break-system-packages -r requirements.txt -# COPY dependencies src/dependencies -# RUN source /opt/ros/jazzy/setup.bash && \ -# rosdep install --from-paths src -y --ignore-src -# (pyrobosim) +COPY simulation/dependencies src/dependencies RUN git clone -b 4.3.3 https://github.com/sea-bass/pyrobosim.git src/pyrobosim +# COPY simulation/behavior_tree src/behavior_tree +# COPY roaml src/roaml +# COPY ros_interfaces src/ros_interfaces +# COPY simulation/generated_skill src/generated_skill + +# `bt_executor` depends on `behaviortree_ros2`, which is not released for Jazzy. +# Bring it in as a source package so rosdep does not need a rosdep key for it. +RUN git clone --depth 1 https://github.com/BehaviorTree/BehaviorTree.ROS2.git src/behaviortree_ros2 +RUN source /opt/ros/jazzy/setup.bash && \ + rosdep install --from-paths src -y --ignore-src +# (pyrobosim) + # # (smc_storm) # RUN curl -O -L https://github.com/convince-project/smc_storm/releases/download/0.0.6/smc_storm_executable.tar.gz && \ @@ -45,6 +68,7 @@ RUN git clone -b 4.3.3 https://github.com/sea-bass/pyrobosim.git src/pyrobosim # RUN chmod -R 0700 /tmp/runtime-root # ENV NO_AT_BRIDGE 1 +RUN apt install -y qt6-scxml* # Set up the entrypoint. CMD /bin/bash COPY .docker/entrypoint.sh /entrypoint.sh diff --git a/examples/overarching_tutorial/simulation/.docker/entrypoint.sh b/examples/overarching_tutorial/.docker/entrypoint.sh similarity index 100% rename from examples/overarching_tutorial/simulation/.docker/entrypoint.sh rename to examples/overarching_tutorial/.docker/entrypoint.sh diff --git a/examples/overarching_tutorial/.gitignore b/examples/overarching_tutorial/.gitignore index 6be53f8f..4ec528ca 100644 --- a/examples/overarching_tutorial/.gitignore +++ b/examples/overarching_tutorial/.gitignore @@ -1,3 +1,4 @@ roaml/scxml roaml/*.jani -**/*.csv \ No newline at end of file +**/*.csv +simulation/.docker/build/ \ No newline at end of file diff --git a/examples/overarching_tutorial/roaml/README.md b/examples/overarching_tutorial/roaml/README.md index a2cd0874..d37d66eb 100644 --- a/examples/overarching_tutorial/roaml/README.md +++ b/examples/overarching_tutorial/roaml/README.md @@ -52,6 +52,36 @@ as2fm_roaml_to_jani main.xml --scxml-out-dir scxml ``` This commands generates a folder with several SCXML files, that can be loaded and executed by SCAN for property verification. +#### Quick run guide (tested commands) + +To avoid environment/path issues, use the commands below exactly as shown. + +From the repository root: + +```bash +cd examples/overarching_tutorial/roaml +source ../ros_interfaces/install/setup.bash +``` + +Generate SCXML models for SCAN: + +```bash +as2fm_roaml_to_jani main.xml --scxml-out-dir new_folder +``` + +Generate JANI model for SMC Storm: + +```bash +as2fm_roaml_to_jani main.xml --jani-out-file main.jani +``` + +You can replace `main.xml` with any other entry model in this folder, for example: + +```bash +as2fm_roaml_to_jani main_locations.xml --scxml-out-dir scxml_main_locations +as2fm_roaml_to_jani main_locations_w_failures.xml --jani-out-file main_locations_w_failures.jani +``` + ### Verify the JANI model using SMC_STORM Once we have a JANI model of the system, we can use SMC Storm to verify properties on it. diff --git a/examples/overarching_tutorial/roaml/bt_plugins/bt_place_object.ascxml b/examples/overarching_tutorial/roaml/bt_plugins/bt_place_object.ascxml index 4ab3e243..a7cec5a1 100644 --- a/examples/overarching_tutorial/roaml/bt_plugins/bt_place_object.ascxml +++ b/examples/overarching_tutorial/roaml/bt_plugins/bt_place_object.ascxml @@ -1,56 +1,38 @@ - - - + + + - - - - - - - - - - + - - - + + + - - + - - - - - - - - - - - - - - - - - - + + + + + + + + + + diff --git a/examples/overarching_tutorial/roaml/bt_plugins/bt_ros2_action.ascxml b/examples/overarching_tutorial/roaml/bt_plugins/bt_ros2_action.ascxml new file mode 100644 index 00000000..6a61b72d --- /dev/null +++ b/examples/overarching_tutorial/roaml/bt_plugins/bt_ros2_action.ascxml @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/overarching_tutorial/roaml/main.xml b/examples/overarching_tutorial/roaml/main.xml index 20a505ae..eabfcf3c 100644 --- a/examples/overarching_tutorial/roaml/main.xml +++ b/examples/overarching_tutorial/roaml/main.xml @@ -14,13 +14,16 @@ + + + diff --git a/examples/overarching_tutorial/roaml/main_locations.xml b/examples/overarching_tutorial/roaml/main_locations.xml index 1118825d..02bdb467 100644 --- a/examples/overarching_tutorial/roaml/main_locations.xml +++ b/examples/overarching_tutorial/roaml/main_locations.xml @@ -15,13 +15,16 @@ + + + diff --git a/examples/overarching_tutorial/roaml/main_locations_failures_unhandled.xml b/examples/overarching_tutorial/roaml/main_locations_failures_unhandled.xml index 14a6b917..3988e669 100644 --- a/examples/overarching_tutorial/roaml/main_locations_failures_unhandled.xml +++ b/examples/overarching_tutorial/roaml/main_locations_failures_unhandled.xml @@ -15,13 +15,16 @@ + + + diff --git a/examples/overarching_tutorial/roaml/main_locations_w_failures.xml b/examples/overarching_tutorial/roaml/main_locations_w_failures.xml index ca240d5a..14af1a7e 100644 --- a/examples/overarching_tutorial/roaml/main_locations_w_failures.xml +++ b/examples/overarching_tutorial/roaml/main_locations_w_failures.xml @@ -15,13 +15,16 @@ + + + diff --git a/examples/overarching_tutorial/roaml/policy/bt_tree.xml b/examples/overarching_tutorial/roaml/policy/bt_tree.xml index 755dd520..acd23129 100644 --- a/examples/overarching_tutorial/roaml/policy/bt_tree.xml +++ b/examples/overarching_tutorial/roaml/policy/bt_tree.xml @@ -1,21 +1,13 @@ - - + - - - - - - - - + + + + + + + diff --git a/examples/overarching_tutorial/roaml/policy/bt_tree_locations.xml b/examples/overarching_tutorial/roaml/policy/bt_tree_locations.xml index 8c6e2703..91e15308 100644 --- a/examples/overarching_tutorial/roaml/policy/bt_tree_locations.xml +++ b/examples/overarching_tutorial/roaml/policy/bt_tree_locations.xml @@ -1,16 +1,21 @@ - + + - - + + + + - - - - + + + + + diff --git a/examples/overarching_tutorial/roaml/policy/bt_tree_locations_handle_failures.xml b/examples/overarching_tutorial/roaml/policy/bt_tree_locations_handle_failures.xml index 19b94f07..0029a203 100644 --- a/examples/overarching_tutorial/roaml/policy/bt_tree_locations_handle_failures.xml +++ b/examples/overarching_tutorial/roaml/policy/bt_tree_locations_handle_failures.xml @@ -1,6 +1,7 @@ + @@ -9,20 +10,26 @@ - - - + + + + + + + + - + - + - + diff --git a/examples/overarching_tutorial/roaml/properties/properties.xml b/examples/overarching_tutorial/roaml/properties/properties.xml new file mode 100644 index 00000000..6640dfe2 --- /dev/null +++ b/examples/overarching_tutorial/roaml/properties/properties.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + x == 4.0 && y == 2.0 && parent == 'world' + + + + + diff --git a/examples/overarching_tutorial/roaml/skills/PlaceObjectSkill.ascxml b/examples/overarching_tutorial/roaml/skills/PlaceObjectSkill.ascxml new file mode 100644 index 00000000..00a7615c --- /dev/null +++ b/examples/overarching_tutorial/roaml/skills/PlaceObjectSkill.ascxml @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/CMakeLists.txt b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/CMakeLists.txt new file mode 100644 index 00000000..d5c0407a --- /dev/null +++ b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(bt_interfaces_dummy) +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} +"srv/TickAction.srv" +"msg/ActionResponse.msg" +"msg/ConditionResponse.msg" +"srv/HaltAction.srv" +"srv/TickCondition.srv" +"srv/ReloadTree.srv" +LIBRARY_NAME ${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/msg/ActionResponse.msg b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/msg/ActionResponse.msg new file mode 100644 index 00000000..02a2b3e8 --- /dev/null +++ b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/msg/ActionResponse.msg @@ -0,0 +1,4 @@ +int8 SKILL_SUCCESS=0 +int8 SKILL_FAILURE=1 +int8 SKILL_RUNNING=2 +int8 status \ No newline at end of file diff --git a/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/msg/ConditionResponse.msg b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/msg/ConditionResponse.msg new file mode 100644 index 00000000..23894c4a --- /dev/null +++ b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/msg/ConditionResponse.msg @@ -0,0 +1,3 @@ +int8 SKILL_SUCCESS=0 +int8 SKILL_FAILURE=1 +int8 status \ No newline at end of file diff --git a/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/package.xml b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/package.xml new file mode 100644 index 00000000..8ceb38e5 --- /dev/null +++ b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/package.xml @@ -0,0 +1,21 @@ + + + + bt_interfaces_dummy + 0.0.0 + TODO: Package description + ste + TODO: License declaration + + ament_cmake + ament_lint_auto + ament_lint_common + rosidl_interface_packages + rosidl_default_runtime + + rosidl_default_generators + + + ament_cmake + + diff --git a/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/HaltAction.srv b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/HaltAction.srv new file mode 100644 index 00000000..22724dec --- /dev/null +++ b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/HaltAction.srv @@ -0,0 +1,2 @@ +--- +bool is_ok \ No newline at end of file diff --git a/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/ReloadTree.srv b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/ReloadTree.srv new file mode 100644 index 00000000..6b2fc3b0 --- /dev/null +++ b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/ReloadTree.srv @@ -0,0 +1,3 @@ +--- +bool is_ok +string error_msg \ No newline at end of file diff --git a/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/TickAction.srv b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/TickAction.srv new file mode 100644 index 00000000..dc9eed69 --- /dev/null +++ b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/TickAction.srv @@ -0,0 +1,3 @@ +--- +int8 status +bool is_ok \ No newline at end of file diff --git a/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/TickCondition.srv b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/TickCondition.srv new file mode 100644 index 00000000..dc9eed69 --- /dev/null +++ b/examples/overarching_tutorial/ros_interfaces/src/bt_interfaces_dummy/srv/TickCondition.srv @@ -0,0 +1,3 @@ +--- +int8 status +bool is_ok \ No newline at end of file diff --git a/examples/overarching_tutorial/simulation/.docker/build/.gitignore b/examples/overarching_tutorial/simulation/.docker/build/.gitignore deleted file mode 100644 index f59ec20a..00000000 --- a/examples/overarching_tutorial/simulation/.docker/build/.gitignore +++ /dev/null @@ -1 +0,0 @@ -* \ No newline at end of file diff --git a/examples/overarching_tutorial/simulation/README.md b/examples/overarching_tutorial/simulation/README.md index e02ddbf8..51912732 100644 --- a/examples/overarching_tutorial/simulation/README.md +++ b/examples/overarching_tutorial/simulation/README.md @@ -1,7 +1,7 @@ # How to use the simulation > [!WARNING] -> You must be in the `data-model/examples/overarching_tutorial/simulation` folder for this to work. +> You must be in the `data-model/examples/overarching_tutorial` folder for this to work. ## Build @@ -12,16 +12,30 @@ docker build -t convince_tutorial -f .docker/Dockerfile . ## Run ```bash -docker run -it --rm\ - --name convince_tutorial\ - -v /tmp/.X11-unix:/tmp/.X11-unix:rw\ - -v ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority\ - -v ./tutorial_sim:/convince_ws/src/tutorial_sim\ - -v ./.docker/build:/convince_ws/build\ - -e DISPLAY\ - -e QT_X11_NO_MITSHM=1\ - convince_tutorial\ - bash + docker run -it --rm --name convince_tutorial \ + -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ + -v ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority \ + -v ./simulation/tutorial_sim:/convince_ws/src/tutorial_sim \ + -v ./ros_interfaces:/convince_ws/src/ros_interfaces \ + -v ./roaml:/convince_ws/src/roaml \ + -v ./simulation/behavior_tree:/convince_ws/src/behavior_tree \ + -v ./simulation/tutorial_run:/convince_ws/src/tutorial_run \ + -v ./simulation/generated_skill:/convince_ws/src/generated_skill \ + -v ./simulation/bt_nodes:/convince_ws/src/bt_nodes \ + -v ./.docker/build:/convince_ws/build \ + -v ./.docker/install:/convince_ws/install \ + -v ./.docker/log:/convince_ws/log \ + -e DISPLAY -e QT_X11_NO_MITSHM=1 \ + convince_tutorial bash +``` + +If you previously mounted only `build`, the colcon cache can become inconsistent across container restarts +(for example missing `/convince_ws/install/.../ament_index/...` files for Python packages like `tutorial_sim`). +If this happens, clear cache once before re-running: + +```bash +rm -rf ./.docker/build ./.docker/install ./.docker/log +mkdir -p ./.docker/build ./.docker/install ./.docker/log ``` Inside the docker container, run @@ -30,6 +44,93 @@ Inside the docker container, run ros2 run tutorial_sim run ``` +In another shell (attach to the same running container) run: + +```bash +ros2 run tutorial_sim translate_component +``` + +In a third shell (attach to the same running container) run: + +```bash +ros2 run bt_executor btcpp_executor --ros-args -p tree:=src/roaml/policy/bt_tree.xml +``` + +To set a global fallback object (used when BT ports do not provide `object_id`), add +`-p default_object_id:=`: + +The object id must refer to an object that is already spawned in the world and +positioned in the place where you want to pick it from. +Check `simulation/tutorial_sim/worlds/world.yaml` for object ids and their +spawn locations. + +```bash +ros2 run bt_executor btcpp_executor --ros-args \ + -p tree:=src/roaml/policy/bt_tree.xml \ + -p default_object_id:=soda0 +``` + +Don't forget to run the custom skill: + +```bash +ros2 run place_object_skill place_object_skill +``` + +Or launch the full stack in one command: + +```bash +convince_build_packages tutorial_run +source /convince_ws/install/setup.bash +ros2 launch tutorial_run full_simulation.launch.py +``` + +Launch arguments: +- `tree:=...` to select a different BT file. +- `default_object_id:=...` to set the global fallback object. + +Example with both arguments: + +```bash +ros2 launch tutorial_run full_simulation.launch.py \ + tree:=src/roaml/policy/bt_tree_locations.xml \ + default_object_id:=soda0 +``` + +Default BT (if `tree` is not provided): + +```bash +src/roaml/policy/bt_tree.xml +``` + +When `btcpp_executor` finishes (SUCCESS or FAILURE), the launch automatically shuts down all started nodes (`tutorial_sim`, `translate_component`, `place_object_skill`) so no ROS processes are left running. + +> [!WARNING] +> Some simulation actions are probabilistic (for example `pick`), so a BT execution can end in `FAILURE` even when the setup is correct. +> If you see messages like `Simulated pick failure`, rerun the BT or use a tree with retries/fallback handling. +> +> To change action success probabilities, edit: +> - `simulation/tutorial_sim/worlds/world.yaml` under `robots[0].action_execution_options.*.success_probability` +> (`navigate`, `pick`, `place`, `open`, `close`, `detect`). +> - `roaml/environment/world_multiple_locations_w_failures.ascxml` if you want to change the +> probabilistic RoAML model used in the tutorial docs/examples (for example transitions with +> `prob="0.9"` / `prob="0.8"`). + +## Translator + +To make dynamic `Place` object resolution reliable during BT execution, two aligned updates were introduced: + +- `tutorial_sim run` now publishes `RobotState` at a higher rate (`state_pub_rate=5.0`) so object updates are visible in time. +- `tutorial_sim translate_component` now resolves the object using: + 1. current `RobotState.manipulated_object`, + 2. `default_object` fallback, + 3. last seen non-empty `manipulated_object`, + 4. short wait window (`wait_for_object_timeout_sec`) before sending empty object. + +Expected translator logs in the successful case: + +- `Resolved object 'butter0' (type='butter', source='robot_state').` +- `Forwarding Place -> ExecuteTaskAction(type='place', robot='robot', object='butter0')` + ## Usage For example: diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/.clang-format b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/.clang-format new file mode 100644 index 00000000..e0f20998 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/.clang-format @@ -0,0 +1,68 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializers: BeforeComma +BinPackParameters: true +ColumnLimit: 100 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: Never +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false +ReflowComments: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true', + AfterControlStatement: 'true', + AfterEnum : 'true', + AfterFunction : 'true', + AfterNamespace : 'true', + AfterStruct : 'true', + AfterUnion : 'true', + BeforeCatch : 'true', + BeforeElse : 'true', + IndentBraces : 'false', + SplitEmptyFunction: 'false' +} +... diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/CMakeLists.txt b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/CMakeLists.txt new file mode 100644 index 00000000..ee6ea7d2 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.8) +project(bt_executor) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +set(THIS_PACKAGE_DEPS + ament_index_cpp + behaviortree_ros2 + pyrobosim_msgs + rclcpp + bt_interfaces_dummy + overarching_msgs + bt_nodes +) + +# for each dependency, find and include the package +foreach(DEPENDENCY ${THIS_PACKAGE_DEPS}) + find_package(${DEPENDENCY} REQUIRED) +endforeach() + +# create executable +add_executable(btcpp_executor src/btcpp_executor.cpp) + +ament_target_dependencies(btcpp_executor ${THIS_PACKAGE_DEPS}) + +target_include_directories(btcpp_executor PUBLIC + $ + $) + + +# Install executable and trees +install( + DIRECTORY trees + DESTINATION share/${PROJECT_NAME} +) + +install( + TARGETS btcpp_executor + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include/ +) + +ament_export_dependencies(${THIS_PACKAGE_DEPS}) +ament_export_include_directories(include) + +ament_package() diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/battery_nodes.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/battery_nodes.hpp new file mode 100644 index 00000000..251c92b1 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/battery_nodes.hpp @@ -0,0 +1,84 @@ + +#pragma once + +#include +#include + +namespace BT +{ + +class IsBatteryLow : public ConditionNode +{ +public: + IsBatteryLow(const std::string& name, const NodeConfig& config, rclcpp::Logger logger) + : ConditionNode(name, config), logger_(logger) + {} + + static PortsList providedPorts() + { + return { InputPort("low_threshold", 20.0, + "Return SUCCESS if {@battery_level} below this value") }; + } + + // You must override the virtual function tick() + NodeStatus tick() override + { + // Read a value from the blackboard + double battery_level = -1; + double low_threshold = 0; + if(!config().blackboard->get("@battery_level", battery_level)) + { + RCLCPP_ERROR(logger_, "Battery level not available yet"); + return NodeStatus::FAILURE; + } + if(!getInput("low_threshold", low_threshold)) + { + throw RuntimeError("Missing parameter [low_threshold] in IsBatteryLow BT node"); + } + RCLCPP_INFO(logger_, "Battery level: %.1f", battery_level); + return (battery_level < low_threshold) ? NodeStatus::SUCCESS : NodeStatus::FAILURE; + } + +private: + rclcpp::Logger logger_; +}; + +//---------------------------------------------------------------------------------------------------- + +class IsBatteryFull : public ConditionNode +{ +public: + IsBatteryFull(const std::string& name, const NodeConfig& config, rclcpp::Logger logger) + : ConditionNode(name, config), logger_(logger) + {} + + static PortsList providedPorts() + { + return { InputPort("full_threshold", 90.0, + "Return SUCCESS if {@battery_level} above this value") }; + } + + // You must override the virtual function tick() + NodeStatus tick() override + { + // Read a value from the blackboard + double battery_level = -1; + double full_threshold = 0; + if(!config().blackboard->get("@battery_level", battery_level)) + { + RCLCPP_ERROR(logger_, "Battery level not available yet"); + return NodeStatus::FAILURE; + } + if(!getInput("full_threshold", full_threshold)) + { + throw RuntimeError("Missing parameter [full_threshold] in IsBatteryFull BT node"); + } + RCLCPP_INFO(logger_, "Battery level: %.1f", battery_level); + return (battery_level >= full_threshold) ? NodeStatus::SUCCESS : NodeStatus::FAILURE; + } + +private: + rclcpp::Logger logger_; +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/close_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/close_node.hpp new file mode 100644 index 00000000..d6100cba --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/close_node.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class CloseAction : public ExecuteTaskNode +{ +public: + CloseAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("target") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string target; + if(!getInput("target", target) || target.empty()) + { + throw BT::RuntimeError("missing required input [target]"); + } + + // prepare the goal message + action.type = "close"; + action.target_location = target; + return true; + } +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/detect_object_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/detect_object_node.hpp new file mode 100644 index 00000000..3f1425f8 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/detect_object_node.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class DetectObject : public ExecuteTaskNode +{ +public: + DetectObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts( + { BT::InputPort("object"), BT::BidirectionalPort("object_id") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if((!getInput("object", object) || object.empty()) && + (!getInput("object_id", object) || object.empty())) + { + if(!config().blackboard || !config().blackboard->get("default_object_id", object) || + object.empty()) + { + object = "butter0"; + } + RCLCPP_DEBUG(logger(), "[%s] missing [object]/[object_id], using fallback object '%s'", + name().c_str(), object.c_str()); + } + last_detected_object_ = object; + // prepare the goal message + action.type = "detect"; + action.object = object; + return true; + } + + NodeStatus onResultReceived(const ExecutionResult& execution_result) override + { + auto status = ExecuteTaskNode::onResultReceived(execution_result); + if(status == NodeStatus::SUCCESS) + { + setOutput("object_id", last_detected_object_); + } + return status; + } + +private: + std::string last_detected_object_{ "butter0" }; +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/execute_task_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/execute_task_node.hpp new file mode 100644 index 00000000..63468e2f --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/execute_task_node.hpp @@ -0,0 +1,113 @@ +#pragma once + +#include +#include + +namespace BT +{ + +inline const char* resultToStr(pyrobosim_msgs::msg::ExecutionResult result) +{ + switch(result.status) + { + case pyrobosim_msgs::msg::ExecutionResult::UNKNOWN: + return "UNKNOWN"; + case pyrobosim_msgs::msg::ExecutionResult::SUCCESS: + return "SUCCESS"; + case pyrobosim_msgs::msg::ExecutionResult::PRECONDITION_FAILURE: + return "PRECONDITION_FAILURE"; + case pyrobosim_msgs::msg::ExecutionResult::PLANNING_FAILURE: + return "PLANNING_FAILURE"; + case pyrobosim_msgs::msg::ExecutionResult::EXECUTION_FAILURE: + return "EXECUTION_FAILURE"; + case pyrobosim_msgs::msg::ExecutionResult::POSTCONDITION_FAILURE: + return "POSTCONDITION_FAILURE"; + case pyrobosim_msgs::msg::ExecutionResult::INVALID_ACTION: + return "INVALID_ACTION"; + case pyrobosim_msgs::msg::ExecutionResult::CANCELED: + return "CANCELED"; + } + return "UNKNOWN"; +} + +/** + * @brief Base class for all the nodes that execute a task using the ExecuteTaskAction + */ +class ExecuteTaskNode : public RosActionNode +{ +public: + ExecuteTaskNode(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : RosActionNode(name, conf, params) + {} + + using TaskAction = pyrobosim_msgs::msg::TaskAction; + using ExecutionResult = pyrobosim_msgs::msg::ExecutionResult; + + // method that sends the goal. Must be implemented by the derived class + virtual bool setGoal(TaskAction& action) = 0; + + // virtual method processing the result. Can be overridden by the derived class + virtual NodeStatus onResultReceived(const ExecutionResult& execution_result); + + // default implementation of the onFailure callback. Can be overridden by the derived class + NodeStatus onFailure(ActionNodeErrorCode error) override; + + // this helper function will add the port "robotID" and "action_name" by default to every node. + // They both have default values and don't need to be specified in the XML files + static BT::PortsList appendProvidedPorts(PortsList other_ports); + +private: + bool setGoal(Goal& goal) override final + { + // initialize the field goal.action.robot + getInput("robotID", goal.action.robot); + return setGoal(goal.action); + } + + NodeStatus onResultReceived(const WrappedResult& wr) override final + { + return onResultReceived(wr.result->execution_result); + } +}; + +//------------------------------------------------------------ +//------------------------------------------------------------ +//------------------------------------------------------------ + +inline NodeStatus ExecuteTaskNode::onResultReceived(const ExecutionResult& execution_result) +{ + if(execution_result.status != ExecutionResult::SUCCESS) + { + RCLCPP_ERROR(logger(), "[%s] failed with error: %s. Message: %s", name().c_str(), + resultToStr(execution_result), execution_result.message.c_str()); + return NodeStatus::FAILURE; + } + RCLCPP_INFO(logger(), "[%s] succeeded. Message: %s", name().c_str(), + execution_result.message.c_str()); + return NodeStatus::SUCCESS; +} + +inline NodeStatus ExecuteTaskNode::onFailure(ActionNodeErrorCode error) +{ + // In this simulation stack, a just-finished action may still be marked as executing + // for a short time, causing the next goal to be aborted transiently. + // Keep running so the action can be retried at the next BT tick. + if(error == ActionNodeErrorCode::ACTION_ABORTED) + { + RCLCPP_WARN(logger(), "[%s] action aborted, retrying on next tick", name().c_str()); + return NodeStatus::RUNNING; + } + + RCLCPP_ERROR(logger(), "[%s] failed with error: %s", name().c_str(), toStr(error)); + return NodeStatus::FAILURE; +} + +inline BT::PortsList ExecuteTaskNode::appendProvidedPorts(PortsList other_ports) +{ + PortsList ports = { InputPort("action_name", "execute_action", "Action server name"), + InputPort("robotID", "robot", "Robot name") }; + ports.insert(other_ports.begin(), other_ports.end()); + return ports; +} + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp new file mode 100644 index 00000000..712680df --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp @@ -0,0 +1,41 @@ + +#pragma once + +#include +#include + +namespace BT +{ + +class GetCurrentLocation : public SyncActionNode +{ +public: + GetCurrentLocation(const std::string& name, const NodeConfig& config, rclcpp::Logger logger) + : SyncActionNode(name, config), logger_(logger) + {} + + static PortsList providedPorts() + { + return { OutputPort("location", "The robot's current location.") }; + } + + // You must override the virtual function tick() + NodeStatus tick() override + { + // Read a value from the blackboard + std::string current_location = ""; + if(!config().blackboard->get("@last_visited_location", current_location)) + { + RCLCPP_ERROR(logger_, "Robot location not available yet"); + return NodeStatus::FAILURE; + } + RCLCPP_INFO(logger_, "Robot location level: %s", current_location.c_str()); + setOutput("location", current_location); + return NodeStatus::SUCCESS; + } + +private: + rclcpp::Logger logger_; +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/get_next_location_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/get_next_location_node.hpp new file mode 100644 index 00000000..b266df76 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/get_next_location_node.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include +#include + +namespace BT +{ + +class DecoratorGetNextLocation : public SyncActionNode +{ +public: + DecoratorGetNextLocation(const std::string& name, const NodeConfig& config, + rclcpp::Logger logger) + : SyncActionNode(name, config), logger_(logger) + {} + + static PortsList providedPorts() + { + return { OutputPort("location", "The next location to visit.") }; + } + + NodeStatus tick() override + { + // Use location names that are valid in the tutorial_sim world model. + static const std::array kSearchLocations = { + "fridge", "table", "kitchen_table", "dining_table"}; + + int search_index = 0; + const bool has_search_index = config().blackboard->get("@next_location_idx", search_index); + if(!has_search_index) + { + // On first tick, start from the BT-selected current location if available. + std::string current_location; + if(config().blackboard->get("@current_location", current_location)) + { + const auto it = std::find(kSearchLocations.begin(), kSearchLocations.end(), current_location); + if(it != kSearchLocations.end()) + { + search_index = static_cast(std::distance(kSearchLocations.begin(), it)); + } + } + } + + if(search_index < 0) + { + search_index = 0; + } + + const int bounded_index = search_index % static_cast(kSearchLocations.size()); + const std::string& next_location = kSearchLocations[bounded_index]; + + config().blackboard->set("@next_location_idx", + (bounded_index + 1) % static_cast(kSearchLocations.size())); + + setOutput("location", next_location); + RCLCPP_INFO(logger_, "Next search location: %s", next_location.c_str()); + return NodeStatus::SUCCESS; + } + +private: + rclcpp::Logger logger_; +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/navigate_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/navigate_node.hpp new file mode 100644 index 00000000..cf656281 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/navigate_node.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class NavigateAction : public ExecuteTaskNode +{ +public: + NavigateAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts( + { BT::InputPort("target"), BT::InputPort("location") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string location; + if((!getInput("target", location) || location.empty()) && + (!getInput("location", location) || location.empty())) + { + RCLCPP_ERROR(logger(), "[%s] missing required input [target] or [location]", name().c_str()); + return false; + } + + // Accept quoted literals from BT XML, e.g. 'fridge' or "fridge". + if(location.size() >= 2) + { + const char first = location.front(); + const char last = location.back(); + if((first == '\'' && last == '\'') || (first == '"' && last == '"')) + { + location = location.substr(1, location.size() - 2); + } + } + + // prepare the goal message + action.type = "navigate"; + action.target_location = location; + return true; + } +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/open_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/open_node.hpp new file mode 100644 index 00000000..cba89535 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/open_node.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class OpenAction : public ExecuteTaskNode +{ +public: + OpenAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("target") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string target; + if(!getInput("target", target) || target.empty()) + { + throw BT::RuntimeError("missing required input [target]"); + } + + // prepare the goal message + action.type = "open"; + action.target_location = target; + return true; + } +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/pick_object_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/pick_object_node.hpp new file mode 100644 index 00000000..d57eeeac --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/pick_object_node.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class PickObject : public ExecuteTaskNode +{ +public: + PickObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts( + { BT::InputPort("object"), BT::InputPort("object_id") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if((!getInput("object", object) || object.empty()) && + (!getInput("object_id", object) || object.empty())) + { + if(!config().blackboard || !config().blackboard->get("default_object_id", object) || + object.empty()) + { + object = "butter0"; + } + RCLCPP_DEBUG(logger(), "[%s] missing [object]/[object_id], using fallback object '%s'", + name().c_str(), object.c_str()); + } + // prepare the goal message + action.type = "pick"; + action.object = object; + return true; + } +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/place_object_node.hpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/place_object_node.hpp new file mode 100644 index 00000000..3a14590d --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/include/pyrobosim_btcpp/nodes/place_object_node.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class PlaceObject : public ExecuteTaskNode +{ +public: + PlaceObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts( + { BT::InputPort("object"), BT::InputPort("object_id") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if((!getInput("object", object) || object.empty()) && + (!getInput("object_id", object) || object.empty())) + { + RCLCPP_WARN(logger(), "[%s] missing [object]/[object_id], defaulting to butter0", + name().c_str()); + object = "butter0"; + } + // prepare the goal message + action.type = "place"; + action.object = object; + return true; + } +}; + +} // namespace BT diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/package.xml b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/package.xml new file mode 100644 index 00000000..1bb50a46 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/package.xml @@ -0,0 +1,26 @@ + + + + bt_executor + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_index_cpp + behaviortree_ros2 + pyrobosim_msgs + rclcpp + bt_interfaces_dummy + bt_nodes + overarching_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/src/btcpp_executor.cpp b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/src/btcpp_executor.cpp new file mode 100644 index 00000000..adf83bca --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/src/btcpp_executor.cpp @@ -0,0 +1,252 @@ +#include +#include +#include +#include + +// ROS includes +#include +#include +#include + +// BTCPP includes +#include +#include +#include +#include +#include +#include +#include + +// BTCPP nodes in this package +#include "pyrobosim_btcpp/nodes/battery_nodes.hpp" +#include "pyrobosim_btcpp/nodes/get_current_location_node.hpp" +#include "pyrobosim_btcpp/nodes/get_next_location_node.hpp" +#include "pyrobosim_btcpp/nodes/open_node.hpp" +#include "pyrobosim_btcpp/nodes/close_node.hpp" +#include "pyrobosim_btcpp/nodes/detect_object_node.hpp" +#include "pyrobosim_btcpp/nodes/navigate_node.hpp" +#include "pyrobosim_btcpp/nodes/pick_object_node.hpp" +#include "pyrobosim_btcpp/nodes/place_object_node.hpp" +#include +// TO_WORKSHOP_USER: add here the include to your custom actions, if you have any + +std::filesystem::path GetFilePath(const std::string& filename) +{ + // check first the given path + if(std::filesystem::exists(filename)) + { + return filename; + } + // try appending the package directory + const std::string package_dir = ament_index_cpp::get_package_share_directory("bt_executor"); + const auto package_path = std::filesystem::path(package_dir) / filename; + if(std::filesystem::exists(package_path)) + { + return package_path; + } + throw std::runtime_error("File not found: " + filename); +} + +int main(int argc, char** argv) +{ + // Create a ROS Node + rclcpp::init(argc, argv); + // Avoid duplicate rosout publisher warnings when multiple same-name nodes + // are created internally during BT/plugin lifecycle. + auto node_options = rclcpp::NodeOptions().enable_rosout(false); + auto nh = std::make_shared("btcpp_executor", node_options); + + nh->declare_parameter("tree", rclcpp::PARAMETER_STRING); + nh->declare_parameter("tree_id", ""); + nh->declare_parameter("default_object_id", "butter0"); + nh->declare_parameter("save_model_only", false); + + std::string tree_filename; + nh->get_parameter("tree", tree_filename); + + std::string tree_id; + nh->get_parameter("tree_id", tree_id); + + std::string default_object_id; + nh->get_parameter("default_object_id", default_object_id); + + bool save_model = false; + nh->get_parameter("save_model_only", save_model); + + if(tree_filename.empty() && !save_model) + { + RCLCPP_FATAL(nh->get_logger(), "Missing parameter 'tree' with the path to the Behavior Tree " + "XML file"); + return 1; + } + + //---------------------------------- + // register all the actions in the factory + BT::BehaviorTreeFactory factory; + + // all the actions are done using the same Action Server. + // Therefore single RosNodeParams will do. + BT::RosNodeParams params; + params.nh = nh; + params.default_port_value = "execute_action"; + params.wait_for_server_timeout = std::chrono::seconds(5); + + factory.registerNodeType("IsBatteryLow", nh->get_logger()); + factory.registerNodeType("IsBatteryFull", nh->get_logger()); + factory.registerNodeType("GetCurrentLocation", nh->get_logger()); + factory.registerNodeType("GetNextLocation", nh->get_logger()); + factory.registerNodeType("Close", params); + factory.registerNodeType("DetectObject", params); + // Keep legacy "Navigate" and add RoAML-aligned "NavigateToLocation". + factory.registerNodeType("Navigate", params); + factory.registerNodeType("NavigateToLocation", params); + factory.registerNodeType("Open", params); + factory.registerNodeType("PickObject", params); + factory.registerNodeType("PlaceObject", params); + factory.registerNodeType("ROS2Action"); + if(factory.manifests().count("Sequence") == 0) + { + factory.registerBuilder( + "Sequence", [](const std::string& name, const BT::NodeConfig& config) { + return std::make_unique(name, false, config); + }); + } + if(factory.manifests().count("Fallback") == 0) + { + factory.registerBuilder( + "Fallback", [](const std::string& name, const BT::NodeConfig&) { + return std::make_unique(name, false); + }); + } + if(factory.manifests().count("Parallel") == 0) + { + factory.registerBuilder( + "Parallel", [](const std::string& name, const BT::NodeConfig& config) { + return std::make_unique(name, config); + }); + } + + // TO_WORKSHOP_USER: register here more Nodes, if you decided to implement your own + + // Optionally, we can save the model of the Nodes registered in the factory + if(save_model) + { + std::string xml_models = BT::writeTreeNodesModelXML(factory); + std::ofstream of("pyrobosim_btcpp_model.xml"); + of << xml_models; + RCLCPP_INFO(nh->get_logger(), "XML model of the tree saved in pyrobosim_btcpp_model.xml"); + return 0; + } + const std::filesystem::path filepath = GetFilePath(tree_filename); + + //---------------------------------- + // load a tree and execute + factory.registerBehaviorTreeFromFile(filepath.string()); + + const auto registered_tree_ids = factory.registeredBehaviorTrees(); + if(registered_tree_ids.empty()) + { + RCLCPP_FATAL(nh->get_logger(), + "No found in XML file: %s. " + "This often happens if you pass a TreeNodesModel XML (node model) instead of an executable tree. " + "Try: /convince_ws/src/behavior_tree/bt_executor/trees/navigation_demo.xml", + filepath.c_str()); + return 1; + } + + if(tree_id.empty()) + { + if(registered_tree_ids.size() == 1) + { + tree_id = registered_tree_ids.front(); + RCLCPP_INFO(nh->get_logger(), "Parameter tree_id is empty. Using the only tree found: '%s'", + tree_id.c_str()); + } + else + { + RCLCPP_FATAL(nh->get_logger(), + "Parameter tree_id is empty and multiple trees are registered. " + "Set -p tree_id:= to choose one."); + std::ostringstream oss; + for(const auto& id : registered_tree_ids) + { + oss << " - " << id << "\n"; + } + RCLCPP_FATAL(nh->get_logger(), "Registered trees:\n%s", oss.str().c_str()); + return 1; + } + } + + if(std::find(registered_tree_ids.begin(), registered_tree_ids.end(), tree_id) == + registered_tree_ids.end()) + { + if(registered_tree_ids.size() == 1) + { + const auto& only_id = registered_tree_ids.front(); + RCLCPP_WARN(nh->get_logger(), "Can't find a tree with name: '%s'. Using the only tree found: '%s'", + tree_id.c_str(), only_id.c_str()); + tree_id = only_id; + } + else + { + std::ostringstream oss; + for(const auto& id : registered_tree_ids) + { + oss << " - " << id << "\n"; + } + RCLCPP_FATAL(nh->get_logger(), "Can't find a tree with name: '%s'. Registered trees:\n%s", + tree_id.c_str(), oss.str().c_str()); + return 1; + } + } + + // the global blackboard patterns is explained here: + // https://www.behaviortree.dev/docs/tutorial-advanced/tutorial_16_global_blackboard + auto global_blackboard = BT::Blackboard::create(); + global_blackboard->set("default_object_id", default_object_id); + BT::Tree tree = factory.createTree(tree_id, BT::Blackboard::create(global_blackboard)); + + // This will add console messages for each action and condition executed + BT::StdCoutLogger console_logger(tree); + console_logger.enableTransitionToIdle(false); + + BT::Groot2Publisher groot2_publisher(tree, 5555); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(nh); + + bool state_received = false; + + // create a subscriber to /robot/robot_state to update the blackboard + // Note that the prefix/namespace used here is "/robot", for the purpose of the + // workshop, but this identifier may change in pyrobosim. + auto robot_state_subscriber = nh->create_subscription( + "/robot/robot_state", 10, + [global_blackboard, &state_received](const pyrobosim_msgs::msg::RobotState::SharedPtr msg) { + global_blackboard->set("battery_level", msg->battery_level); + global_blackboard->set("holding_object", msg->holding_object); + global_blackboard->set("last_visited_location", msg->last_visited_location); + global_blackboard->set("executing_action", msg->executing_action); + state_received = true; + }); + + // wait for the first message to be received by robot_state_subscriber + while(!state_received) + { + executor.spin_once(std::chrono::milliseconds(10)); + } + + // This is the "main loop". Execution is completed once the tick() method returns SUCCESS of FAILURE + BT::NodeStatus status = BT::NodeStatus::RUNNING; + while(rclcpp::ok() && status == BT::NodeStatus::RUNNING) + { + // tick once the tree + status = tree.tickOnce(); + // Spin to update robot_state_subscriber + executor.spin_once(std::chrono::milliseconds(1)); + } + + std::cout << "Execution completed. Result: " << BT::toStr(status) << std::endl; + + return 0; +} \ No newline at end of file diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/navigation_demo.xml b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/navigation_demo.xml new file mode 100644 index 00000000..41be7bb1 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/navigation_demo.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/pyrobosim_btcpp_model.xml b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/pyrobosim_btcpp_model.xml new file mode 100644 index 00000000..3bff3a18 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/pyrobosim_btcpp_model.xml @@ -0,0 +1,43 @@ + + + + + Robot name + Action server name + + + Robot name + + Action server name + + + The robot's current location. + + + Return SUCCESS if {@battery_level} above this value + + + Return SUCCESS if {@battery_level} below this value + + + + Robot name + Action server name + + + + Robot name + Action server name + + + Robot name + + Action server name + + + Robot name + + Action server name + + + diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/roscon2024.btproj b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/roscon2024.btproj new file mode 100644 index 00000000..33e4abdf --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/roscon2024.btproj @@ -0,0 +1,46 @@ + + + + + + + + Robot name + Action server name + + + Robot name + + Action server name + + + The robot's current location. + + + Return SUCCESS if {@battery_level} above this value + + + Return SUCCESS if {@battery_level} below this value + + + + Robot name + Action server name + + + + Robot name + Action server name + + + Robot name + + Action server name + + + Robot name + + Action server name + + + diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem1.xml b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem1.xml new file mode 100644 index 00000000..84985eac --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem1.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem2.xml b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem2.xml new file mode 100644 index 00000000..065709b0 --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem2.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem3.xml b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem3.xml new file mode 100644 index 00000000..453282ab --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem3.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem4.xml b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem4.xml new file mode 100644 index 00000000..e075ba3b --- /dev/null +++ b/examples/overarching_tutorial/simulation/behavior_tree/bt_executor/trees/solution/problem4.xml @@ -0,0 +1,178 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/overarching_tutorial/simulation/bt_nodes/CMakeLists.txt b/examples/overarching_tutorial/simulation/bt_nodes/CMakeLists.txt new file mode 100644 index 00000000..508bb6b0 --- /dev/null +++ b/examples/overarching_tutorial/simulation/bt_nodes/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.8) +project(bt_nodes) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(rclcpp REQUIRED) +find_package(Boost COMPONENTS coroutine QUIET) +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(behaviortree_cpp REQUIRED) + +add_library(${PROJECT_NAME} + ${CMAKE_CURRENT_SOURCE_DIR}/include/ROS2Action.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/ROS2Action.cpp + ) + +set(dependencies bt_interfaces_dummy rclcpp behaviortree_cpp) + +# this line to exports the library +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) + + find_package(bt_interfaces_dummy REQUIRED) + ament_target_dependencies(${PROJECT_NAME} ${dependencies}) + ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) + + + install( + DIRECTORY include/ + DESTINATION include + ) + + install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include + ) + + + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/examples/overarching_tutorial/simulation/bt_nodes/include/ROS2Action.h b/examples/overarching_tutorial/simulation/bt_nodes/include/ROS2Action.h new file mode 100644 index 00000000..f3adb3ff --- /dev/null +++ b/examples/overarching_tutorial/simulation/bt_nodes/include/ROS2Action.h @@ -0,0 +1,49 @@ +/****************************************************************************** + * * + * Copyright (C) 2023 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ +/** + * @file ROS2Action.h + * @authors: Stefano Bernagozzi + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include +#include +// namespace BT +// { +// using NodeConfiguration = NodeConfig; +// using AsyncActionNode = ThreadedAction; +// using Optional = Expected; +// } +class ROS2Action : public BT::ActionNodeBase +{ +public: + ROS2Action (const std::string& name, const BT::NodeConfig &config); + int sendTickToSkill(); + void halt() override; + BT::NodeStatus tick() override; + bool init(); + bool stop(); + static BT::PortsList providedPorts(); + +private: + std::mutex m_requestMutex; + rclcpp::Client::SharedPtr m_clientTick; + rclcpp::Client::SharedPtr m_clientHalt; + std::shared_ptr m_node; + std::string m_name; + int m_tick_count{0}; + double m_average_time{0.0}; + std::string m_suffixMonitor; +}; + diff --git a/examples/overarching_tutorial/simulation/bt_nodes/package.xml b/examples/overarching_tutorial/simulation/bt_nodes/package.xml new file mode 100644 index 00000000..1bcc191f --- /dev/null +++ b/examples/overarching_tutorial/simulation/bt_nodes/package.xml @@ -0,0 +1,19 @@ + + + + bt_nodes + 0.0.0 + TODO: Package description + Stefano Bernagozzi + TODO: License declaration + + ament_cmake + bt_interfaces_dummy + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/examples/overarching_tutorial/simulation/bt_nodes/src/ROS2Action.cpp b/examples/overarching_tutorial/simulation/bt_nodes/src/ROS2Action.cpp new file mode 100644 index 00000000..611dc584 --- /dev/null +++ b/examples/overarching_tutorial/simulation/bt_nodes/src/ROS2Action.cpp @@ -0,0 +1,159 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ +/** + * @file ROS2Action.cpp + * @authors: Stefano Bernagozzi + */ + +#include // std::chrono::seconds + +#include +#define VERBOSE_LOGGING +ROS2Action::ROS2Action(const std::string& name, const BT::NodeConfig& config) : + ActionNodeBase(name, config) +{ + + BT::Expected is_monitored = BT::TreeNode::getInput("isMonitored"); + if (is_monitored && is_monitored.value() == "true") + { + m_suffixMonitor = "_mon"; + } + BT::Expected interface = BT::TreeNode::getInput("interface"); + bool ok = init(); + + if(!ok) + { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Something went wrong in the node init() of %s", ActionNodeBase::name().c_str()); + } +} + + +int ROS2Action::sendTickToSkill() +{ + auto msg = bt_interfaces_dummy::msg::ActionResponse(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending tick to %s ", ActionNodeBase::name().c_str()); + auto request = std::make_shared(); + while (!m_clientTick->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service TickAction. Exiting."); + return msg.SKILL_FAILURE; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service TickAction in %s not available, waiting again...", ActionNodeBase::name().c_str()); + } + auto result = m_clientTick->async_send_request(request); + std::this_thread::sleep_for (std::chrono::milliseconds(100)); + if (rclcpp::spin_until_future_complete(m_node, result) == + rclcpp::FutureReturnCode::SUCCESS) { + return result.get()->status; + } + return msg.SKILL_FAILURE; +} + + +BT::NodeStatus ROS2Action::tick() +{ + std::lock_guard lock(m_requestMutex); + auto message = bt_interfaces_dummy::msg::ActionResponse(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Node %s sending tick to skill", ActionNodeBase::name().c_str()); + auto time_start = std::chrono::high_resolution_clock::now(); + auto status = sendTickToSkill(); + auto time_end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(time_end - time_start); + #ifdef VERBOSE_LOGGING + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Node %s tick to skill took %ld microseconds", ActionNodeBase::name().c_str(), duration.count()); + m_tick_count++; + if (m_tick_count > 4) + { + m_average_time = (duration.count() + (m_tick_count - 1-4) * m_average_time) / (m_tick_count-4); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Node %s average tick time %f, number of ticks %d", ActionNodeBase::name().c_str(), m_average_time, m_tick_count); + } + #endif + + switch (status) { + case message.SKILL_RUNNING: + return BT::NodeStatus::RUNNING; + case message.SKILL_SUCCESS: + return BT::NodeStatus::SUCCESS; + case message.SKILL_FAILURE: + return BT::NodeStatus::FAILURE; + default: + break; + } + return BT::NodeStatus::FAILURE; +} + + + +BT::PortsList ROS2Action::providedPorts() +{ + return { BT::InputPort("interface"), + BT::InputPort("isMonitored"), + BT::InputPort("service_name_tick"), + BT::InputPort("service_name_halt") }; +} + +void ROS2Action::halt() +{ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Node %s sending halt to skill@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@", ActionNodeBase::name().c_str()); + + bool success = false; + do { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Node %s sending halt to skill", ActionNodeBase::name().c_str()); + auto request = std::make_shared(); + while (!m_clientHalt->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service TickAction. Exiting."); + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "%s service TickAction not available, waiting again...", ActionNodeBase::name().c_str()); + } + auto result = m_clientHalt->async_send_request(request); + // std::this_thread::sleep_for (std::chrono::milliseconds(100)); + if (rclcpp::spin_until_future_complete(m_node, result) == + rclcpp::FutureReturnCode::SUCCESS) { + success = true; + } + } while (!success); +} + + + +bool ROS2Action::init() +{ + + if(!rclcpp::ok()) + { + rclcpp::init(/*argc*/ 0, /*argv*/ nullptr); + } + + m_node = rclcpp::Node::make_shared(ActionNodeBase::name()+ "ActionLeaf"); + + std::string tick_service = ActionNodeBase::name() + "Skill/tick" + m_suffixMonitor; + std::string halt_service = ActionNodeBase::name() + "Skill/halt" + m_suffixMonitor; + + if (auto tick_input = BT::TreeNode::getInput("service_name_tick")) + { + tick_service = tick_input.value(); + } + if (auto halt_input = BT::TreeNode::getInput("service_name_halt")) + { + halt_service = halt_input.value(); + } + + m_clientTick = m_node->create_client(tick_service); + m_clientHalt = m_node->create_client(halt_service); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"),"name -- " << ActionNodeBase::name() << " -- suffixmonitor " << m_suffixMonitor); + + return true; + +} + + +bool ROS2Action::stop() +{ + rclcpp::shutdown(); + return 0; +} diff --git a/examples/overarching_tutorial/simulation/dependencies/BehaviorTree.ROS2 b/examples/overarching_tutorial/simulation/dependencies/BehaviorTree.ROS2 new file mode 160000 index 00000000..6c6aa078 --- /dev/null +++ b/examples/overarching_tutorial/simulation/dependencies/BehaviorTree.ROS2 @@ -0,0 +1 @@ +Subproject commit 6c6aa078ee7bc52fec98984bed4964556abf5beb diff --git a/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/CMakeLists.txt b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/CMakeLists.txt new file mode 100644 index 00000000..922afca0 --- /dev/null +++ b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/CMakeLists.txt @@ -0,0 +1,69 @@ +# Autogenerated by model2code (https://github.com/convince-project/model2code) +# File autogenerated at 2026-02-17 16:41:13 CET with M2C version 1.0.0 +# This is an automatically generated file. + +cmake_minimum_required(VERSION 3.16) +project(place_object_skill) +# set(CMAKE_CXX_STANDARD 20) +# set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) +find_package(bt_interfaces_dummy REQUIRED) + +find_package(overarching_msgs REQUIRED) +find_package(Qt6 COMPONENTS Core Scxml StateMachine REQUIRED) + +add_executable(${PROJECT_NAME} ) + +if (NOT Qt6_FOUND) + message("qt6 not found") +endif() + +# find dependencies +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +ament_target_dependencies(${PROJECT_NAME} + std_msgs + bt_interfaces_dummy + rclcpp + rclcpp_action + overarching_msgs + + ) +target_link_libraries(${PROJECT_NAME} Qt6::Core Qt6::Scxml Qt6::StateMachine) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) +target_sources( ${PROJECT_NAME} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/PlaceObjectSkill.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/PlaceObjectSkill.h + ) + + +install(TARGETS ${PROJECT_NAME} +DESTINATION lib/${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +qt6_add_statecharts(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/src/PlaceObjectSkillSM.scxml) + +ament_package() diff --git a/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/include/PlaceObjectSkill.h b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/include/PlaceObjectSkill.h new file mode 100644 index 00000000..f57d4d98 --- /dev/null +++ b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/include/PlaceObjectSkill.h @@ -0,0 +1,81 @@ +// Autogenerated by model2code (https://github.com/convince-project/model2code) +// File autogenerated at 2026-02-17 16:41:13 CET with M2C version 1.0.0 +// This is an automatically generated file. + +# pragma once + +#include +#include +#include +#include "rclcpp_action/rclcpp_action.hpp" +#include "PlaceObjectSkillSM.h" +#include +#include + + + +#include +#include + +#include + +#define SERVICE_TIMEOUT 8 +#define SKILL_SUCCESS 0 +#define SKILL_FAILURE 1 +#define SKILL_RUNNING 2 + +enum class Status{ + undefined, + running, + success, + failure +}; + +class PlaceObjectSkill +{ +public: + PlaceObjectSkill(std::string name ); + ~PlaceObjectSkill(); + + bool start(int argc, char * argv[]); + static void spin(std::shared_ptr node); + + void tick( [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response); + + void halt( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response); + + + +private: + std::shared_ptr m_threadSpin; + std::shared_ptr m_node; + std::mutex m_requestMutex; + std::string m_name; + PlaceObjectSkillAction m_stateMachine; + std::atomic m_tickResult{Status::undefined}; + rclcpp::Service::SharedPtr m_tickService; + std::atomic m_haltResult{false}; + rclcpp::Service::SharedPtr m_haltService; + + + + + + std::shared_ptr m_node_action; + std::mutex m_actionMutex; + std::mutex m_feedbackMutex; + rclcpp_action::Client::SendGoalOptions m_send_goal_options; + rclcpp_action::Client::SharedPtr m_actionClient; + void goal_response_callback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle); + void send_goal(overarching_msgs::action::Place::Goal); + void feedback_callback( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback); + void result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); + + + +}; + diff --git a/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/package.xml b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/package.xml new file mode 100644 index 00000000..107fa016 --- /dev/null +++ b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/package.xml @@ -0,0 +1,29 @@ + + + + + + + place_object_skill + 0.0.0 + Package description + + License declaration + + ament_cmake + bt_interfaces_dummy + + overarching_msgs + std_msgs + + ament_lint_auto + ament_lint_common + rosidl_interface_packages + rosidl_default_runtime + + rosidl_default_generators + + + ament_cmake + + diff --git a/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/PlaceObjectSkill.cpp b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/PlaceObjectSkill.cpp new file mode 100644 index 00000000..98c21f17 --- /dev/null +++ b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/PlaceObjectSkill.cpp @@ -0,0 +1,291 @@ +// Autogenerated by model2code (https://github.com/convince-project/model2code) +// File autogenerated at 2026-02-17 16:41:13 CET with M2C version 1.0.0 +// This is an automatically generated file. + +#include "PlaceObjectSkill.h" +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +template +T convert(const std::string& str) { + if constexpr (std::is_same_v) { + return std::stoi(str); + } else if constexpr (std::is_same_v) { + return std::stod(str); + } else if constexpr (std::is_same_v) { + return std::stof(str); + } + else if constexpr (std::is_same_v) { + if (str == "true" || str == "1") { + return true; + } else if (str == "false" || str == "0") { + return false; + } else { + throw std::invalid_argument("Invalid boolean value"); + } + } + else if constexpr (std::is_same_v) { + return str; + } + else { + throw std::invalid_argument("Unsupported type conversion"); + } +} + +PlaceObjectSkill::PlaceObjectSkill(std::string name ) : + m_name(std::move(name)) +{ + +} + +PlaceObjectSkill::~PlaceObjectSkill() +{ + //std::cout << "DEBUG: Invoked destructor of PlaceObjectSkill" << std::endl; + m_threadSpin->join(); +} + +void PlaceObjectSkill::spin(std::shared_ptr node) +{ + rclcpp::spin(node); + rclcpp::shutdown(); + QCoreApplication::quit(); + //std::cout << "DEBUG: PlaceObjectSkill::spin successfully ended" << std::endl; +} + +bool PlaceObjectSkill::start(int argc, char*argv[]) +{ + if(!rclcpp::ok()) + { + rclcpp::init(/*argc*/ argc, /*argv*/ argv); + } + + m_node = rclcpp::Node::make_shared(m_name + "Skill"); + RCLCPP_DEBUG_STREAM(m_node->get_logger(), "PlaceObjectSkill::start"); + std::cout << "DEBUG: PlaceObjectSkill::start" << std::endl; + + + m_tickService = m_node->create_service(m_name + "Skill/tick", + std::bind(&PlaceObjectSkill::tick, + this, + std::placeholders::_1, + std::placeholders::_2)); + m_tickService->configure_introspection(m_node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + + m_haltService = m_node->create_service(m_name + "Skill/halt", + std::bind(&PlaceObjectSkill::halt, + this, + std::placeholders::_1, + std::placeholders::_2)); + m_haltService->configure_introspection(m_node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + + m_actionClient = rclcpp_action::create_client(m_node, "/PlaceComponet/Place"); + m_send_goal_options.goal_response_callback = std::bind(&PlaceObjectSkill::goal_response_callback, this, std::placeholders::_1); + m_send_goal_options.feedback_callback = std::bind(&PlaceObjectSkill::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + m_send_goal_options.result_callback = std::bind(&PlaceObjectSkill::result_callback, this, std::placeholders::_1); + + + + + m_stateMachine.connectToEvent("TICK_RESPONSE", [this]([[maybe_unused]]const QScxmlEvent & event){ + RCLCPP_INFO(m_node->get_logger(), "PlaceObjectSkill::tickReturn %s", event.data().toMap()["status"].toString().toStdString().c_str()); + std::string result = event.data().toMap()["status"].toString().toStdString(); + if (result == std::to_string(SKILL_SUCCESS) ) + { + m_tickResult.store(Status::success); + } + else if (result == std::to_string(SKILL_RUNNING) ) + { + m_tickResult.store(Status::running); + } + else if (result == std::to_string(SKILL_FAILURE) ) + { + m_tickResult.store(Status::failure); + } + }); + + m_stateMachine.connectToEvent("HALT_RESPONSE", [this]([[maybe_unused]]const QScxmlEvent & event){ + RCLCPP_INFO(m_node->get_logger(), "PlaceObjectSkill::haltresponse"); + m_haltResult.store(true); + }); + + m_stateMachine.connectToEvent("PlaceComponet.Place.SendGoal", [this]([[maybe_unused]]const QScxmlEvent & event){ + RCLCPP_INFO(m_node->get_logger(), "PlaceObjectSkill::PlaceComponet.Place.SendGoal"); + RCLCPP_INFO(m_node->get_logger(), "calling send goal"); + std::shared_ptr nodePlace = rclcpp::Node::make_shared(m_name + "SkillNodePlace"); + rclcpp_action::Client::SharedPtr clientPlace = + rclcpp_action::create_client(nodePlace, "/PlaceComponet/Place"); + overarching_msgs::action::Place::Goal goal_msg; + + send_goal(goal_msg); + RCLCPP_INFO(m_node->get_logger(), "done send goal"); + }); + m_stateMachine.connectToEvent("PlaceComponet.Place.ResultRequest", [this]([[maybe_unused]]const QScxmlEvent & event){ + RCLCPP_INFO(m_node->get_logger(), "PlaceObjectSkill::PlaceComponet.Place.ResultRequest"); + std::shared_ptr nodePlace = rclcpp::Node::make_shared(m_name + "SkillNodePlace"); + rclcpp_action::Client::SharedPtr clientPlace = + rclcpp_action::create_client(nodePlace, "/PlaceComponet/GoToPoi"); + RCLCPP_INFO(m_node->get_logger(), "result request"); + }); + m_stateMachine.connectToEvent("PlaceComponet.Place.Feedback", [this]([[maybe_unused]]const QScxmlEvent & event){ + RCLCPP_INFO(m_node->get_logger(), "PlaceComponet.Place.Feedback"); + QVariantMap data; + m_feedbackMutex.lock(); + + m_feedbackMutex.unlock(); + m_stateMachine.submitEvent("PlaceComponet.Place.FeedbackReturn", data); + RCLCPP_INFO(m_node->get_logger(), "PlaceComponet.Place.FeedbackReturn"); + }); + + + + + m_stateMachine.start(); + m_threadSpin = std::make_shared(spin, m_node); + + return true; +} + +void PlaceObjectSkill::tick( [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response) +{ + std::lock_guard lock(m_requestMutex); + RCLCPP_INFO(m_node->get_logger(), "PlaceObjectSkill::tick"); + m_tickResult.store(Status::undefined); + m_stateMachine.submitEvent("CMD_TICK"); + + int load_counter=0; + auto start_timer = std::chrono::steady_clock::now(); + while(m_tickResult.load()== Status::undefined) { + std::this_thread::sleep_for (std::chrono::milliseconds(5)); + load_counter++; + } + auto end_timer = std::chrono::steady_clock::now(); + auto duration_ms = std::chrono::duration_cast(end_timer - start_timer).count(); + switch(m_tickResult.load()) + { + case Status::running: + response->status = SKILL_RUNNING; + break; + case Status::failure: + response->status = SKILL_FAILURE; + break; + case Status::success: + response->status = SKILL_SUCCESS; + break; + case Status::undefined: + response->status = SKILL_FAILURE; + break; + } + RCLCPP_INFO(m_node->get_logger(), "PlaceObjectSkill::tickDone"); + RCLCPP_DEBUG(m_node->get_logger(), "PlaceObjectSkill num_retry: %d tick time: %ld", load_counter, duration_ms); + response->is_ok = true; +} + +void PlaceObjectSkill::halt( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) +{ + std::lock_guard lock(m_requestMutex); + RCLCPP_INFO(m_node->get_logger(), "PlaceObjectSkill::halt"); + m_haltResult.store(false); + m_stateMachine.submitEvent("CMD_HALT"); + while(!m_haltResult.load()) { + std::this_thread::sleep_for (std::chrono::milliseconds(100)); + } + RCLCPP_INFO(m_node->get_logger(), "PlaceObjectSkill::haltDone"); + response->is_ok = true; +} + + + + + + +void PlaceObjectSkill::send_goal(overarching_msgs::action::Place::Goal goal_msg) +{ + using namespace std::placeholders; + bool wait_succeded{true}; + int retries = 0; + + while (!m_actionClient->wait_for_action_server(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service 'Place'. Exiting."); + wait_succeded = false; + break; + } + retries++; + if(retries == SERVICE_TIMEOUT) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Timed out while waiting for the service 'Place'."); + wait_succeded = false; + QVariantMap data; + data.insert("call_succeeded", false); + m_stateMachine.submitEvent("PlaceComponet.Place.GoalResponse", data); + break; + } + } + if (wait_succeded) { + RCLCPP_INFO(m_node->get_logger(), "Sending goal"); + m_actionClient->async_send_goal(goal_msg, m_send_goal_options); + QVariantMap data; + data.insert("call_succeeded", true); + m_stateMachine.submitEvent("PlaceComponet.Place.GoalResponse", data); + } + } + +void PlaceObjectSkill::result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result) +{ + bool call_succeeded = false; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + call_succeeded = true; + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(m_node->get_logger(), "Goal was aborted"); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(m_node->get_logger(), "Goal was canceled"); + break; + default: + RCLCPP_ERROR(m_node->get_logger(), "Unknown result code"); + break; + } + //std::cout << "Result received: " << result.result->is_ok << std::endl; + // RCLCPP_INFO(m_node->get_logger(), "Result received: %d ", result.result->is_ok); + QVariantMap data; + data.insert("call_succeeded", call_succeeded); + m_stateMachine.submitEvent("PlaceComponet.Place.ResultResponse", data); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "PlaceComponet.Place.ResultResponse"); +} +void PlaceObjectSkill::goal_response_callback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) +{ + // std::cout << "Provaa" << std::endl; + QVariantMap data; + if (!goal_handle) { + data.insert("call_succeeded", false); + m_stateMachine.submitEvent("PlaceComponet.Place.GoalResponse", data); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "PlaceComponet.Place.GoalResponse Failure"); + RCLCPP_ERROR(m_node->get_logger(), "Goal was rejected by server"); + } else { + data.insert("call_succeeded", true); + m_stateMachine.submitEvent("PlaceComponet.Place.GoalResponse", data); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "PlaceComponet.Place.GoalResponse Success"); + RCLCPP_INFO(m_node->get_logger(), "Goal accepted by server, waiting for result"); + } +} + +void PlaceObjectSkill::feedback_callback( + rclcpp_action::ClientGoalHandle::SharedPtr, + [[maybe_unused]] const std::shared_ptr feedback) +{ + +} + + + diff --git a/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/PlaceObjectSkillSM.scxml b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/PlaceObjectSkillSM.scxml new file mode 100644 index 00000000..9d5be512 --- /dev/null +++ b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/PlaceObjectSkillSM.scxml @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/main.cpp b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/main.cpp new file mode 100644 index 00000000..c556c3fc --- /dev/null +++ b/examples/overarching_tutorial/simulation/generated_skill/place_object_skill/src/main.cpp @@ -0,0 +1,24 @@ +// Autogenerated by model2code (https://github.com/convince-project/model2code) +// File autogenerated at 2026-02-17 16:41:13 CET with M2C version 1.0.0 +// This is an automatically generated file. + +#include +#include +#include +#include +#include +#include +#include "PlaceObjectSkill.h" + +int main(int argc, char *argv[]) +{ + QCoreApplication app(argc, argv); + PlaceObjectSkill stateMachine("PlaceObject"); + stateMachine.start(argc, argv); + + int ret=app.exec(); + + std::cout << "PlaceObjectSkill successfully closed" << std::endl; + return ret; +} + diff --git a/examples/overarching_tutorial/simulation/tutorial_run/launch/full_simulation.launch.py b/examples/overarching_tutorial/simulation/tutorial_run/launch/full_simulation.launch.py new file mode 100644 index 00000000..5cde6fea --- /dev/null +++ b/examples/overarching_tutorial/simulation/tutorial_run/launch/full_simulation.launch.py @@ -0,0 +1,77 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + tree_arg = DeclareLaunchArgument( + "tree", + default_value="src/roaml/policy/bt_tree.xml", + description="Behavior tree file path passed to bt_executor", + ) + + default_object_id_arg = DeclareLaunchArgument( + "default_object_id", + default_value="butter0", + description="Fallback object id used by bt_executor when BT ports do not provide one", + ) + + simulator = Node( + package="tutorial_sim", + executable="run", + name="tutorial_sim", + output="screen", + emulate_tty=True, + ) + + translator = Node( + package="tutorial_sim", + executable="translate_component", + name="translate_component", + output="screen", + emulate_tty=True, + ) + + place_object_skill = Node( + package="place_object_skill", + executable="place_object_skill", + name="place_object_skill", + output="screen", + emulate_tty=True, + ) + + bt_executor = Node( + package="bt_executor", + executable="btcpp_executor", + name="btcpp_executor", + output="screen", + emulate_tty=True, + parameters=[ + { + "tree": LaunchConfiguration("tree"), + "default_object_id": LaunchConfiguration("default_object_id"), + } + ], + ) + + shutdown_on_bt_exit = RegisterEventHandler( + OnProcessExit( + target_action=bt_executor, + on_exit=[EmitEvent(event=Shutdown(reason="btcpp_executor finished"))], + ) + ) + + return LaunchDescription( + [ + tree_arg, + default_object_id_arg, + simulator, + translator, + place_object_skill, + bt_executor, + shutdown_on_bt_exit, + ] + ) \ No newline at end of file diff --git a/examples/overarching_tutorial/simulation/tutorial_run/package.xml b/examples/overarching_tutorial/simulation/tutorial_run/package.xml new file mode 100644 index 00000000..1b77ea08 --- /dev/null +++ b/examples/overarching_tutorial/simulation/tutorial_run/package.xml @@ -0,0 +1,24 @@ + + + + tutorial_run + 1.0.0 + Launch package to run the overarching tutorial simulation stack. + Christian Henkel + Apache-2.0 + + launch + launch_ros + tutorial_sim + bt_executor + place_object_skill + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/examples/overarching_tutorial/simulation/tutorial_run/resource/tutorial_run b/examples/overarching_tutorial/simulation/tutorial_run/resource/tutorial_run new file mode 100644 index 00000000..e69de29b diff --git a/examples/overarching_tutorial/simulation/tutorial_run/setup.cfg b/examples/overarching_tutorial/simulation/tutorial_run/setup.cfg new file mode 100644 index 00000000..e951952c --- /dev/null +++ b/examples/overarching_tutorial/simulation/tutorial_run/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/tutorial_run +[install] +install_scripts=$base/lib/tutorial_run \ No newline at end of file diff --git a/examples/overarching_tutorial/simulation/tutorial_run/setup.py b/examples/overarching_tutorial/simulation/tutorial_run/setup.py new file mode 100644 index 00000000..cb9da037 --- /dev/null +++ b/examples/overarching_tutorial/simulation/tutorial_run/setup.py @@ -0,0 +1,23 @@ +from glob import glob + +from setuptools import find_packages, setup + +package_name = "tutorial_run" + +setup( + name=package_name, + version="1.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.py")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Christian Henkel", + maintainer_email="christian.henkel2@de.bosch.com", + description="Launch package for the CONVINCE overarching tutorial simulation", + license="Apache-2.0", + tests_require=["pytest"], +) \ No newline at end of file diff --git a/examples/overarching_tutorial/simulation/tutorial_run/tutorial_run/__init__.py b/examples/overarching_tutorial/simulation/tutorial_run/tutorial_run/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/README.md b/examples/overarching_tutorial/simulation/tutorial_sim/README.md new file mode 100644 index 00000000..92698852 --- /dev/null +++ b/examples/overarching_tutorial/simulation/tutorial_sim/README.md @@ -0,0 +1,143 @@ +# tutorial_sim + +## Place Translator Component + +This package includes a ROS 2 bridge node that translates the tutorial `Place` action into the pyrobosim `ExecuteTaskAction`. + +The input interface definition used by this node is the `Place.action` file in: +- `examples/overarching_tutorial/ros_interfaces/src/overarching_msgs/action/Place.action` + +Implementation file: +- `tutorial_sim/translate_component.py` + +Compatibility wrapper: +- `../component/translate_component.py` + +### What it does + +The translator exposes an action server for: +- Input action: `/PlaceComponet/Place` +- Input type: `overarching_msgs/action/Place` + +When a goal is received, it forwards a new goal to pyrobosim: +- Output action: `/execute_action` +- Output type: `pyrobosim_msgs/action/ExecuteTaskAction` + +Forwarded fields are mapped as: +- `action.robot` <- `robot_id` parameter (default: `robot`) +- `action.type` <- `"place"` +- `action.object` <- resolved object to place + +Object resolution order: +1. `RobotState.manipulated_object` (when available) +2. `default_object` parameter +3. Last seen non-empty `RobotState.manipulated_object` +4. If still empty, the request is forwarded with an empty object and downstream behavior decides the outcome + +If no object is immediately available, the node waits briefly for `RobotState` updates +before falling back to empty object forwarding (configurable timeout below). + +### Behavior difference vs previous version + +- Forwarded `action.object` remains the resolved object name/instance. +- `run.py` now uses a higher `RobotState` publication rate to improve timing alignment with `Place` translation. +- Additional logs now show resolved object name, inferred type, and source (`robot_state`, `default_object`, or `last_seen_robot_state`). +- The translator waits up to `wait_for_object_timeout_sec` (default `0.5`) for dynamic updates before sending an empty object. + +### Why this change was needed + +During BT execution, the object to place is decided dynamically and may arrive slightly later on `/robot/robot_state`. +With low state publication frequency, the translator could miss that update and forward `object=''`. + +This package now addresses it by combining: + +- higher simulator state publication rate in `tutorial_sim run`, +- short wait window in the translator, +- fallback to last seen non-empty `manipulated_object`. + +### Result handling + +The bridge waits for pyrobosim action completion and maps the result back to the original `Place` goal: +- `ExecutionResult.SUCCESS` -> `Place` goal succeeds +- Any other status -> `Place` goal aborts + +The goal is aborted if: +- `/execute_action` is not available, +- pyrobosim rejects the forwarded goal, +- no result is returned. + +### ROS parameters + +- `input_action_name` (default: `/PlaceComponet/Place`) +- `output_action_name` (default: `/execute_action`) +- `robot_state_topic` (default: `/robot/robot_state`) +- `robot_id` (default: `robot`) +- `default_object` (default: empty string) +- `wait_for_object_timeout_sec` (default: `0.5`) +- `wait_server_timeout_sec` (default: `5.0`) + +### Run + +After building and sourcing your workspace: + +```bash +ros2 run tutorial_sim translate_component +``` + +For reliable dynamic object resolution in `translate_component`, run the simulator (`tutorial_sim run`) from this package version: it publishes `RobotState` at a higher rate (`state_pub_rate=5.0`), so `manipulated_object` updates are visible in time for `Place` translation. + +Example with custom parameters: + +```bash +ros2 run tutorial_sim translate_component --ros-args \ + -p input_action_name:=/PlaceComponet/Place \ + -p output_action_name:=/execute_action \ + -p robot_id:=robot +``` + +To see which object is being placed, run the translator and check its logs when a `Place` goal arrives: + +```bash +ros2 run tutorial_sim translate_component +``` + +You will see log lines like: +- `Resolved object 'butter0' (type='butter', source='robot_state').` +- `Forwarding Place -> ExecuteTaskAction(type='place', robot='robot', object='butter0')` + +If you want strict behavior (no wait), set: + +```bash +ros2 run tutorial_sim translate_component --ros-args \ + -p wait_for_object_timeout_sec:=0.0 +``` + +### Send commands after startup + +Once `translate_component` is running, send a `Place` request to the new action endpoint: + +```bash +ros2 action send_goal /PlaceComponet/Place overarching_msgs/action/Place "{}" +``` +otherwise bypassing the bridger/translator: + +```bash +ros2 action send_goal /execute_action pyrobosim_msgs/action/ExecuteTaskAction "{action: {robot: robot, type: place, object: butter}, realtime_factor: 1.0}" +``` + + +Useful checks: + +```bash +ros2 action list +ros2 action info /PlaceComponet/Place +ros2 action info /execute_action +``` + +If the robot is not currently holding an object (`/robot/robot_state`) and `default_object` is empty, +the translator still forwards the request with an empty object. If your downstream requires an explicit +object name, launch the translator with a fallback object: + +```bash +ros2 run tutorial_sim translate_component --ros-args -p default_object:=butter +``` diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/package.xml b/examples/overarching_tutorial/simulation/tutorial_sim/package.xml index 9bcba092..bb911858 100644 --- a/examples/overarching_tutorial/simulation/tutorial_sim/package.xml +++ b/examples/overarching_tutorial/simulation/tutorial_sim/package.xml @@ -8,6 +8,7 @@ Apache-2.0 pyrobosim_ros + overarching_msgs ament_copyright ament_flake8 diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/setup.py b/examples/overarching_tutorial/simulation/tutorial_sim/setup.py index 65209084..dbc9f504 100644 --- a/examples/overarching_tutorial/simulation/tutorial_sim/setup.py +++ b/examples/overarching_tutorial/simulation/tutorial_sim/setup.py @@ -23,6 +23,8 @@ entry_points={ "console_scripts": [ "run = tutorial_sim.run:main", + "translate_component = tutorial_sim.translate_component:main", ], }, ) + diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/__init__.cpython-312.pyc b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 00000000..6cb36f05 Binary files /dev/null and b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/__init__.cpython-312.pyc differ diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/run.cpython-312.pyc b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/run.cpython-312.pyc new file mode 100644 index 00000000..9926fd4e Binary files /dev/null and b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/run.cpython-312.pyc differ diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/translate_component.cpython-312.pyc b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/translate_component.cpython-312.pyc new file mode 100644 index 00000000..7a50385e Binary files /dev/null and b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/__pycache__/translate_component.cpython-312.pyc differ diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/run.py b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/run.py index 5c2ab9c0..c72361df 100644 --- a/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/run.py +++ b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/run.py @@ -17,7 +17,7 @@ def create_ros_node(): """Initializes ROS node""" rclpy.init() - node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) + node = WorldROSWrapper(state_pub_rate=5.0, dynamics_rate=0.01) world_file = os.path.join( get_package_share_directory("tutorial_sim"), @@ -32,6 +32,41 @@ def create_ros_node(): return node +def _reset_world_and_planners(node) -> None: + """Best-effort reset of world and planners before process exit.""" + world = getattr(node, "world", None) + if world is None: + return + + try: + world.reset() + except Exception as exc: + print(f"[tutorial_sim] world reset failed during shutdown: {exc}") + + robots = getattr(world, "robots", []) + if isinstance(robots, dict): + robots = robots.values() + + for robot in robots: + planner = getattr(robot, "path_planner", None) + if planner is not None and hasattr(planner, "reset"): + try: + planner.reset() + except Exception as exc: + print(f"[tutorial_sim] planner reset failed during shutdown: {exc}") + + +def _shutdown_ros_node(node) -> None: + """Stop ROS wrapper cleanly if the wrapper exposes stop/shutdown hooks.""" + for method_name in ("shutdown", "stop", "destroy_node"): + method = getattr(node, method_name, None) + if callable(method): + try: + method() + except Exception as exc: + print(f"[tutorial_sim] {method_name} failed during shutdown: {exc}") + + def main(): node = create_ros_node() @@ -39,8 +74,19 @@ def main(): ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True)) ros_thread.start() - # Start GUI in main thread - start_gui(node.world) + try: + # Start GUI in main thread + start_gui(node.world) + except KeyboardInterrupt: + pass + finally: + _reset_world_and_planners(node) + _shutdown_ros_node(node) + if rclpy.ok(): + rclpy.shutdown() + + # Avoid hanging on exit if ROS thread is still unwinding. + ros_thread.join(timeout=2.0) if __name__ == "__main__": diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/translate_component.py b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/translate_component.py new file mode 100644 index 00000000..4ff519d3 --- /dev/null +++ b/examples/overarching_tutorial/simulation/tutorial_sim/tutorial_sim/translate_component.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python3 + +""" +Place translator node. + +This node bridges: +- input action: overarching_msgs/Place (default topic: /PlaceComponet/Place) +- output action: pyrobosim_msgs/ExecuteTaskAction (default topic: /execute_action) + +It receives a Place goal, resolves which object should be placed, +forwards a "place" task to pyrobosim, and maps the result back to Place. +""" + +import rclpy +import re +import time +from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node + +from overarching_msgs.action import Place +from pyrobosim_msgs.action import ExecuteTaskAction +from pyrobosim_msgs.msg import ExecutionResult, RobotState + + +class PlaceTranslatorNode(Node): + """ROS 2 node that translates Place requests into pyrobosim task actions.""" + + def __init__(self) -> None: + super().__init__("place_translator") + + # Input/output endpoints and behavior configuration. + self.declare_parameter("input_action_name", "/PlaceComponet/Place") + self.declare_parameter("output_action_name", "/execute_action") + self.declare_parameter("robot_state_topic", "/robot/robot_state") + self.declare_parameter("robot_id", "robot") + self.declare_parameter("default_object", "") + self.declare_parameter("wait_for_object_timeout_sec", 0.5) + self.declare_parameter("wait_server_timeout_sec", 5.0) + + # Cache parameter values for faster usage in callbacks. + self._input_action_name = self.get_parameter("input_action_name").value + self._output_action_name = self.get_parameter("output_action_name").value + self._robot_state_topic = self.get_parameter("robot_state_topic").value + self._robot_id = self.get_parameter("robot_id").value + self._default_object = self.get_parameter("default_object").value + self._wait_for_object_timeout_sec = float( + self.get_parameter("wait_for_object_timeout_sec").value + ) + self._wait_server_timeout_sec = float(self.get_parameter("wait_server_timeout_sec").value) + + # Reentrant callback group allows overlapping callbacks if needed. + self._cb_group = ReentrantCallbackGroup() + + # Last robot state is used to infer which object is currently held. + self._latest_robot_state = None + self._last_seen_manipulated_object = "" + + # Subscribe to robot state updates from pyrobosim. + self._state_sub = self.create_subscription( + RobotState, + self._robot_state_topic, + self._on_robot_state, + 10, + ) + + # Client towards pyrobosim action server. + self._pyrobosim_client = ActionClient( + self, + ExecuteTaskAction, + self._output_action_name, + callback_group=self._cb_group, + ) + + # Server exposed to the tutorial/BT side. + self._place_server = ActionServer( + self, + Place, + self._input_action_name, + execute_callback=self._execute_place, + goal_callback=self._on_goal, + cancel_callback=self._on_cancel, + callback_group=self._cb_group, + ) + + self.get_logger().info( + f"Place translator ready: {self._input_action_name} -> {self._output_action_name}" + ) + + def _on_robot_state(self, msg: RobotState) -> None: + """Store latest robot state for object resolution.""" + self._latest_robot_state = msg + if msg.manipulated_object: + self._last_seen_manipulated_object = msg.manipulated_object + + def _on_goal(self, _goal_request: Place.Goal) -> GoalResponse: + """Accept all incoming Place goals.""" + return GoalResponse.ACCEPT + + def _on_cancel(self, _goal_handle) -> CancelResponse: + """Accept cancel requests from clients.""" + return CancelResponse.ACCEPT + + def _infer_object_type(self, object_name: str) -> str: + """Infer object type/category from object instance name (e.g. butter0 -> butter).""" + if not object_name: + return "" + + inferred_type = re.sub(r"\d+$", "", object_name) + inferred_type = re.sub(r"[_-]+$", "", inferred_type) + return inferred_type or object_name + + def _resolve_object_to_place(self) -> tuple[str, str]: + """ + Resolve object name to place. + + Priority: + 1) object currently manipulated by robot (from RobotState) + 2) configured default_object parameter + 3) last seen manipulated_object from RobotState + 4) empty string => let downstream decide (e.g., place currently held object) + + Returns: + tuple(object_name, source) + """ + if self._latest_robot_state is not None and self._latest_robot_state.manipulated_object: + object_name = self._latest_robot_state.manipulated_object + return object_name, "robot_state" + + if self._default_object: + object_name = self._default_object + return object_name, "default_object" + + if self._last_seen_manipulated_object: + object_name = self._last_seen_manipulated_object + return object_name, "last_seen_robot_state" + + return "", "none" + + def _resolve_object_to_place_with_wait(self) -> tuple[str, str]: + """ + Resolve object, optionally waiting a short time for RobotState updates. + + This helps when Place is requested right after Pick and RobotState is still propagating. + """ + object_name, object_source = self._resolve_object_to_place() + if object_name or self._wait_for_object_timeout_sec <= 0.0: + return object_name, object_source + + deadline = time.monotonic() + self._wait_for_object_timeout_sec + self.get_logger().info( + "No object currently resolved; waiting briefly for RobotState update " + f"(timeout={self._wait_for_object_timeout_sec:.2f}s)." + ) + + while time.monotonic() < deadline: + time.sleep(0.05) + object_name, object_source = self._resolve_object_to_place() + if object_name: + self.get_logger().info( + "Object resolved after wait: " f"'{object_name}' (source='{object_source}')." + ) + return object_name, object_source + + return "", "none" + + async def _execute_place(self, goal_handle) -> Place.Result: + """Main translation flow for a Place goal.""" + + # Determine which object should be placed. + object_name, object_source = self._resolve_object_to_place_with_wait() + object_type = self._infer_object_type(object_name) + + # Ensure downstream action server is reachable. + if not self._pyrobosim_client.wait_for_server(timeout_sec=self._wait_server_timeout_sec): + self.get_logger().error( + f"Output action server '{self._output_action_name}' not available" + ) + goal_handle.abort() + return Place.Result() + + # Build translated goal for pyrobosim. + forward_goal = ExecuteTaskAction.Goal() + forward_goal.action.robot = self._robot_id + forward_goal.action.type = "place" + forward_goal.action.object = object_name + + if not object_name: + self.get_logger().warn( + "Place resolve: no object found (robot_state/default/last_seen). " + "Forwarding with empty object." + ) + else: + self.get_logger().info( + f"Resolved object '{object_name}' (type='{object_type}', source='{object_source}')." + ) + + self.get_logger().info( + "Forwarding Place -> ExecuteTaskAction(" + f"type='place', robot='{self._robot_id}', object='{object_name}')" + ) + + # Send translated goal and wait for goal acceptance. + send_goal_future = self._pyrobosim_client.send_goal_async(forward_goal) + remote_goal_handle = await send_goal_future + + if remote_goal_handle is None or not remote_goal_handle.accepted: + self.get_logger().error("Output action goal rejected") + goal_handle.abort() + return Place.Result() + + # Wait for downstream completion. + result_future = remote_goal_handle.get_result_async() + wrapped_result = await result_future + + if wrapped_result is None: + self.get_logger().error("Output action returned no result") + goal_handle.abort() + return Place.Result() + + # Map downstream status to upstream Place result. + execution_result = wrapped_result.result.execution_result + if execution_result.status == ExecutionResult.SUCCESS: + goal_handle.succeed() + return Place.Result() + + self.get_logger().error( + "Place failed in pyrobosim: " + f"status={execution_result.status}, message='{execution_result.message}'" + ) + goal_handle.abort() + return Place.Result() + + +def main(args=None) -> None: + """Node entrypoint.""" + rclpy.init(args=args) + node = PlaceTranslatorNode() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/examples/overarching_tutorial/simulation/tutorial_sim/worlds/world.yaml b/examples/overarching_tutorial/simulation/tutorial_sim/worlds/world.yaml index 83ae43ac..66ef3278 100644 --- a/examples/overarching_tutorial/simulation/tutorial_sim/worlds/world.yaml +++ b/examples/overarching_tutorial/simulation/tutorial_sim/worlds/world.yaml @@ -29,10 +29,10 @@ robots: rng_seed: 42 # battery_usage: 0.5 # Per meter traveled pick: - success_probability: 0.7 + success_probability: 0.9 # battery_usage: 5 place: - success_probability: 0.7 + success_probability: 0.9 # battery_usage: 5 open: success_probability: 0.9 @@ -41,7 +41,7 @@ robots: success_probability: 0.6 # battery_usage: 8 detect: - success_probability: 0.8 + success_probability: 0.9 # battery_usage: 0 rooms: