+
+This repository contains a **copy** of the git repositories that were added as subtrees.
+File changes and commits are treated as if they happen only in this repository.
+If you update the contents of a subtree, you can merge the latest `main` branch of [lab_sim](https://github.com/PickNikRobotics/lab_sim) using the following command:
+```bash
+git subtree pull --prefix src/lab_sim https://github.com/PickNikRobotics/lab_sim main --squash
+```
+
+To pull the upstream changes to all subtrees and submodules, a convenience script is provided.
+From the top level, you can execute:
+```bash
+./sync_subtrees.sh
+```
diff --git a/colcon-defaults.yaml b/colcon-defaults.yaml
index 9aedbf87e..1129e69fa 100644
--- a/colcon-defaults.yaml
+++ b/colcon-defaults.yaml
@@ -2,6 +2,9 @@
build:
# Enable this for bidirectional syncing from the UI
symlink-install: true
+ allow-overriding:
+ - robotiq_description
+ - ur_description
mixin:
# Enable ccache support
- ccache
@@ -13,7 +16,33 @@ build:
- build-testing-on
- coverage-gcc
- coverage-pytest
+ packages-skip:
+ - libfranka
+ - franka_bringup
+ - franka_example_controllers
+ - franka_fr3_moveit_config
+ - franka_gazebo_bringup
+ - franka_ign_ros2_control
+ - franka_gripper
+ - franka_hardware
+ - franka_robot_state_broadcaster
+ - franka_ros2
+ - franka_semantic_components
+ - integration_launch_testing
test:
event-handlers:
- console_direct+
- desktop_notification+
+ packages-skip:
+ - libfranka
+ - franka_bringup
+ - franka_example_controllers
+ - franka_fr3_moveit_config
+ - franka_gazebo_bringup
+ - franka_ign_ros2_control
+ - franka_gripper
+ - franka_hardware
+ - franka_robot_state_broadcaster
+ - franka_ros2
+ - franka_semantic_components
+ - integration_launch_testing
diff --git a/docker-compose.yaml b/docker-compose.yaml
index 7fe7e4edf..ce6e08906 100644
--- a/docker-compose.yaml
+++ b/docker-compose.yaml
@@ -3,28 +3,26 @@
# Services that are listed under `/opt/moveit_pro/docker-compose.yaml` are mirrored here for merging.
# Feel free to remove services here that are unmodified.
+#
services:
+
# The base image that all MoveIt Pro services extend off of. Builds the user workspace.
- base:
- build:
- # List any arguments for building the user workspace here.
- args:
- # IMPORTANT: Optionally install Nvidia drivers for improved simulator performance with Nvidia GPUs.
- # To do this you must
- # 1. Uncomment the BASE and NVIDIA_DRIVER_PACKAGE build args below
- # 2. Replace the 'nvidia-driver-555' apt package with the Nvidia driver version on your host, e.g. nvidia-driver-535, nvidia-driver-555. Use nvidia-smi on your host to determine the driver version.
- # After rebuilding via `moveit_pro build` verify the drivers are active in your container by running `nvidia_smi` inside of `moveit_pro shell`.
- # - BASE=nvidia
- # - NVIDIA_DRIVER_PACKAGE=nvidia-driver-555
+ base: {}
# Starts the MoveIt Pro Agent and the Bridge between the Agent and the Web UI.
- agent_bridge:
+ agent_bridge: {}
# Starts the robot drivers.
drivers:
+ volumes:
+ # Allow access to host hardware e.g. RealSense cameras
+ - /dev:/dev
# Starts the web UI frontend.
- web_ui:
+ web_ui: {}
# Developer specific configuration when running `moveit_pro dev`.
dev:
+ volumes:
+ # Allow access to host hardware e.g. RealSense cameras
+ - /dev:/dev
diff --git a/src/april_tag_sim/CMakeLists.txt b/src/april_tag_sim/CMakeLists.txt
new file mode 100644
index 000000000..3715fb432
--- /dev/null
+++ b/src/april_tag_sim/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.22)
+project(april_tag_sim)
+
+find_package(ament_cmake REQUIRED)
+find_package(picknik_accessories REQUIRED)
+
+
+# Install all XML files in directory
+set(PICKNIK_ACCESSORIES_SHARE_DIR
+"${CMAKE_INSTALL_PREFIX}/../picknik_accessories/share/picknik_accessories/mujoco_assets/"
+)
+# Destination directory
+set(DEST_DIR "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/description/")
+
+install(DIRECTORY "${PICKNIK_ACCESSORIES_SHARE_DIR}"
+ DESTINATION "${DEST_DIR}"
+ FILES_MATCHING PATTERN "*")
+
+install(
+ DIRECTORY
+ config
+ description
+ launch
+ objectives
+ waypoints
+ DESTINATION
+ share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/april_tag_sim/CONTRIBUTING.md b/src/april_tag_sim/CONTRIBUTING.md
new file mode 100644
index 000000000..be449dc44
--- /dev/null
+++ b/src/april_tag_sim/CONTRIBUTING.md
@@ -0,0 +1,7 @@
+Any contribution that you make to this repository will
+be under the 3-Clause BSD License, as dictated by that
+[license](https://opensource.org/licenses/BSD-3-Clause).
+
+# Contributing to this Repository
+
+Thanks for getting involved! If you want to add to this repository, please reach out to support@picknik.ai.
diff --git a/src/april_tag_sim/LICENSE b/src/april_tag_sim/LICENSE
new file mode 100644
index 000000000..574ef0790
--- /dev/null
+++ b/src/april_tag_sim/LICENSE
@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
diff --git a/src/april_tag_sim/MOVEIT_PRO_IGNORE b/src/april_tag_sim/MOVEIT_PRO_IGNORE
new file mode 100644
index 000000000..e69de29bb
diff --git a/src/april_tag_sim/README.md b/src/april_tag_sim/README.md
new file mode 100644
index 000000000..12e4c39f0
--- /dev/null
+++ b/src/april_tag_sim/README.md
@@ -0,0 +1,5 @@
+# april_tag_sim
+
+A MoveIt Pro simulation for benchmarking AprilTag detection.
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)
diff --git a/src/april_tag_sim/config/config.yaml b/src/april_tag_sim/config/config.yaml
new file mode 100644
index 000000000..f495a9560
--- /dev/null
+++ b/src/april_tag_sim/config/config.yaml
@@ -0,0 +1,33 @@
+#
+# This contains information for a unique instance of a robot.
+#
+
+# Name of the package to specialize
+based_on_package: "lab_sim"
+hardware:
+ # Parameters used to configure the robot description through XACRO.
+ # A URDF and SRDF are both required.
+ # [Required]
+ robot_description:
+ urdf:
+ package: "lab_sim"
+ path: "description/picknik_ur.xacro"
+ srdf:
+ package: "lab_sim"
+ path: "config/moveit/picknik_ur.srdf"
+ urdf_params:
+ - mujoco_model_package: "april_tag_sim"
+# Configuration for loading behaviors and objectives.
+# [Required]
+objectives:
+ # Specify source folder for objectives
+ # [Required]
+ objective_library_paths:
+ sim_objectives:
+ package_name: "april_tag_sim"
+ relative_path: "objectives"
+ # Specify the location of the saved waypoints file.
+ # [Required]
+ waypoints_file:
+ package_name: "april_tag_sim"
+ relative_path: "waypoints/ur_waypoints.yaml"
diff --git a/src/april_tag_sim/description/LICENSE b/src/april_tag_sim/description/LICENSE
new file mode 100644
index 000000000..f24e07cb6
--- /dev/null
+++ b/src/april_tag_sim/description/LICENSE
@@ -0,0 +1,26 @@
+Copyright 2018 ROS Industrial Consortium
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors
+may be used to endorse or promote products derived from this software without
+specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/src/april_tag_sim/description/scene.xml b/src/april_tag_sim/description/scene.xml
new file mode 100644
index 000000000..c79867d24
--- /dev/null
+++ b/src/april_tag_sim/description/scene.xml
@@ -0,0 +1,151 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/launch/agent_bridge.launch.xml b/src/april_tag_sim/launch/agent_bridge.launch.xml
new file mode 100644
index 000000000..b2aa75434
--- /dev/null
+++ b/src/april_tag_sim/launch/agent_bridge.launch.xml
@@ -0,0 +1,6 @@
+
+
+
+
diff --git a/src/april_tag_sim/objectives/0degree_runs/.gitkeep b/src/april_tag_sim/objectives/0degree_runs/.gitkeep
new file mode 100644
index 000000000..e69de29bb
diff --git a/src/april_tag_sim/objectives/45degree_runs/.gitkeep b/src/april_tag_sim/objectives/45degree_runs/.gitkeep
new file mode 100644
index 000000000..e69de29bb
diff --git a/src/april_tag_sim/objectives/apriltag_pick_object.xml b/src/april_tag_sim/objectives/apriltag_pick_object.xml
new file mode 100644
index 000000000..453ec5f37
--- /dev/null
+++ b/src/april_tag_sim/objectives/apriltag_pick_object.xml
@@ -0,0 +1,89 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/close_gripper.xml b/src/april_tag_sim/objectives/close_gripper.xml
new file mode 100644
index 000000000..afb243195
--- /dev/null
+++ b/src/april_tag_sim/objectives/close_gripper.xml
@@ -0,0 +1,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/collect_angled_apriltag_detection_data.xml b/src/april_tag_sim/objectives/collect_angled_apriltag_detection_data.xml
new file mode 100644
index 000000000..f2f2cb05e
--- /dev/null
+++ b/src/april_tag_sim/objectives/collect_angled_apriltag_detection_data.xml
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/collect_apriltag_detection_data.xml b/src/april_tag_sim/objectives/collect_apriltag_detection_data.xml
new file mode 100644
index 000000000..c8d6b6d9f
--- /dev/null
+++ b/src/april_tag_sim/objectives/collect_apriltag_detection_data.xml
@@ -0,0 +1,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/collect_parallel_apriltag_detection_data.xml b/src/april_tag_sim/objectives/collect_parallel_apriltag_detection_data.xml
new file mode 100644
index 000000000..46f41dcef
--- /dev/null
+++ b/src/april_tag_sim/objectives/collect_parallel_apriltag_detection_data.xml
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/get_april_tag_pose_from_image.xml b/src/april_tag_sim/objectives/get_april_tag_pose_from_image.xml
new file mode 100644
index 000000000..9649449c3
--- /dev/null
+++ b/src/april_tag_sim/objectives/get_april_tag_pose_from_image.xml
@@ -0,0 +1,47 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/move_to_and_detect_tag_-45.xml b/src/april_tag_sim/objectives/move_to_and_detect_tag_-45.xml
new file mode 100644
index 000000000..3751951c8
--- /dev/null
+++ b/src/april_tag_sim/objectives/move_to_and_detect_tag_-45.xml
@@ -0,0 +1,128 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/move_to_and_detect_tag_0.xml b/src/april_tag_sim/objectives/move_to_and_detect_tag_0.xml
new file mode 100644
index 000000000..ab3ef649a
--- /dev/null
+++ b/src/april_tag_sim/objectives/move_to_and_detect_tag_0.xml
@@ -0,0 +1,128 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/move_to_and_detect_tag_45.xml b/src/april_tag_sim/objectives/move_to_and_detect_tag_45.xml
new file mode 100644
index 000000000..6c49fc181
--- /dev/null
+++ b/src/april_tag_sim/objectives/move_to_and_detect_tag_45.xml
@@ -0,0 +1,128 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/objectives/open_gripper.xml b/src/april_tag_sim/objectives/open_gripper.xml
new file mode 100644
index 000000000..14921bf6d
--- /dev/null
+++ b/src/april_tag_sim/objectives/open_gripper.xml
@@ -0,0 +1,24 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/april_tag_sim/package.xml b/src/april_tag_sim/package.xml
new file mode 100644
index 000000000..4a5b4c72c
--- /dev/null
+++ b/src/april_tag_sim/package.xml
@@ -0,0 +1,46 @@
+
+
+ april_tag_sim
+ 6.5.0
+
+
+ MuJoCo simulation configuration package for Picknik's UR robot on a linear
+ rail
+
+
+ MoveIt Pro Maintainer
+
+ BSD-3-Clause
+
+ ament_cmake
+
+ admittance_controller
+ moveit_planners_stomp
+ moveit_ros_perception
+ moveit_studio_agent
+ moveit_studio_behavior
+ moveit_studio_ur_pstop_manager
+ picknik_accessories
+ picknik_mujoco_ros
+ picknik_ur_base_config
+ realsense2_camera
+ realsense2_description
+ robotiq_controllers
+ robotiq_description
+ ur_description
+ ur_robot_driver
+ velocity_force_controller
+
+ ament_lint_auto
+
+ ament_clang_format
+ ament_clang_tidy
+ ament_cmake_copyright
+ ament_cmake_lint_cmake
+ picknik_ament_copyright
+ ament_flake8
+
+
+ ament_cmake
+
+
diff --git a/src/april_tag_sim/scripts/analyze_pose_detection.py b/src/april_tag_sim/scripts/analyze_pose_detection.py
new file mode 100644
index 000000000..ad167f068
--- /dev/null
+++ b/src/april_tag_sim/scripts/analyze_pose_detection.py
@@ -0,0 +1,197 @@
+#!/usr/bin/env python3
+
+# Copyright 2025 PickNik Inc.
+# All rights reserved.
+#
+# Unauthorized copying of this code base via any medium is strictly prohibited.
+# Proprietary and confidential.
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+from pathlib import Path
+from scipy.spatial.transform import Rotation as R
+import yaml
+
+
+DIR_0DEG = "../objectives/0degree_runs"
+DIR_45DEG = "../objectives/45degree_runs"
+NUM_TAGS = 6
+
+
+def load_yaml_file(file_path):
+ with open(file_path, "r") as f:
+ return yaml.safe_load(f)
+
+
+def save_yaml_file(data, file_path):
+ with open(file_path, "w") as f:
+ yaml.dump(data, f, default_flow_style=False)
+
+
+def extract_yaml_data(tag_dfs, base_path, num_runs):
+ base_path = Path(base_path)
+
+ for tag_num in range(1, NUM_TAGS + 1):
+ tag_file = base_path / f"tag{tag_num}.yaml"
+ runs_yaml = load_yaml_file(tag_file)
+ for run_num in runs_yaml.keys():
+ pose_yaml = runs_yaml[run_num]
+ position = pose_yaml["pose"]["position"]
+ orientation = pose_yaml["pose"]["orientation"]
+
+ # Extract position and orientation
+ tag_dfs[tag_num].loc[run_num] = {
+ "x": position["x"],
+ "y": position["y"],
+ "z": position["z"],
+ "qw": orientation["w"],
+ "qx": orientation["x"],
+ "qy": orientation["y"],
+ "qz": orientation["z"],
+ }
+
+
+def get_deviations(row):
+ pos_mag = np.linalg.norm([row["x"], row["y"], row["z"]])
+ r = R.from_quat([row["qx"], row["qy"], row["qz"], row["qw"]])
+ rot_mag = r.magnitude()
+ rot_mag_deg = np.degrees(rot_mag)
+ rot_vec = r.as_rotvec()
+ return {
+ "x": row["x"],
+ "y": row["y"],
+ "z": row["z"],
+ "pos_mag": pos_mag,
+ "rx": rot_vec[0],
+ "ry": rot_vec[1],
+ "rz": rot_vec[2],
+ "rot_mag": rot_mag_deg,
+ }
+
+
+def create_dfs(num_runs):
+ tag_dfs = {
+ tag_num: pd.DataFrame(
+ columns=["x", "y", "z", "qx", "qy", "qz", "qw"],
+ index=range(1, num_runs + 1),
+ )
+ for tag_num in range(1, NUM_TAGS + 1)
+ }
+ deviation_dfs = {
+ tag_num: pd.DataFrame(
+ columns=["x", "y", "z", "pos_mag", "rx", "ry", "rz", "rot_mag"],
+ index=range(1, num_runs + 1),
+ )
+ for tag_num in range(1, NUM_TAGS + 1)
+ }
+ mean_df = pd.DataFrame(
+ columns=["x", "y", "z", "pos_mag", "rx", "ry", "rz", "rot_mag"],
+ index=range(1, NUM_TAGS + 1),
+ )
+ stdev_df = pd.DataFrame(
+ columns=["x", "y", "z", "pos_mag", "rx", "ry", "rz", "rot_mag"],
+ index=range(1, NUM_TAGS + 1),
+ )
+ return tag_dfs, deviation_dfs, mean_df, stdev_df
+
+
+def fill_deviations_dfs(deviation_dfs, tag_dfs):
+ for tag_num in range(1, NUM_TAGS + 1):
+ results = tag_dfs[tag_num].apply(lambda row: get_deviations(row), axis=1)
+ for idx, res in results.items():
+ deviation_dfs[tag_num].loc[idx] = res
+
+
+def fill_summary_dfs(mean_df, stdev_df, deviation_dfs):
+ for tag_num in range(1, NUM_TAGS + 1):
+ mean_df.loc[tag_num] = deviation_dfs[tag_num].mean()
+ stdev_df.loc[tag_num] = deviation_dfs[tag_num].std()
+
+
+def create_plot(y1, err1, y2, err2, ylabel, title):
+ plt.figure(figsize=(10, 6))
+ segments = [0, 1, 2, 3, 4, 5]
+ plt.errorbar(
+ segments,
+ y1,
+ yerr=err1,
+ fmt="o",
+ capsize=5,
+ elinewidth=2,
+ markerfacecolor="black",
+ label="Parallel (0°) detections",
+ )
+ plt.errorbar(
+ segments,
+ y2,
+ yerr=err2,
+ fmt="o",
+ capsize=5,
+ elinewidth=2,
+ markerfacecolor="black",
+ label="Angled (45°) detections",
+ )
+
+ # X-axis
+ tag_sizes = [0.48, 0.32, 0.24, 0.16, 0.12, 0.08]
+ plt.xticks(segments, [f"Tag {i+1}, size: {tag_sizes[i]}" for i in segments])
+
+ plt.ylabel(ylabel)
+ plt.title(title)
+ plt.grid(True)
+ plt.legend()
+ plt.tight_layout()
+
+
+if __name__ == "__main__":
+ sample_yaml = load_yaml_file(DIR_0DEG + "/tag1.yaml")
+ num_runs = len(sample_yaml)
+
+ tag_dfs_0deg, deviation_dfs_0deg, mean_df_0deg, stdev_df_0deg = create_dfs(num_runs)
+ tag_dfs_45deg, deviation_dfs_45deg, mean_df_45deg, stdev_df_45deg = create_dfs(
+ num_runs
+ )
+
+ # Extract data from YAML files into the tag DataFrames.
+ extract_yaml_data(tag_dfs_0deg, DIR_0DEG, num_runs)
+ extract_yaml_data(tag_dfs_45deg, DIR_45DEG, num_runs)
+
+ # Fill deviations DataFrame.
+ fill_deviations_dfs(deviation_dfs_0deg, tag_dfs_0deg)
+ fill_deviations_dfs(deviation_dfs_45deg, tag_dfs_45deg)
+
+ # Fill summary (avg & stdev) DataFrames.
+ fill_summary_dfs(mean_df_0deg, stdev_df_0deg, deviation_dfs_0deg)
+ fill_summary_dfs(mean_df_45deg, stdev_df_45deg, deviation_dfs_45deg)
+
+ # Positional deviation plot
+ pos_mag_means_0deg = mean_df_0deg["pos_mag"].values
+ pos_mag_stdevs_0deg = stdev_df_0deg["pos_mag"].values
+ pos_mag_means_45deg = mean_df_45deg["pos_mag"].values
+ pos_mag_stdevs_45deg = stdev_df_45deg["pos_mag"].values
+ create_plot(
+ pos_mag_means_0deg,
+ pos_mag_stdevs_0deg,
+ pos_mag_means_45deg,
+ pos_mag_stdevs_45deg,
+ "AprilTag Position Deviation (m)",
+ f"AprilTag Position Deviation by Tag (n={num_runs})",
+ )
+
+ # Rotational deviation plot
+ # y-values: mean rot_mag for each segment
+ rot_mag_means_0deg = mean_df_0deg["rot_mag"].values
+ rot_mag_stdevs_0deg = stdev_df_0deg["rot_mag"].values
+ rot_mag_means_45deg = mean_df_45deg["rot_mag"].values
+ rot_mag_stdevs_45deg = stdev_df_45deg["rot_mag"].values
+ create_plot(
+ rot_mag_means_0deg,
+ rot_mag_stdevs_0deg,
+ rot_mag_means_45deg,
+ rot_mag_stdevs_45deg,
+ "AprilTag Orientation Deviation (°)",
+ f"AprilTag Orientation Deviation by Tag (n={num_runs})",
+ )
+
+ plt.show()
diff --git a/src/april_tag_sim/scripts/requirements.txt b/src/april_tag_sim/scripts/requirements.txt
new file mode 100644
index 000000000..aafa0cbb9
--- /dev/null
+++ b/src/april_tag_sim/scripts/requirements.txt
@@ -0,0 +1,4 @@
+scipy
+pandas
+matplotlib
+pyyaml
diff --git a/src/april_tag_sim/waypoints/ur_waypoints.yaml b/src/april_tag_sim/waypoints/ur_waypoints.yaml
new file mode 100644
index 000000000..3dd470918
--- /dev/null
+++ b/src/april_tag_sim/waypoints/ur_waypoints.yaml
@@ -0,0 +1,430 @@
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.01688327588273299
+ - -7.398158482916995e-11
+ - -3.8075799469380365e-09
+ - -1.8849146307875049
+ - 1.1008590123422952
+ - -1.256467912694488
+ - -1.5707500007156492
+ - 2.510085262493143e-12
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Home
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.003289905825896901
+ - -1.2085967565116629
+ - -0.38053130439833605
+ - 0.06646891177887178
+ - 0.6704411533015396
+ - -2.2598174786501377
+ - -1.5491163084575408
+ - -0.37975580107980283
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Look at Tag 1
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.003289895001232932
+ - -0.584237138085879
+ - -0.22445303770229816
+ - -0.03740938238661679
+ - 0.8442556071089173
+ - -2.327480342361729
+ - -1.5419380273363887
+ - -0.22379315558677204
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Look at Tag 2
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.003289878545032619
+ - -0.013331825276253031
+ - -0.007738854023507954
+ - -0.07155091132172103
+ - 0.8464016738230742
+ - -2.2935454146048495
+ - -1.5311630331449393
+ - -0.00721935810195775
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Look at Tag 3
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.003289868143547263
+ - 0.5983636189339089
+ - 0.13555159878309758
+ - -0.05766674362562976
+ - 0.7293397422861606
+ - -2.1900791668360156
+ - -1.5241317280474103
+ - 0.13603181574026543
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Look at Tag 4
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.0032898512121919484
+ - 1.3252080263883375
+ - 0.2232575515459395
+ - -0.03160676238665353
+ - 0.5925165639376702
+ - -2.0736004948166467
+ - -1.5197166259507557
+ - 0.22373111913195776
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Look at Tag 6
+- description: Move out of view of scene camera
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.00780498767879874
+ - -1.7207208458780023
+ - -2.9355287074907658e-05
+ - -1.8849275826089145
+ - 1.102019084998803
+ - -1.2563366868253756
+ - -1.5707679924921147
+ - 1.5931846964135707e-07
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Park Far Right
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.039808309949994805
+ - -0.5839327290315586
+ - -0.2687381616263423
+ - -1.4142103820838365
+ - 1.1521752505644343
+ - -1.3067562163163808
+ - -1.5708040696131618
+ - -0.2687661213277525
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Workspace Right
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.36093425387337075
+ - 0.3468469487674742
+ - -0.08122232656648681
+ - -1.1118986430431266
+ - 0.7735698103006698
+ - -1.2414135949062157
+ - 3.14
+ - -0.0803135811027271
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Wrist 2 Max
+- description: ''
+ favorite: false
+ joint_group_names:
+ - gripper
+ - linear_actuator
+ - manipulator
+ joint_state:
+ effort: []
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ name:
+ - robotiq_85_left_knuckle_joint
+ - linear_rail_joint
+ - shoulder_pan_joint
+ - shoulder_lift_joint
+ - elbow_joint
+ - wrist_1_joint
+ - wrist_2_joint
+ - wrist_3_joint
+ position:
+ - 0.36093425387337075
+ - 0.3468469487674742
+ - -0.08122232656648681
+ - -1.1118986430431266
+ - 0.7735698103006698
+ - -1.2414135949062157
+ - -3.14
+ - -0.0803135811027271
+ velocity: []
+ multi_dof_joint_state:
+ header:
+ frame_id: ''
+ stamp:
+ nanosec: 0
+ sec: 0
+ joint_names: []
+ transforms: []
+ twist: []
+ wrench: []
+ name: Wrist 2 Min
diff --git a/src/example_behaviors/CMakeLists.txt b/src/example_behaviors/CMakeLists.txt
new file mode 100644
index 000000000..3ecbd0c62
--- /dev/null
+++ b/src/example_behaviors/CMakeLists.txt
@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.22)
+project(example_behaviors CXX)
+
+find_package(moveit_studio_common REQUIRED)
+find_package(example_interfaces REQUIRED)
+moveit_studio_package()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS cartesian_planning moveit_studio_behavior
+ moveit_studio_behavior_interface pluginlib moveit_studio_vision
+ moveit_studio_vision_msgs PCL pcl_conversions pcl_ros example_interfaces)
+foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+ find_package(${package} REQUIRED)
+endforeach()
+
+add_library(
+ example_behaviors
+ SHARED
+ src/example_add_two_ints_service_client.cpp
+ src/example_convert_mtc_solution_to_joint_trajectory.cpp
+ src/example_delayed_message.cpp
+ src/example_get_string_from_topic.cpp
+ src/example_fibonacci_action_client.cpp
+ src/example_hello_world.cpp
+ src/example_publish_color_rgba.cpp
+ src/example_setup_mtc_wave_hand.cpp
+ src/example_ndt_registration.cpp
+ src/example_ransac_registration.cpp
+ src/example_sam2_segmentation.cpp
+ src/example_create_string_msg.cpp
+ src/register_behaviors.cpp)
+target_include_directories(
+ example_behaviors
+ PUBLIC $
+ $
+ PRIVATE ${PCL_INCLUDE_DIRS})
+ament_target_dependencies(example_behaviors
+ ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Install Libraries
+install(
+ TARGETS example_behaviors
+ EXPORT example_behaviorsTargets
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION bin
+ INCLUDES
+ DESTINATION include)
+
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+ moveit_pro_behavior_test(example_behaviors)
+endif()
+
+# Export the behavior plugins defined in this package so they are available to
+# plugin loaders that load the behavior base class library from the
+# moveit_studio_behavior package.
+pluginlib_export_plugin_description_file(
+ moveit_studio_behavior_interface example_behaviors_plugin_description.xml)
+
+ament_export_targets(example_behaviorsTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()
diff --git a/src/example_behaviors/COLCON_IGNORE b/src/example_behaviors/COLCON_IGNORE
new file mode 100644
index 000000000..e69de29bb
diff --git a/src/example_behaviors/README.md b/src/example_behaviors/README.md
new file mode 100644
index 000000000..c240711fe
--- /dev/null
+++ b/src/example_behaviors/README.md
@@ -0,0 +1 @@
+# example_behaviors
diff --git a/src/example_behaviors/behavior_plugin.yaml b/src/example_behaviors/behavior_plugin.yaml
new file mode 100644
index 000000000..a5f10861b
--- /dev/null
+++ b/src/example_behaviors/behavior_plugin.yaml
@@ -0,0 +1,4 @@
+objectives:
+ behavior_loader_plugins:
+ example_behaviors:
+ - "example_behaviors::ExampleBehaviorsLoader"
diff --git a/src/example_behaviors/config/tree_nodes_model.xml b/src/example_behaviors/config/tree_nodes_model.xml
new file mode 100644
index 000000000..f3844c223
--- /dev/null
+++ b/src/example_behaviors/config/tree_nodes_model.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
diff --git a/src/example_behaviors/example_behaviors_plugin_description.xml b/src/example_behaviors/example_behaviors_plugin_description.xml
new file mode 100644
index 000000000..347594f9b
--- /dev/null
+++ b/src/example_behaviors/example_behaviors_plugin_description.xml
@@ -0,0 +1,7 @@
+
+
+
+
diff --git a/src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp b/src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp
new file mode 100644
index 000000000..22b9f21d4
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp
@@ -0,0 +1,51 @@
+#pragma once
+
+#include
+#include
+
+using moveit_studio::behaviors::BehaviorContext;
+using moveit_studio::behaviors::ServiceClientBehaviorBase;
+using AddTwoInts = example_interfaces::srv::AddTwoInts;
+
+namespace example_behaviors
+{
+class ExampleAddTwoIntsServiceClient final : public ServiceClientBehaviorBase
+{
+public:
+ ExampleAddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+private:
+ /** @brief User-provided function to get the name of the service when initializing the service client. */
+ tl::expected getServiceName() override;
+
+ /**
+ * @brief User-provided function to create the service request.
+ * @return Returns a service request message. If not successful, returns an error message. Note that the criteria for
+ * success or failure is defined by the user's implementation of this function.
+ */
+ tl::expected createRequest() override;
+
+ /** @brief Optional user-provided function to process the service response after the service has finished. */
+ tl::expected processResponse(const AddTwoInts::Response& response) override;
+
+ /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+ std::shared_future>& getFuture() override
+ {
+ return future_;
+ }
+
+ /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+ std::shared_future> future_;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp b/src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp
new file mode 100644
index 000000000..f12ee159b
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp
@@ -0,0 +1,51 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace example_behaviors
+{
+/**
+ * @brief Converts a MoveIt Task Constructor Solution into a JointTrajectory.
+ *
+ * @details
+ * | Data Port Name | Port Type | Object Type |
+ * | ----------------- |---------------|---------------------------------------------------------|
+ * | solution | Input | moveit_task_constructor_msgs::msg::Solution |
+ * | joint_group | Input | std::string |
+ * | velocity_scaling_factor | Input | double |
+ * | acceleration_scaling_factor | Input | double |
+ * | sampling_rate | Input | int |
+ * | joint_trajectory | Output | trajectory_msgs::msg::JointTrajectory |
+ */
+class ExampleConvertMtcSolutionToJointTrajectory final
+ : public moveit_studio::behaviors::SharedResourcesNode
+{
+public:
+ ExampleConvertMtcSolutionToJointTrajectory(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ static BT::PortsList providedPorts();
+
+ static BT::KeyValueVector metadata();
+
+ BT::NodeStatus tick() override;
+
+private:
+ std::unique_ptr robot_model_loader_;
+
+ const std::vector& extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution);
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_create_string_msg.hpp b/src/example_behaviors/include/example_behaviors/example_create_string_msg.hpp
new file mode 100644
index 000000000..0dee05b40
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_create_string_msg.hpp
@@ -0,0 +1,59 @@
+#pragma once
+
+#include
+
+// This header includes the SharedResourcesNode type
+#include
+
+namespace example_behaviors
+{
+/**
+ * @brief The ExampleCreateStringMsg Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS
+ */
+class ExampleCreateStringMsg : public moveit_studio::behaviors::SharedResourcesNode
+{
+public:
+ /**
+ * @brief Constructor for the hello_world behavior.
+ * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when
+ * this Behavior is created within a new behavior tree.
+ * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in
+ * the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
+ * @param config This contains runtime configuration info for this Behavior, such as the mapping between the
+ * Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this
+ * Behavior is created within a new behavior tree.
+ * @details An important limitation is that the members of the base Behavior class are not instantiated until after
+ * the initialize() function is called, so these classes should not be used within the constructor.
+ */
+ ExampleCreateStringMsg(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /**
+ * @brief Implementation of the required providedPorts() function for the hello_world Behavior.
+ * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named
+ * providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function
+ * must return an empty BT::PortsList. This function returns a list of ports with their names and port info, which is
+ * used internally by the behavior tree.
+ * @return hello_world does not expose any ports, so this function returns an empty list.
+ */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+ /**
+ * @brief Implementation of BT::SyncActionNode::tick() for ExampleCreateStringMsg.
+ * @details This function is where the Behavior performs its work when the behavior tree is being run.
+ * Since ExampleCreateStringMsg is derived from BT::SyncActionNode, it is very important that its tick() function always
+ * finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree,
+ * which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly.
+ * @return BT::NodeStatus::RUNNING, BT::NodeStatus::SUCCESS, or BT::NodeStatus::FAILURE depending on the result of the
+ * work done in this function.
+ */
+ BT::NodeStatus tick() override;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_delayed_message.hpp b/src/example_behaviors/include/example_behaviors/example_delayed_message.hpp
new file mode 100644
index 000000000..26b721d28
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_delayed_message.hpp
@@ -0,0 +1,72 @@
+#pragma once
+
+#include
+
+// This header includes the SharedResourcesNode type
+#include
+
+#include
+
+#include
+
+namespace example_behaviors
+{
+/**
+ * @brief ExampleDelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on
+ * the input port
+ */
+class ExampleDelayedMessage : public moveit_studio::behaviors::SharedResourcesNode
+{
+private:
+ std::chrono::time_point start_time_;
+ double delay_duration_;
+
+public:
+ /**
+ * @brief Constructor for the delayed_message behavior.
+ * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when
+ * this Behavior is created within a new behavior tree.
+ * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in
+ * the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
+ * @param config This contains runtime configuration info for this Behavior, such as the mapping between the
+ * Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this
+ * Behavior is created within a new behavior tree.
+ * @details An important limitation is that the members of the base Behavior class are not instantiated until after
+ * the initialize() function is called, so these classes should not be used within the constructor.
+ */
+ ExampleDelayedMessage(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /**
+ * @brief Implementation of the required providedPorts() function for the delayed_message Behavior.
+ * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named
+ * providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function
+ * must return an empty BT::PortsList. This function returns a list of ports with their names and port info, which is
+ * used internally by the behavior tree.
+ * @return If delayed_message does not expose any ports, this function returns an empty list.
+ */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+ /**
+ * @brief Implementation of onStart(). Runs when the Behavior is ticked for the first time.
+ * @return Always returns BT::NodeStatus::RUNNING, since the success of Behavior's initialization is checked in @ref
+ * onRunning().
+ */
+ BT::NodeStatus onStart() override;
+
+ /**
+ * @brief Implementation of onRunning(). Checks the status of the Behavior when it is ticked after it starts running.
+ * @return BT::NodeStatus::RUNNING, BT::NodeStatus::SUCCESS, or BT::NodeStatus::FAILURE depending on the Behavior status.
+ */
+ BT::NodeStatus onRunning() override;
+
+ void onHalted() override;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_fibonacci_action_client.hpp b/src/example_behaviors/include/example_behaviors/example_fibonacci_action_client.hpp
new file mode 100644
index 000000000..f13090fd3
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_fibonacci_action_client.hpp
@@ -0,0 +1,50 @@
+#pragma once
+
+#include
+#include
+
+using moveit_studio::behaviors::ActionClientBehaviorBase;
+using moveit_studio::behaviors::BehaviorContext;
+using Fibonacci = example_interfaces::action::Fibonacci;
+
+namespace example_behaviors
+{
+class ExampleFibonacciActionClient final : public ActionClientBehaviorBase
+{
+public:
+ ExampleFibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+private:
+ /** @brief User-provided function to get the name of the action when initializing the action client. */
+ tl::expected getActionName() override;
+
+ /** @brief User-provided function to create the action goal before sending the action goal request. */
+ tl::expected createGoal() override;
+
+ /** @brief Optional user-provided function to process the action result after the action has finished. */
+ tl::expected processResult(const std::shared_ptr result) override;
+
+ /** @brief Optional user-provided function to process feedback sent by the action server. */
+ void processFeedback(const std::shared_ptr feedback) override;
+
+ /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+ std::shared_future>& getFuture() override
+ {
+ return future_;
+ }
+
+ /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+ std::shared_future> future_;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_get_string_from_topic.hpp b/src/example_behaviors/include/example_behaviors/example_get_string_from_topic.hpp
new file mode 100644
index 000000000..1b4e05fb9
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_get_string_from_topic.hpp
@@ -0,0 +1,37 @@
+#pragma once
+
+#include
+#include
+
+using moveit_studio::behaviors::BehaviorContext;
+using moveit_studio::behaviors::GetMessageFromTopicBehaviorBase;
+
+namespace example_behaviors
+{
+class ExampleGetStringFromTopic final : public GetMessageFromTopicBehaviorBase
+{
+public:
+ ExampleGetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+private:
+ /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+ std::shared_future>& getFuture() override
+ {
+ return future_;
+ }
+
+ /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+ std::shared_future> future_;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_hello_world.hpp b/src/example_behaviors/include/example_behaviors/example_hello_world.hpp
new file mode 100644
index 000000000..71232d6b5
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_hello_world.hpp
@@ -0,0 +1,59 @@
+#pragma once
+
+#include
+
+// This header includes the SharedResourcesNode type
+#include
+
+namespace example_behaviors
+{
+/**
+ * @brief The ExampleHelloWorld Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS
+ */
+class ExampleHelloWorld : public moveit_studio::behaviors::SharedResourcesNode
+{
+public:
+ /**
+ * @brief Constructor for the hello_world behavior.
+ * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when
+ * this Behavior is created within a new behavior tree.
+ * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in
+ * the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
+ * @param config This contains runtime configuration info for this Behavior, such as the mapping between the
+ * Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this
+ * Behavior is created within a new behavior tree.
+ * @details An important limitation is that the members of the base Behavior class are not instantiated until after
+ * the initialize() function is called, so these classes should not be used within the constructor.
+ */
+ ExampleHelloWorld(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /**
+ * @brief Implementation of the required providedPorts() function for the hello_world Behavior.
+ * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named
+ * providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function
+ * must return an empty BT::PortsList. This function returns a list of ports with their names and port info, which is
+ * used internally by the behavior tree.
+ * @return hello_world does not expose any ports, so this function returns an empty list.
+ */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+ /**
+ * @brief Implementation of BT::SyncActionNode::tick() for ExampleHelloWorld.
+ * @details This function is where the Behavior performs its work when the behavior tree is being run.
+ * Since ExampleHelloWorld is derived from BT::SyncActionNode, it is very important that its tick() function always
+ * finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree,
+ * which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly.
+ * @return BT::NodeStatus::RUNNING, BT::NodeStatus::SUCCESS, or BT::NodeStatus::FAILURE depending on the result of the
+ * work done in this function.
+ */
+ BT::NodeStatus tick() override;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_ndt_registration.hpp b/src/example_behaviors/include/example_behaviors/example_ndt_registration.hpp
new file mode 100644
index 000000000..ed574c928
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_ndt_registration.hpp
@@ -0,0 +1,62 @@
+#pragma once
+
+#include
+
+// This header includes the SharedResourcesNode type
+#include
+#include
+
+namespace example_behaviors
+{
+/**
+ * @brief TODO(...)
+ */
+class ExampleNDTRegistration : public moveit_studio::behaviors::AsyncBehaviorBase
+{
+public:
+ /**
+ * @brief Constructor for the behavior.
+ * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when
+ * this Behavior is created within a new behavior tree.
+ * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in
+ * the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
+ * @param config This contains runtime configuration info for this Behavior, such as the mapping between the
+ * Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this
+ * Behavior is created within a new behavior tree.
+ * @details An important limitation is that the members of the base Behavior class are not instantiated until after
+ * the initialize() function is called, so these classes should not be used within the constructor.
+ */
+ ExampleNDTRegistration(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /**
+ * @brief Implementation of the required providedPorts() function for the Behavior.
+ * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named
+ * providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function
+ * must return an empty BT::PortsList. This function returns a list of ports with their names and port info, which is
+ * used internally by the behavior tree.
+ * @return See ndt_registration.cpp for port list and description.
+ */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+protected:
+ tl::expected doWork() override;
+
+private:
+ /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+ std::shared_future>& getFuture() override
+ {
+ return future_;
+ }
+
+ /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+ std::shared_future> future_;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_publish_color_rgba.hpp b/src/example_behaviors/include/example_behaviors/example_publish_color_rgba.hpp
new file mode 100644
index 000000000..74ad74045
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_publish_color_rgba.hpp
@@ -0,0 +1,30 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+namespace example_behaviors
+{
+class ExamplePublishColorRGBA final : public moveit_studio::behaviors::SharedResourcesNode
+{
+public:
+ ExamplePublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ static BT::PortsList providedPorts();
+
+ static BT::KeyValueVector metadata();
+
+ BT::NodeStatus tick() override;
+
+private:
+ std::shared_ptr> publisher_;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp b/src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp
new file mode 100644
index 000000000..7534266e6
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_ransac_registration.hpp
@@ -0,0 +1,62 @@
+#pragma once
+
+#include
+
+// This header includes the SharedResourcesNode type
+#include
+#include
+
+namespace example_behaviors
+{
+/**
+ * @brief TODO(...)
+ */
+class ExampleRANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase
+{
+public:
+ /**
+ * @brief Constructor for the behavior.
+ * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when
+ * this Behavior is created within a new behavior tree.
+ * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in
+ * the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
+ * @param config This contains runtime configuration info for this Behavior, such as the mapping between the
+ * Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this
+ * Behavior is created within a new behavior tree.
+ * @details An important limitation is that the members of the base Behavior class are not instantiated until after
+ * the initialize() function is called, so these classes should not be used within the constructor.
+ */
+ ExampleRANSACRegistration(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /**
+ * @brief Implementation of the required providedPorts() function for the Behavior.
+ * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named
+ * providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function
+ * must return an empty BT::PortsList. This function returns a list of ports with their names and port info, which is
+ * used internally by the behavior tree.
+ * @return See ransac_registration.cpp for port list and description.
+ */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+protected:
+ tl::expected doWork() override;
+
+private:
+ /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+ std::shared_future>& getFuture() override
+ {
+ return future_;
+ }
+
+ /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+ std::shared_future> future_;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_sam2_segmentation.hpp b/src/example_behaviors/include/example_behaviors/example_sam2_segmentation.hpp
new file mode 100644
index 000000000..988d9702b
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_sam2_segmentation.hpp
@@ -0,0 +1,70 @@
+#pragma once
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace example_behaviors
+{
+/**
+ * @brief Segment an image using the SAM 2 model
+ */
+class ExampleSAM2Segmentation : public moveit_studio::behaviors::AsyncBehaviorBase
+{
+public:
+ /**
+ * @brief Constructor for the ExampleSAM2Segmentation behavior.
+ * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when
+ * this Behavior is created within a new behavior tree.
+ * @param config This contains runtime configuration info for this Behavior, such as the mapping between the
+ * Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this
+ * Behavior is created within a new behavior tree.
+ * @details An important limitation is that the members of the base Behavior class are not instantiated until after
+ * the initialize() function is called, so these classes should not be used within the constructor.
+ */
+ ExampleSAM2Segmentation(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /**
+ * @brief Implementation of the required providedPorts() function for the Behavior.
+ * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named
+ * providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function
+ * must return an empty BT::PortsList. This function returns a list of ports with their names and port info, which is
+ * used internally by the behavior tree.
+ * @return List of ports for the behavior.
+ */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+protected:
+ tl::expected doWork() override;
+
+private:
+ std::unique_ptr sam2_;
+ moveit_pro_ml::ONNXImage onnx_image_;
+ sensor_msgs::msg::Image mask_image_msg_;
+ moveit_studio_vision_msgs::msg::Mask2D mask_msg_;
+
+ /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+ std::shared_future>& getFuture() override
+ {
+ return future_;
+ }
+
+ /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+ std::shared_future> future_;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/include/example_behaviors/example_setup_mtc_wave_hand.hpp b/src/example_behaviors/include/example_behaviors/example_setup_mtc_wave_hand.hpp
new file mode 100644
index 000000000..3bfbb890b
--- /dev/null
+++ b/src/example_behaviors/include/example_behaviors/example_setup_mtc_wave_hand.hpp
@@ -0,0 +1,84 @@
+
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+namespace example_behaviors
+{
+/**
+ * @brief The ExampleSetupMTCWaveHand behavior makes any robot move the end of its arm back and forth.
+ * @details This is an example of a behavior that uses MoveIt Task Constructor to configure an MTC task,
+ * which can be planned and executed by MoveIt Pro's core PlanMTCTask and ExecuteMTCTask Behaviors.
+ * It is derived from AsyncBehaviorBase, an extension of the templated SharedResourcesNode type,
+ * which augments the core BehaviorTree.Cpp types with an additional constructor parameter
+ * to allow the Behavior to access a rclcpp Node owned by the Agent's ObjectiveServerNode.
+ * The AsyncBehaviorBase requires a user-implemented function doWork()
+ * which is called within an async process in a separate thread.
+ * It returns an tl::expected which contains a bool indicating task success
+ * If the task fails, doWork() returns a tl::unexpected which contains a string indicating the error
+ * if the process completed successfully or was canceled,
+ * or an error message if the process failed unexpectedly.
+ */
+class ExampleSetupMTCWaveHand final : public moveit_studio::behaviors::AsyncBehaviorBase
+{
+public:
+ /**
+ * @brief Constructor for the ExampleSetupMTCWaveHand behavior.
+ * @param name The name of a particular instance of this Behavior. This will be set by the behavior
+ * tree factory when this Behavior is created within a new behavior tree.
+ * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all
+ * SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the MoveIt Pro
+ * Agent's ObjectiveServerNode.
+ * @param config This contains runtime configuration info for this Behavior, such as the mapping
+ * between the Behavior's data ports on the behavior tree's blackboard. This will be set by the
+ * behavior tree factory when this Behavior is created within a new behavior tree.
+ */
+ ExampleSetupMTCWaveHand(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources);
+
+ /**
+ * @brief Implementation of the required providedPorts() function for the ExampleSetupMTCWaveHand Behavior.
+ * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function
+ * named providedPorts() which defines their input and output ports. If the Behavior does not use
+ * any ports, this function must return an empty BT::PortsList.
+ * This function returns a list of ports with their names and port info, which is used internally
+ * by the behavior tree.
+ * @return ExampleSetupMTCWaveHand has one data port: a bidirectional port named "task", which is a shared_ptr
+ * to an MTC task object. This function returns a BT::PortsList that declares this single port.
+ */
+ static BT::PortsList providedPorts();
+
+ /**
+ * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+ * subcategory, in the MoveIt Studio Developer Tool.
+ * @return A BT::KeyValueVector containing the Behavior metadata.
+ */
+ static BT::KeyValueVector metadata();
+
+private:
+ /**
+ * @brief Async thread for ExampleSetupMTCWaveHand. Adds MTC stages to an MTC task provided on a data port.
+ * @details This function is where the Behavior performs its work asynchronously while the behavior tree ticks.
+ * It is very important that behaviors return from being ticked very quickly because if it blocks before returning
+ * it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors
+ * that require a fast update rate to work correctly.
+ * @return An tl::expected which contains a true if the MTC stages were configured and added to the MTC task,
+ * or a string if it failed to retrieve the MTC task from the "task" data port.
+ */
+ tl::expected doWork() override;
+
+ /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+ std::shared_future>& getFuture() override
+ {
+ return future_;
+ }
+
+ /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+ std::shared_future> future_;
+};
+} // namespace example_behaviors
diff --git a/src/example_behaviors/package.xml b/src/example_behaviors/package.xml
new file mode 100644
index 000000000..0ac3ce5d5
--- /dev/null
+++ b/src/example_behaviors/package.xml
@@ -0,0 +1,32 @@
+
+
+ example_behaviors
+ 6.5.0
+ Example behaviors for MoveIt Pro
+
+ MoveIt Pro Maintainer
+ MoveIt Pro Maintainer
+
+ BSD-3-Clause
+
+ ament_cmake
+
+ moveit_studio_common
+
+ moveit_studio_behavior_interface
+ example_interfaces
+ perception_pcl
+ moveit_ros_planning_interface
+ moveit_studio_vision_msgs
+ moveit_studio_vision
+ moveit_studio_behavior
+
+ ament_lint_auto
+ ament_cmake_gtest
+ ament_clang_format
+ ament_clang_tidy
+
+
+ ament_cmake
+
+
diff --git a/src/example_behaviors/src/example_add_two_ints_service_client.cpp b/src/example_behaviors/src/example_add_two_ints_service_client.cpp
new file mode 100644
index 000000000..5e90fa054
--- /dev/null
+++ b/src/example_behaviors/src/example_add_two_ints_service_client.cpp
@@ -0,0 +1,57 @@
+#include
+
+// Include the template implementation for GetMessageFromTopicBehaviorBase.
+#include
+namespace example_behaviors
+{
+ExampleAddTwoIntsServiceClient::ExampleAddTwoIntsServiceClient(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : ServiceClientBehaviorBase(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleAddTwoIntsServiceClient::providedPorts()
+{
+ // This node has three input ports and one output port
+ return BT::PortsList({
+ BT::InputPort("service_name", "/add_two_ints", "The name of the service to call."),
+ BT::InputPort("addend1", "The first int to add to the other."),
+ BT::InputPort("addend2", "The second int to add to the other."),
+ BT::OutputPort("result", "{result}", "Result of the AddTwoInts service."),
+ });
+}
+
+BT::KeyValueVector ExampleAddTwoIntsServiceClient::metadata()
+{
+ return { { "subcategory", "Example Behaviors" },
+ { "description", "Calls a service to add two integers and makes the result available on an output port." } };
+}
+
+tl::expected ExampleAddTwoIntsServiceClient::getServiceName()
+{
+ const auto service_name = getInput("service_name");
+ if (const auto error = moveit_studio::behaviors::maybe_error(service_name))
+ {
+ return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
+ }
+ return service_name.value();
+}
+
+tl::expected ExampleAddTwoIntsServiceClient::createRequest()
+{
+ const auto a = getInput("addend1");
+ const auto b = getInput("addend2");
+ if (const auto error = moveit_studio::behaviors::maybe_error(a, b))
+ {
+ return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
+ }
+ return example_interfaces::build().a(a.value()).b(b.value());
+}
+
+tl::expected ExampleAddTwoIntsServiceClient::processResponse(const AddTwoInts::Response& response)
+{
+ setOutput("result", response.sum);
+ return { true };
+}
+} // namespace example_behaviors
diff --git a/src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp b/src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp
new file mode 100644
index 000000000..f8f28e13f
--- /dev/null
+++ b/src/example_behaviors/src/example_convert_mtc_solution_to_joint_trajectory.cpp
@@ -0,0 +1,143 @@
+#include
+
+namespace
+{
+const auto kLogger = rclcpp::get_logger("ExampleConvertMtcSolutionToJointTrajectory");
+
+// Port names for input and output ports.
+constexpr auto kPortIDSolution = "solution";
+constexpr auto kPortIDJointGroup = "joint_group";
+constexpr auto kPortIDJointTrajectory = "joint_trajectory";
+constexpr auto kPortIDVelocityScalingFactor = "velocity_scaling_factor";
+constexpr auto kPortIDAccelerationScalingFactor = "acceleration_scaling_factor";
+constexpr auto kPortIDSamplingRate = "sampling_rate";
+} // namespace
+
+namespace example_behaviors
+{
+ExampleConvertMtcSolutionToJointTrajectory::ExampleConvertMtcSolutionToJointTrajectory(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleConvertMtcSolutionToJointTrajectory::providedPorts()
+{
+ return {
+ BT::InputPort(kPortIDSolution, "{mtc_solution}",
+ "MoveIt Task Constructor solution."),
+ BT::InputPort(kPortIDJointGroup, "manipulator", "Joint group name used in the MTC solution."),
+ BT::InputPort(kPortIDVelocityScalingFactor, 1.0, "Velocity scaling factor for trajectory generation."),
+ BT::InputPort(kPortIDAccelerationScalingFactor, 1.0,
+ "Acceleration scaling factor for trajectory generation."),
+ BT::InputPort(kPortIDSamplingRate, 100, "Sampling rate for trajectory generation."),
+ BT::OutputPort(kPortIDJointTrajectory, "{joint_trajectory_msg}",
+ "Resulting joint trajectory."),
+ };
+}
+
+BT::KeyValueVector ExampleConvertMtcSolutionToJointTrajectory::metadata()
+{
+ return { { "subcategory", "Motion Planning" },
+ { "description",
+ "Extracts joint space trajectories from an MTC solution and adds time parameterization using TOTG." } };
+}
+
+BT::NodeStatus ExampleConvertMtcSolutionToJointTrajectory::tick()
+{
+ using namespace moveit_studio::behaviors;
+
+ // Load data from the behavior input ports.
+ const auto solution = getInput(kPortIDSolution);
+ const auto joint_group_name = getInput(kPortIDJointGroup);
+ const auto velocity_scaling_factor = getInput(kPortIDVelocityScalingFactor);
+ const auto acceleration_scaling_factor = getInput(kPortIDAccelerationScalingFactor);
+ const auto sampling_rate = getInput(kPortIDSamplingRate);
+
+ // Check that the required input data port was set
+ if (const auto error =
+ maybe_error(solution, joint_group_name, velocity_scaling_factor, acceleration_scaling_factor, sampling_rate);
+ error)
+ {
+ spdlog::error("Failed to get required value from input data port: {}", error.value());
+ return BT::NodeStatus::FAILURE;
+ }
+ // Initialize the robot model loader if this is the first time this Behavior has been run.
+ if (robot_model_loader_ == nullptr)
+ {
+ robot_model_loader_ = std::make_unique(shared_resources_->node);
+ }
+
+ // Get the robot model from the robot model loader.
+ const auto robot_model = robot_model_loader_->getModel();
+ if (robot_model == nullptr)
+ {
+ // Return as a failure case if the robot model loader has no robot model.
+ spdlog::error("Failed to load robot model.");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ // Get the JointModelGroup using the joint group name from the input port
+ auto joint_model_group = robot_model->getJointModelGroup(joint_group_name.value());
+ if (!joint_model_group)
+ {
+ spdlog::error("Failed to get JointModelGroup '{}'.", joint_group_name.value());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ // Extract joint positions from the MTC solution
+ const auto& waypoints = extractJointPositions(solution.value());
+
+ // Use trajectory_utils.hpp to create a trajectory
+ auto trajectory_result =
+ cartesian_planning::createTrajectoryFromWaypoints(*joint_model_group, waypoints, velocity_scaling_factor.value(),
+ acceleration_scaling_factor.value(), sampling_rate.value());
+
+ if (!trajectory_result)
+ {
+ spdlog::error("Trajectory generation failed: {}", trajectory_result.error());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ // Set the output port with the resulting joint trajectory
+ setOutput(kPortIDJointTrajectory, trajectory_result.value());
+
+ spdlog::info("Successfully converted MTC Solution to JointTrajectory with {} points.",
+ trajectory_result.value().points.size());
+
+ return BT::NodeStatus::SUCCESS;
+}
+
+const std::vector& ExampleConvertMtcSolutionToJointTrajectory::extractJointPositions(
+ const moveit_task_constructor_msgs::msg::Solution& solution)
+{
+ static std::vector waypoints;
+ waypoints.clear();
+ for (size_t sub_traj_idx = 0; sub_traj_idx < solution.sub_trajectory.size(); ++sub_traj_idx)
+ {
+ const auto& sub_traj = solution.sub_trajectory[sub_traj_idx];
+ const auto& joint_traj = sub_traj.trajectory.joint_trajectory;
+
+ if (joint_traj.points.empty())
+ {
+ spdlog::warn("Sub-trajectory {} has no points.", sub_traj_idx);
+ continue;
+ }
+
+ spdlog::info("Processing sub-trajectory {} with {} points.", sub_traj_idx, joint_traj.points.size());
+
+ for (const auto& point : joint_traj.points)
+ {
+ Eigen::VectorXd positions(point.positions.size());
+ for (size_t i = 0; i < point.positions.size(); ++i)
+ {
+ positions[i] = point.positions[i];
+ }
+ waypoints.push_back(positions);
+ }
+ }
+ return waypoints;
+}
+
+} // namespace example_behaviors
diff --git a/src/example_behaviors/src/example_create_string_msg.cpp b/src/example_behaviors/src/example_create_string_msg.cpp
new file mode 100644
index 000000000..224813616
--- /dev/null
+++ b/src/example_behaviors/src/example_create_string_msg.cpp
@@ -0,0 +1,51 @@
+#include
+
+#include
+#include
+
+namespace example_behaviors
+{
+ExampleCreateStringMsg::ExampleCreateStringMsg(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleCreateStringMsg::providedPorts()
+{
+ // This node has no input or output ports
+ return BT::PortsList(
+ { BT::InputPort("string", "example_string", "The value of the string ROS message to be created."),
+ BT::OutputPort("string_msg", "{string_msg}", "The string ROS message.") });
+}
+
+BT::KeyValueVector ExampleCreateStringMsg::metadata()
+{
+ return { { "subcategory", "Example Behaviors" }, { "description", "Create a ROS String msg on the blackboard." } };
+}
+
+BT::NodeStatus ExampleCreateStringMsg::tick()
+{
+ // getInput returns a BT::Expected so we'll store the result temporarily while we check if it was set correctly
+ const auto expected_string = getInput("string");
+
+ // The maybe_error function returns a std::optional with an error message if the port was set incorrectly
+ if (const auto error = moveit_studio::behaviors::maybe_error(expected_string); error)
+ {
+ // If the port was set incorrectly, we will log an error message to the UI and the node will return FAILURE
+ shared_resources_->logger->publishFailureMessage(name(), "Failed to get required values from input data ports." +
+ error.value());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ // Create the string msg with the input string.
+ std_msgs::msg::String string_msg;
+ string_msg.data = expected_string.value();
+
+ setOutput("string_msg", string_msg);
+
+ return BT::NodeStatus::SUCCESS;
+}
+
+} // namespace example_behaviors
diff --git a/src/example_behaviors/src/example_delayed_message.cpp b/src/example_behaviors/src/example_delayed_message.cpp
new file mode 100644
index 000000000..8522c852b
--- /dev/null
+++ b/src/example_behaviors/src/example_delayed_message.cpp
@@ -0,0 +1,78 @@
+#include
+
+namespace example_behaviors
+{
+ExampleDelayedMessage::ExampleDelayedMessage(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleDelayedMessage::providedPorts()
+{
+ // delay_duration: Number of seconds to wait before logging a message
+ return BT::PortsList(
+ { BT::InputPort("delay_duration", "5", "The duration, in seconds, to wait before logging a message.") });
+}
+
+BT::KeyValueVector ExampleDelayedMessage::metadata()
+{
+ return { { "subcategory", "Example Behaviors" },
+ { "description", "After some time, log a message that says \"Hello, world!\"." } };
+}
+
+BT::NodeStatus ExampleDelayedMessage::onStart()
+{
+ // Store the time at which this node was first ticked
+ start_time_ = std::chrono::steady_clock::now();
+
+ // getInput returns a BT::Expected so we'll store the result temporarily while we check if it was set correctly
+ const auto maybe_duration = getInput("delay_duration");
+
+ // The maybe_error function returns a std::optional with an error message if the port was set incorrectly
+ if (const auto error = moveit_studio::behaviors::maybe_error(maybe_duration); error)
+ {
+ // If the port was set incorrectly, we will log an error message to the UI and the node will return FAILURE
+ shared_resources_->logger->publishFailureMessage(name(), "Failed to get required values from input data ports." +
+ error.value());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ // Store the value of the port
+ delay_duration_ = maybe_duration.value();
+
+ // If the duration is zero, we can log the message immediately
+ if (delay_duration_ <= 0)
+ {
+ // Log the "Hello, world!" message.
+ // Setting the third argument to false ensures the message will be shown immediately
+ shared_resources_->logger->publishInfoMessage(name(), "Hello, world!");
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus ExampleDelayedMessage::onRunning()
+{
+ // If the delay duration has not elapsed since this node was started, return RUNNING
+ if (std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time_).count() <
+ delay_duration_)
+ {
+ // Log the "Hello, world!" message.
+ // Setting the third argument to false ensures the message will be shown immediately
+ shared_resources_->logger->publishInfoMessage(name(), "Hello, world!");
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+}
+
+void ExampleDelayedMessage::onHalted()
+{
+}
+
+} // namespace example_behaviors
diff --git a/src/example_behaviors/src/example_fibonacci_action_client.cpp b/src/example_behaviors/src/example_fibonacci_action_client.cpp
new file mode 100644
index 000000000..c46252912
--- /dev/null
+++ b/src/example_behaviors/src/example_fibonacci_action_client.cpp
@@ -0,0 +1,90 @@
+#include
+
+// Include the template implementation for ActionClientBehaviorBase.
+#include
+
+namespace example_behaviors
+{
+ExampleFibonacciActionClient::ExampleFibonacciActionClient(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : ActionClientBehaviorBase(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleFibonacciActionClient::providedPorts()
+{
+ // This node has two input ports and two output ports
+ return BT::PortsList({
+ BT::InputPort("action_name", "/fibonacci", "The name of the action to send a goal to."),
+ BT::InputPort("order", "The order of the Fibonacci sequence"),
+ BT::OutputPort>("feedback", "{feedback}",
+ "The action feedback containing the latest element in the current Fibonacci "
+ "sequence."),
+ BT::OutputPort>(
+ "result", "{result}", "The result containing the entire Fibonacci sequence up to the specified order."),
+ });
+}
+
+BT::KeyValueVector ExampleFibonacciActionClient::metadata()
+{
+ return { { "subcategory", "Example Behaviors" },
+ { "description",
+ "Calls an action to get a Fibonacci sequence and makes the result available on an output port." } };
+}
+
+tl::expected ExampleFibonacciActionClient::getActionName()
+{
+ const auto action_name = getInput("action_name");
+ if (const auto error = moveit_studio::behaviors::maybe_error(action_name))
+ {
+ return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
+ }
+ return action_name.value();
+}
+
+tl::expected ExampleFibonacciActionClient::createGoal()
+{
+ const auto order = getInput("order");
+
+ if (const auto error = moveit_studio::behaviors::maybe_error(order))
+ {
+ return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
+ }
+
+ return example_interfaces::build().order(order.value());
+}
+
+tl::expected
+ExampleFibonacciActionClient::processResult(const std::shared_ptr result)
+{
+ std::stringstream stream;
+ for (const auto& value : result->sequence)
+ {
+ stream << value << " ";
+ }
+ std::cout << stream.str() << std::endl;
+
+ // Publish the result to the UI
+ shared_resources_->logger->publishInfoMessage(name(), "Result: " + stream.str());
+
+ setOutput>("result", result->sequence);
+
+ return { true };
+}
+
+void ExampleFibonacciActionClient::processFeedback(const std::shared_ptr feedback)
+{
+ std::stringstream stream;
+ for (const auto& value : feedback->sequence)
+ {
+ stream << value << " ";
+ }
+ std::cout << stream.str() << std::endl;
+
+ // Publish the feedback to the UI
+ shared_resources_->logger->publishInfoMessage(name(), "Feedback: " + stream.str());
+
+ setOutput>("feedback", feedback->sequence);
+}
+} // namespace example_behaviors
diff --git a/src/example_behaviors/src/example_get_string_from_topic.cpp b/src/example_behaviors/src/example_get_string_from_topic.cpp
new file mode 100644
index 000000000..8915df967
--- /dev/null
+++ b/src/example_behaviors/src/example_get_string_from_topic.cpp
@@ -0,0 +1,32 @@
+#include
+
+// Include the template implementation for GetMessageFromTopicBehaviorBase.
+#include
+
+namespace example_behaviors
+{
+ExampleGetStringFromTopic::ExampleGetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : GetMessageFromTopicBehaviorBase(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleGetStringFromTopic::providedPorts()
+{
+ // This node has one input port and one output port
+ return BT::PortsList({
+ BT::InputPort("topic_name", "/chatter", "The name of the topic the Behavior subscribes to."),
+ BT::OutputPort>("message_out", "{my_string}", "The output String message."),
+ });
+}
+
+BT::KeyValueVector ExampleGetStringFromTopic::metadata()
+{
+ return { { "subcategory", "Example Behaviors" },
+ { "description", "Captures a string message and makes it available on an output port." } };
+}
+
+} // namespace example_behaviors
+
+// Template specialization of GetMessageFromTopicBehaviorBase for the String message type
+template class moveit_studio::behaviors::GetMessageFromTopicBehaviorBase;
diff --git a/src/example_behaviors/src/example_hello_world.cpp b/src/example_behaviors/src/example_hello_world.cpp
new file mode 100644
index 000000000..33de9997b
--- /dev/null
+++ b/src/example_behaviors/src/example_hello_world.cpp
@@ -0,0 +1,31 @@
+#include
+
+namespace example_behaviors
+{
+ExampleHelloWorld::ExampleHelloWorld(const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleHelloWorld::providedPorts()
+{
+ // This node has no input or output ports
+ return BT::PortsList({});
+}
+
+BT::KeyValueVector ExampleHelloWorld::metadata()
+{
+ return { { "subcategory", "Example Behaviors" }, { "description", "Log a message that says \"Hello, world!\"." } };
+}
+
+BT::NodeStatus ExampleHelloWorld::tick()
+{
+ // Do ExampleHelloWorld's useful work.
+ // Setting the third argument to false ensures the message will be shown immediately
+ shared_resources_->logger->publishInfoMessage(name(), "Hello, world!");
+
+ return BT::NodeStatus::SUCCESS;
+}
+
+} // namespace example_behaviors
diff --git a/src/example_behaviors/src/example_ndt_registration.cpp b/src/example_behaviors/src/example_ndt_registration.cpp
new file mode 100644
index 000000000..6c8af7179
--- /dev/null
+++ b/src/example_behaviors/src/example_ndt_registration.cpp
@@ -0,0 +1,143 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace example_behaviors
+{
+namespace
+{
+
+std::shared_ptr>
+getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg)
+{
+ const auto cloud = std::make_shared>();
+ pcl::fromROSMsg(cloud_msg, *cloud);
+
+ const pcl::PointIndices valid_indices =
+ moveit_studio::selectPointIndices(*cloud, moveit_studio::point_cloud_tools::getAllIndices(cloud),
+ moveit_studio::NeitherNanNorNearZeroPointValidator);
+ auto filtered_cloud = std::make_shared>();
+ pcl::copyPointCloud(*cloud, valid_indices, *filtered_cloud);
+
+ return filtered_cloud;
+}
+
+} // namespace
+
+ExampleNDTRegistration::ExampleNDTRegistration(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleNDTRegistration::providedPorts()
+{
+ return { BT::InputPort("base_point_cloud", "{point_cloud}",
+ "Point cloud to align with the target point cloud."),
+ BT::InputPort("target_point_cloud", "{target_point_cloud}",
+ "Point cloud to which align the base point cloud."),
+ BT::InputPort("max_iterations", 30,
+ "Maximum number of attempts to find the transform. Setting a higher number of iterations "
+ "will allow the solver to converge even if the initial estimate of the transform was far "
+ "from the actual transform, but it may take longer to complete."),
+ BT::InputPort("transformation_epsilon", 0.001,
+ "Minimum transformation difference for termination condition "),
+ BT::InputPort("step_size", 0.1, "Maximum step size for More-Thuente line search "),
+ BT::InputPort("resolution", 1.0, "Resolution of NDT grid structure (VoxelGridCovariance) "),
+ BT::InputPort("inlier_threshold", 1.0,
+ "Resolution of NDT grid structure (VoxelGridCovariance) "),
+ BT::OutputPort("target_pose_in_base_frame", "{target_pose}",
+ "The pose of the target point cloud relative to the frame "
+ "of the base point cloud.") };
+}
+BT::KeyValueVector ExampleNDTRegistration::metadata()
+{
+ return { { "subcategory", "Perception - 3D Point Cloud" },
+ { "description", "Finds the pose of a target point cloud relative to the base frame of a base point cloud "
+ "using the Normal Distributions Transform (NDT) algorithm" } };
+}
+
+tl::expected ExampleNDTRegistration::doWork()
+{
+ const auto base_point_cloud_msg = getInput("base_point_cloud");
+ const auto target_point_cloud_msg = getInput("target_point_cloud");
+ const auto max_iterations = getInput("max_iterations");
+ const auto transformation_epsilon = getInput("transformation_epsilon");
+ const auto step_size = getInput("step_size");
+ const auto resolution = getInput("resolution");
+
+ if (const auto error = moveit_studio::behaviors::maybe_error(
+ base_point_cloud_msg, target_point_cloud_msg, max_iterations, transformation_epsilon, step_size, resolution);
+ error)
+ {
+ return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
+ }
+
+ const auto base_cloud = getFilteredPointCloudFromMessage(base_point_cloud_msg.value());
+ if (base_cloud->empty())
+ {
+ return tl::make_unexpected("Base point cloud has no valid points");
+ }
+ const auto target_cloud = getFilteredPointCloudFromMessage(target_point_cloud_msg.value());
+ if (target_cloud->empty())
+ {
+ return tl::make_unexpected("Target point cloud has no valid points");
+ }
+ // Initializing Normal Distributions Transform (NDT).
+ pcl::NormalDistributionsTransform ndt;
+
+ // Setting scale dependent NDT parameters
+ // Setting minimum transformation difference for termination condition.
+ ndt.setTransformationEpsilon(transformation_epsilon.value());
+ // Setting maximum step size for More-Thuente line search.
+ ndt.setStepSize(step_size.value());
+ // Setting Resolution of NDT grid structure (VoxelGridCovariance).
+ ndt.setResolution(resolution.value());
+
+ // Setting max number of registration iterations.
+ ndt.setMaximumIterations(max_iterations.value());
+
+ // Setting point cloud to be aligned.
+ ndt.setInputSource(base_cloud);
+ // Setting point cloud to be aligned to.
+ ndt.setInputTarget(target_cloud);
+
+ // Calculating required rigid transform to align the input cloud to the target cloud.
+ pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud);
+ ndt.align(*output_cloud);
+
+ if (!ndt.hasConverged())
+ {
+ return tl::make_unexpected("NDT could not converge to a transform that aligns both point clouds");
+ }
+
+ geometry_msgs::msg::PoseStamped out;
+ out.pose = tf2::toMsg(Eigen::Isometry3d{ ndt.getFinalTransformation().cast() });
+ out.header.frame_id = base_point_cloud_msg.value().header.frame_id;
+ out.header.stamp = base_point_cloud_msg.value().header.stamp;
+ setOutput("target_pose_in_base_frame", out);
+
+ return true;
+}
+
+} // namespace example_behaviors
diff --git a/src/example_behaviors/src/example_publish_color_rgba.cpp b/src/example_behaviors/src/example_publish_color_rgba.cpp
new file mode 100644
index 000000000..18d6d27b7
--- /dev/null
+++ b/src/example_behaviors/src/example_publish_color_rgba.cpp
@@ -0,0 +1,35 @@
+#include
+
+namespace example_behaviors
+{
+ExamplePublishColorRGBA::ExamplePublishColorRGBA(
+ const std::string& name, const BT::NodeConfiguration& config,
+ const std::shared_ptr& shared_resources)
+ : moveit_studio::behaviors::SharedResourcesNode(name, config, shared_resources)
+ , publisher_{ shared_resources_->node->create_publisher("/my_topic", rclcpp::QoS(1)) }
+{
+}
+
+BT::PortsList ExamplePublishColorRGBA::providedPorts()
+{
+ return {};
+}
+
+BT::KeyValueVector ExamplePublishColorRGBA::metadata()
+{
+ return { { "subcategory", "Example Behaviors" },
+ { "description", "Publishes a fixed std_msgs::msg::ColorRGBA message to a topic named \"/my_topic\"" } };
+}
+
+BT::NodeStatus ExamplePublishColorRGBA::tick()
+{
+ std_msgs::msg::ColorRGBA color_msg;
+ color_msg.r = 0.5;
+ color_msg.g = 0.5;
+ color_msg.b = 0.5;
+ color_msg.a = 0.5;
+ publisher_->publish(color_msg);
+
+ return BT::NodeStatus::SUCCESS;
+}
+} // namespace example_behaviors
diff --git a/src/example_behaviors/src/example_ransac_registration.cpp b/src/example_behaviors/src/example_ransac_registration.cpp
new file mode 100644
index 000000000..009d3ef21
--- /dev/null
+++ b/src/example_behaviors/src/example_ransac_registration.cpp
@@ -0,0 +1,235 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include