Squashed 'src/example_behaviors/' content from commit d1451e3

git-subtree-dir: src/example_behaviors
git-subtree-split: d1451e331ba27aee0af9bbaafaefc8bf37a48aca

Author: MikeWrock

CMakeLists.txt

@@ -0,0 +1,57 @@
+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_interface pluginlib 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/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/register_behaviors.cpp)
+target_include_directories(
+  example_behaviors
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include>)
+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()

README.md

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

behavior_plugin.yaml

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

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>

example_behaviors_plugin_description.xml

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

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

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

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

include/example_behaviors/get_string_from_topic.hpp

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <moveit_studio_behavior_interface/get_message_from_topic.hpp>
+#include <std_msgs/msg/string.hpp>
+
+using moveit_studio::behaviors::BehaviorContext;
+using moveit_studio::behaviors::GetMessageFromTopicBehaviorBase;
+
+namespace example_behaviors
+{
+class GetStringFromTopic final : public GetMessageFromTopicBehaviorBase<std_msgs::msg::String>
+{
+public:
+  GetStringFromTopic(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 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

include/example_behaviors/hello_world.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+
+// This header includes the SharedResourcesNode type
+#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
+
+namespace example_behaviors
+{
+/**
+ * @brief The HelloWorld Behavior uses FailureLoggerROS to log a "Hello World" message and will always return SUCCESS
+ */
+class HelloWorld : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
+{
+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.
+   */
+  HelloWorld(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 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 HelloWorld.
+   * @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
+   * 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