Added example behavior and interface behaviors loader

Former-commit-id: 73feb3b

Author: MikeWrock

src/example_behaviors/CMakeLists.txt

@@ -5,7 +5,7 @@ find_package(moveit_studio_common REQUIRED)
 find_package(example_interfaces REQUIRED)
 moveit_studio_package()
 
-set(THIS_PACKAGE_INCLUDE_DEPENDS cartesian_planning moveit_studio_behavior
+set(THIS_PACKAGE_INCLUDE_DEPENDS cartesian_planning geometry_msgs 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})
@@ -18,6 +18,7 @@ add_library(
   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_pose_stamped_from_topic.cpp
   src/example_get_string_from_topic.cpp
   src/example_fibonacci_action_client.cpp
   src/example_hello_world.cpp

src/example_behaviors/include/example_behaviors/example_get_pose_stamped_from_topic.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <moveit_studio_behavior_interface/get_message_from_topic.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+namespace example_behaviors
+{
+class ExampleGetPoseStampedFromTopic final
+  : public moveit_studio::behaviors::GetMessageFromTopicBehaviorBase<geometry_msgs::msg::PoseStamped>
+{
+public:
+  ExampleGetPoseStampedFromTopic(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();
+
+private:
+  std::shared_future<tl::expected<bool, std::string>>& getFuture() override
+  {
+    return future_;
+  }
+
+  std::shared_future<tl::expected<bool, std::string>> future_;
+};
+}  // namespace example_behaviors

src/example_behaviors/package.xml

@@ -15,6 +15,7 @@
 
   <depend>moveit_studio_behavior_interface</depend>
   <depend>example_interfaces</depend>
+  <depend>geometry_msgs</depend>
   <depend>perception_pcl</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_studio_vision_msgs</depend>

src/example_behaviors/src/example_get_pose_stamped_from_topic.cpp

@@ -0,0 +1,34 @@
+#include <example_behaviors/example_get_pose_stamped_from_topic.hpp>
+
+#include <moveit_studio_behavior_interface/impl/get_message_from_topic_impl.hpp>
+
+namespace example_behaviors
+{
+// Mirrors the PoseStamped specialization produced by generate_topic_interface_behaviors.py so developers
+// can see the generated pattern inside example_behaviors.
+ExampleGetPoseStampedFromTopic::ExampleGetPoseStampedFromTopic(
+    const std::string& name, const BT::NodeConfiguration& config,
+    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
+  : GetMessageFromTopicBehaviorBase<geometry_msgs::msg::PoseStamped>(name, config, shared_resources)
+{
+}
+
+BT::PortsList ExampleGetPoseStampedFromTopic::providedPorts()
+{
+  return BT::PortsList({
+      BT::InputPort<std::string>(kPortIDTopicName, "/pose_stamped",
+                                 "Topic that publishes <code>geometry_msgs::msg::PoseStamped</code> messages."),
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>(kPortIDMessageOut, "{pose_stamped}",
+                                                      "Output pose from the subscribed topic."),
+  });
+}
+
+BT::KeyValueVector ExampleGetPoseStampedFromTopic::metadata()
+{
+  return { { "subcategory", "Example Behaviors" },
+           { "description", "Capture a pose stamped message from a topic and store it on the blackboard." } };
+}
+
+}  // namespace example_behaviors
+
+template class moveit_studio::behaviors::GetMessageFromTopicBehaviorBase<geometry_msgs::msg::PoseStamped>;

src/example_behaviors/src/register_behaviors.cpp

@@ -9,6 +9,7 @@
 #include <example_behaviors/example_create_string_msg.hpp>
 #include <example_behaviors/example_delayed_message.hpp>
 #include <example_behaviors/example_fibonacci_action_client.hpp>
+#include <example_behaviors/example_get_pose_stamped_from_topic.hpp>
 #include <example_behaviors/example_get_string_from_topic.hpp>
 #include <example_behaviors/example_hello_world.hpp>
 #include <example_behaviors/example_ndt_registration.hpp>
@@ -36,6 +37,9 @@ class ExampleBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesN
                                                                         shared_resources);
     moveit_studio::behaviors::registerBehavior<ExampleGetStringFromTopic>(factory, "ExampleGetStringFromTopic",
                                                                           shared_resources);
+    moveit_studio::behaviors::registerBehavior<ExampleGetPoseStampedFromTopic>(factory,
+                                                                              "ExampleGetPoseStampedFromTopic",
+                                                                              shared_resources);
     moveit_studio::behaviors::registerBehavior<ExampleAddTwoIntsServiceClient>(
         factory, "ExampleAddTwoIntsServiceClient", shared_resources);
     moveit_studio::behaviors::registerBehavior<ExampleFibonacciActionClient>(factory, "ExampleFibonacciActionClient",

src/factory_sim/config/config.yaml

@@ -133,6 +133,7 @@ objectives:
     # Add additional plugin loaders as needed.
     core:
       - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+      - "moveit_studio::behaviors::TopicInterfaceBehaviorsLoader"
       - "moveit_studio::behaviors::CoreBehaviorsLoader"
       - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
       - "moveit_studio::behaviors::ServoBehaviorsLoader"

src/grinding_sim/config/config.yaml

@@ -129,6 +129,7 @@ objectives:
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
       - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+      - "moveit_studio::behaviors::TopicInterfaceBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:

src/hangar_sim/config/config.yaml

@@ -70,6 +70,7 @@ objectives:
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
       - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+      - "moveit_studio::behaviors::TopicInterfaceBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:

src/lab_sim/config/config.yaml

@@ -54,6 +54,7 @@ objectives:
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
       - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+      - "moveit_studio::behaviors::TopicInterfaceBehaviorsLoader"
       - "moveit_studio::behaviors::MujocoBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]

src/moveit_pro_franka_configs/dual_arm_sim/config/config.yaml

@@ -99,6 +99,7 @@ objectives:
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
       - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+      - "moveit_studio::behaviors::TopicInterfaceBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths: