Add Registration Behaviors

.gitignore

@@ -0,0 +1,4 @@
+build/
+install/
+log/
+.vscode/

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "src/picknik_accessories"]
+	path = src/picknik_accessories
+	url = https://github.com/PickNikRobotics/picknik_accessories.git

README.md

@@ -1,7 +1,63 @@
-# MoveIt Pro Workspace
+# MoveIt Pro Example Workspace
 
-This is a minimal user workspace that can be used to build customized MoveIt Pro workspaces.
+This is fork of the [MoveIt Pro Empty Workspace](https://github.com/PickNikRobotics/moveit_pro_empty_ws).
+This workspace contains reference materials for using MoveIt Pro, including:
+- [Example base UR5e configuration](src/moveit_pro_ur_configs/picknik_ur_base_config)
+- [A physics based simulation environment with a robot on a linear rail](src/lab_sim)
+- [Mobile manipulation configuration](src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config)
+- [Example behaviors](src/example_behaviors)
 
-You may fork this repository and add the MoveIt Pro configurations and ROS 2 packages of your choosing to the `src` folder.
+Since the [picknik_accessories](https://github.com/PickNikRobotics/picknik_accessories) package uses git LFS, [it cannot be added as a subtree](https://github.com/git-lfs/git-lfs/issues/854).
+Please ensure you have the submodule up to date using:
+```bash
+git submodule update --recursive --init
+```
+
+## Working with Git Subtrees
+
+This repository was created through the combination of multiple repositories using git subtree.
+If you have no intrest in manually pulling or pushing upstream changes, you can ignore the following section and treat this repository as a single repository.
+
+### Repository Structure
+
+
+The structure of this repository is as follows:
+
+.
+├── README.md
+└── src
+    ├── [example_behaviors](https://github.com/PickNikRobotics/example_behaviors)
+    ├── [lab_sim](https://github.com/PickNikRobotics/lab_sim)
+    ├── [moveit_pro_ur_configs](https://github.com/PickNikRobotics/moveit_pro_ur_configs)
+    │   ├── picknik_ur_base_config
+    │   ├── picknik_ur_mock_hw_config
+    │   ├── picknik_ur_multi_arm_config
+    │   ├── picknik_ur_sim_config
+    │   └── picknik_ur_site_config
+    ├── [moveit_pro_kinova_configs](https://github.com/PickNikRobotics/moveit_pro_kinova_configs)
+    │   ├── kinova_gen3_base_config
+    │   ├── kinova_gen3_mujoco_config
+    │   └── moveit_studio_kinova_pstop_manager
+    ├── [moveit_pro_mobile_manipulation](https://github.com/PickNikRobotics/moveit_pro_mobile_manipulation)
+    │   ├── mobile_manipulation_config
+    │   └── picknik_ur_mobile_config
+    ├── [fanuc_sim](https://github.com/PickNikRobotics/fanuc_sim)
+    ├── [picknik_accessories](https://github.com/PickNikRobotics/picknik_accessories) (submodule)
+    └── external_dependencies
+        ├── [ridgeback](https://github.com/sjahr/ridgeback/tree/ros2)
+        ├── [ros2_robotiq_gripper](https://github.com/PickNikRobotics/ros2_robotiq_gripper)
+        └── [serial](https://github.com/tylerjw/serial/tree/ros2)
+
+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
+```
 
-For more information, refer to the [MoveIt Pro Documentation](https://docs.picknik.ai/).

src/example_behaviors/CMakeLists.txt

@@ -0,0 +1,67 @@
+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 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/add_two_ints_service_client.cpp
+  src/convert_mtc_solution_to_joint_trajectory.cpp
+  src/delayed_message.cpp
+  src/get_string_from_topic.cpp
+  src/fibonacci_action_client.cpp
+  src/hello_world.cpp
+  src/publish_color_rgba.cpp
+  src/setup_mtc_pick_from_pose.cpp
+  src/setup_mtc_place_from_pose.cpp
+  src/setup_mtc_wave_hand.cpp
+  src/ndt_registration.cpp
+  src/ransac_registration.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+  example_behaviors
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  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()

src/example_behaviors/README.md

@@ -0,0 +1 @@
+# example_behaviors

src/example_behaviors/behavior_plugin.yaml

@@ -0,0 +1,4 @@
+objectives:
+  behavior_loader_plugins:
+    example_behaviors:
+      - "example_behaviors::ExampleBehaviorsLoader"

src/example_behaviors/config/tree_nodes_model.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root>
+  <TreeNodesModel>
+    <!-- Include additional SubTree metadata in this file. -->
+  </TreeNodesModel>
+</root>

src/example_behaviors/example_behaviors_plugin_description.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<library path="example_behaviors">
+  <class
+    type="example_behaviors::ExampleBehaviorsLoader"
+    base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"
+  />
+</library>

src/example_behaviors/include/example_behaviors/add_two_ints_service_client.hpp

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <moveit_studio_behavior_interface/service_client_behavior_base.hpp>
+#include <example_interfaces/srv/add_two_ints.hpp>
+
+using moveit_studio::behaviors::BehaviorContext;
+using moveit_studio::behaviors::ServiceClientBehaviorBase;
+using AddTwoInts = example_interfaces::srv::AddTwoInts;
+
+namespace example_behaviors
+{
+class AddTwoIntsServiceClient final : public ServiceClientBehaviorBase<AddTwoInts>
+{
+public:
+  AddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config,
+                          const std::shared_ptr<BehaviorContext>& 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<std::string, std::string> 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<AddTwoInts::Request, std::string> createRequest() override;
+
+  /** @brief Optional user-provided function to process the service response after the service has finished. */
+  tl::expected<bool, std::string> 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<tl::expected<bool, std::string>>& getFuture() override
+  {
+    return future_;
+  }
+
+  /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+  std::shared_future<tl::expected<bool, std::string>> future_;
+};
+}  // namespace example_behaviors

src/example_behaviors/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <behaviortree_cpp/action_node.h>
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit_msgs/msg/robot_trajectory.hpp>
+#include <moveit_studio_behavior/utils/trajectory_utils.hpp>
+#include <moveit_studio_behavior_interface/behavior_context.hpp>
+#include <moveit_studio_behavior_interface/check_for_error.hpp>
+#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
+#include <moveit_task_constructor_msgs/msg/solution.hpp>
+#include <spdlog/spdlog.h>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <yaml-cpp/yaml.h>
+
+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 ConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
+{
+public:
+  ConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config,
+                                      const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+
+  static BT::KeyValueVector metadata();
+
+  BT::NodeStatus tick() override;
+
+private:
+  std::unique_ptr<robot_model_loader::RobotModelLoader> robot_model_loader_;
+
+  const std::vector<Eigen::VectorXd>& extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution);
+};
+}  // namespace example_behaviors

src/example_behaviors/include/example_behaviors/delayed_message.hpp

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+
+// This header includes the SharedResourcesNode type
+#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
+
+#include <moveit_msgs/msg/move_it_error_codes.hpp>
+
+#include <moveit_studio_behavior_interface/check_for_error.hpp>
+
+namespace example_behaviors
+{
+/**
+ * @brief DelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on the input port
+ */
+class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode<BT::StatefulActionNode>
+{
+private:
+  std::chrono::time_point<std::chrono::steady_clock> 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.
+   */
+  DelayedMessage(const std::string& name, const BT::NodeConfiguration& config,
+                 const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& 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

src/example_behaviors/include/example_behaviors/fibonacci_action_client.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <moveit_studio_behavior_interface/action_client_behavior_base.hpp>
+#include <example_interfaces/action/fibonacci.hpp>
+
+using moveit_studio::behaviors::ActionClientBehaviorBase;
+using moveit_studio::behaviors::BehaviorContext;
+using Fibonacci = example_interfaces::action::Fibonacci;
+
+namespace example_behaviors
+{
+class FibonacciActionClient final : public ActionClientBehaviorBase<Fibonacci>
+{
+public:
+  FibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config,
+                        const std::shared_ptr<BehaviorContext>& 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<std::string, std::string> getActionName() override;
+
+  /** @brief User-provided function to create the action goal before sending the action goal request. */
+  tl::expected<Fibonacci::Goal, std::string> createGoal() override;
+
+  /** @brief Optional user-provided function to process the action result after the action has finished. */
+  tl::expected<bool, std::string> processResult(const std::shared_ptr<Fibonacci::Result> result) override;
+
+  /** @brief Optional user-provided function to process feedback sent by the action server. */
+  void processFeedback(const std::shared_ptr<const Fibonacci::Feedback> feedback) override;
+
+  /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+  std::shared_future<tl::expected<bool, std::string>>& getFuture() override
+  {
+    return future_;
+  }
+
+  /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+  std::shared_future<tl::expected<bool, std::string>> future_;
+};
+}  // namespace example_behaviors