Squashed 'src/example_behaviors/' changes from d1451e3..b360dc0

b360dc0 Merge commit '7e8c6066177e2fd16ea46cfb16d8c207e17fd929' a71ffeb fixed build and test for example behaviors (#39) 72ad640 Sam naming (#38) 46a17ce Sync 6.5 to main (#37) d41f38d Add example SAM 2 behavior and objective (#23) f6ac548 Sync V6.5 to main (#29) e932e45 Merge branch 'main' into pr-update-to-make-ports-explicit 6a14c44 Fix depend of moveit_studio_behavior 1416547 version bump 5f6056a formatting a06f504 pre-commit formatting 5bfeaa5 Moving registration behaviors 93dd16f Added mtc to jtc converter git-subtree-dir: src/example_behaviors git-subtree-split: b360dc0970f6dbe4b633cb33edb9ef75042f00d7
PickNikRobotics · Dec 6, 2024 · 2bc1b8e · 2bc1b8e
1 parent d0dad84
commit 2bc1b8e
Show file tree

Hide file tree

Showing 41 changed files with 1,749 additions and 136 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,30 +5,43 @@ find_package(moveit_studio_common REQUIRED)
 find_package(example_interfaces REQUIRED)
 moveit_studio_package()
 
-set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib example_interfaces)
+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()
+find_package(moveit_pro_ml REQUIRED)
 
 add_library(
   example_behaviors
   SHARED 
-  src/add_two_ints_service_client.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/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_pick_from_pose.cpp
+  src/example_setup_mtc_place_from_pose.cpp
+  src/example_setup_mtc_wave_hand.cpp
+  src/example_ndt_registration.cpp
+  src/example_ransac_registration.cpp
+  src/example_sam2_segmentation.cpp
   src/register_behaviors.cpp)
 target_include_directories(
   example_behaviors
   PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-         $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+  PRIVATE ${PCL_INCLUDE_DIRS})
 ament_target_dependencies(example_behaviors
                           ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(example_behaviors onnx_sam2)
 
 # Install Libraries
 install(
@@ -40,7 +53,7 @@ install(
   INCLUDES
   DESTINATION include)
 
-install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config models DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   moveit_pro_behavior_test(example_behaviors)

diff --git a/COLCON_IGNORE b/COLCON_IGNORE
diff --git a/behavior_plugin.yaml b/behavior_plugin.yaml
@@ -1,4 +1,4 @@
 objectives:
   behavior_loader_plugins:
     example_behaviors:
-      - "example_behaviors::ExampleBehaviorsBehaviorsLoader"
+      - "example_behaviors::ExampleBehaviorsLoader"
diff --git a/example_behaviors_plugin_description.xml b/example_behaviors_plugin_description.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8" ?>
 <library path="example_behaviors">
   <class
-    type="example_behaviors::ExampleBehaviorsBehaviorsLoader"
+    type="example_behaviors::ExampleBehaviorsLoader"
     base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"
   />
 </library>
diff --git a/include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp b/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
diff --git a/...behaviors/add_two_ints_service_client.hpp → ...s/example_add_two_ints_service_client.hpp b/...behaviors/add_two_ints_service_client.hpp → ...s/example_add_two_ints_service_client.hpp
@@ -9,10 +9,10 @@ using AddTwoInts = example_interfaces::srv::AddTwoInts;
 
 namespace example_behaviors
 {
-class AddTwoIntsServiceClient final : public ServiceClientBehaviorBase<AddTwoInts>
+class ExampleAddTwoIntsServiceClient final : public ServiceClientBehaviorBase<AddTwoInts>
 {
 public:
-  AddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config,
+  ExampleAddTwoIntsServiceClient(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. */

diff --git a/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp b/include/example_behaviors/example_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 ExampleConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
+{
+public:
+  ExampleConvertMtcSolutionToJointTrajectory(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
diff --git a/...ude/example_behaviors/delayed_message.hpp → ...ple_behaviors/example_delayed_message.hpp b/...ude/example_behaviors/delayed_message.hpp → ...ple_behaviors/example_delayed_message.hpp
@@ -12,9 +12,9 @@
 namespace example_behaviors
 {
 /**
- * @brief DelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on the input port
+ * @brief ExampleDelayedMessage 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>
+class ExampleDelayedMessage : public moveit_studio::behaviors::SharedResourcesNode<BT::StatefulActionNode>
 {
 private:
   std::chrono::time_point<std::chrono::steady_clock> start_time_;
@@ -33,7 +33,7 @@ class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode<BT::
    * @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,
+  ExampleDelayedMessage(const std::string& name, const BT::NodeConfiguration& config,
                  const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
 
   /**

diff --git a/...ple_behaviors/fibonacci_action_client.hpp → ...viors/example_fibonacci_action_client.hpp b/...ple_behaviors/fibonacci_action_client.hpp → ...viors/example_fibonacci_action_client.hpp
@@ -9,10 +9,10 @@ using Fibonacci = example_interfaces::action::Fibonacci;
 
 namespace example_behaviors
 {
-class FibonacciActionClient final : public ActionClientBehaviorBase<Fibonacci>
+class ExampleFibonacciActionClient final : public ActionClientBehaviorBase<Fibonacci>
 {
 public:
-  FibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config,
+  ExampleFibonacciActionClient(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. */

diff --git a/...ample_behaviors/get_string_from_topic.hpp → ...haviors/example_get_string_from_topic.hpp b/...ample_behaviors/get_string_from_topic.hpp → ...haviors/example_get_string_from_topic.hpp
@@ -8,10 +8,10 @@ using moveit_studio::behaviors::GetMessageFromTopicBehaviorBase;
 
 namespace example_behaviors
 {
-class GetStringFromTopic final : public GetMessageFromTopicBehaviorBase<std_msgs::msg::String>
+class ExampleGetStringFromTopic final : public GetMessageFromTopicBehaviorBase<std_msgs::msg::String>
 {
 public:
-  GetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config,
+  ExampleGetStringFromTopic(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. */

diff --git a/include/example_behaviors/hello_world.hpp → ...example_behaviors/example_hello_world.hpp b/include/example_behaviors/hello_world.hpp → ...example_behaviors/example_hello_world.hpp
@@ -8,9 +8,9 @@
 namespace example_behaviors
 {
 /**
- * @brief The HelloWorld Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS
+ * @brief The ExampleHelloWorld Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS
  */
-class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
+class ExampleHelloWorld : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
 {
 public:
   /**
@@ -25,7 +25,7 @@ class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode<BT::Sync
    * @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.
    */
-  HelloWorld(const std::string& name, const BT::NodeConfiguration& config,
+  ExampleHelloWorld(const std::string& name, const BT::NodeConfiguration& config,
              const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
 
   /**
@@ -46,9 +46,9 @@ class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode<BT::Sync
   static BT::KeyValueVector metadata();
 
   /**
-   * @brief Implementation of BT::SyncActionNode::tick() for HelloWorld.
+   * @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 HelloWorld is derived from BT::SyncActionNode, it is very important that its tick() function always finishes
+   * 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

diff --git a/include/example_behaviors/example_ndt_registration.hpp b/include/example_behaviors/example_ndt_registration.hpp
@@ -0,0 +1,55 @@
+#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_studio_behavior_interface/async_behavior_base.hpp>
+
+
+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<moveit_studio::behaviors::BehaviorContext>& 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<bool, std::string> doWork() override;
+
+private:
+  /** @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
diff --git a/.../example_behaviors/publish_color_rgba.hpp → ..._behaviors/example_publish_color_rgba.hpp b/.../example_behaviors/publish_color_rgba.hpp → ..._behaviors/example_publish_color_rgba.hpp
@@ -12,10 +12,10 @@
 
 namespace example_behaviors
 {
-class PublishColorRGBA final : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
+class ExamplePublishColorRGBA final : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
 {
 public:
-  PublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config,
+  ExamplePublishColorRGBA(const std::string& name, const BT::NodeConfiguration& config,
                    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
 
   static BT::PortsList providedPorts();

diff --git a/include/example_behaviors/example_ransac_registration.hpp b/include/example_behaviors/example_ransac_registration.hpp
@@ -0,0 +1,55 @@
+#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_studio_behavior_interface/async_behavior_base.hpp>
+
+
+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<moveit_studio::behaviors::BehaviorContext>& 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<bool, std::string> doWork() override;
+
+private:
+  /** @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