Add visual marker behaviors

Signed-off-by: Paul Gesel <[email protected]>

src/visualization_behaviors/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.22)
+project(visualization_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
+        geometry_msgs
+        pluginlib
+        tf2
+        tf2_eigen
+        tf2_geometry_msgs
+        tf2_ros
+        visualization_msgs
+        example_interfaces)
+foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${package} REQUIRED)
+endforeach()
+
+add_library(
+  visualization_behaviors
+  SHARED
+  src/example_publish_text_marker.cpp
+  src/example_publish_arrow_marker.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+        visualization_behaviors
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  PRIVATE ${PCL_INCLUDE_DIRS})
+ament_target_dependencies(visualization_behaviors
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Install Libraries
+install(
+  TARGETS visualization_behaviors
+  EXPORT visualization_behaviorsTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+# 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(visualization_behaviorsTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/visualization_behaviors/behavior_plugin.yaml

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

src/visualization_behaviors/example_behaviors_plugin_description.xml

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

src/visualization_behaviors/include/example_behaviors/example_publish_arrow_marker.hpp

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+
+// This header includes the SharedResourcesNode type
+#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
+
+#include <rclcpp/publisher.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace example_behaviors
+{
+/**
+ * @brief The ExamplePublishArrowMarker Behavior uses FailureLoggerROS to log a "Hello World" message and will always
+ * return SUCCESS
+ */
+class ExamplePublishArrowMarker : 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.
+   */
+  ExamplePublishArrowMarker(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 ExamplePublishArrowMarker.
+   * @details This function is where the Behavior performs its work when the behavior tree is being run.
+   * Since ExamplePublishArrowMarker 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;
+
+private:
+  std::shared_ptr<rclcpp::Publisher<visualization_msgs::msg::MarkerArray>> marker_publisher_;
+};
+}  // namespace example_behaviors

src/visualization_behaviors/include/example_behaviors/example_publish_text_marker.hpp

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+
+// This header includes the SharedResourcesNode type
+#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
+
+#include <rclcpp/publisher.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace example_behaviors
+{
+/**
+ * @brief The ExamplePublishTextMarker Behavior uses FailureLoggerROS to log a "Hello World" message and will always
+ * return SUCCESS
+ */
+class ExamplePublishTextMarker : 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.
+   */
+  ExamplePublishTextMarker(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 ExamplePublishTextMarker.
+   * @details This function is where the Behavior performs its work when the behavior tree is being run.
+   * Since ExamplePublishTextMarker 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;
+
+private:
+  std::shared_ptr<rclcpp::Publisher<visualization_msgs::msg::MarkerArray>> marker_publisher_;
+};
+}  // namespace example_behaviors

src/visualization_behaviors/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<package format="3">
+  <name>visualization_behaviors</name>
+  <version>6.5.0</version>
+  <description>Example behaviors for MoveIt Pro</description>
+
+  <maintainer email="[email protected]">MoveIt Pro Maintainer</maintainer>
+  <author email="[email protected]">MoveIt Pro Maintainer</author>
+
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>moveit_studio_common</build_depend>
+
+  <depend>moveit_studio_behavior_interface</depend>
+  <depend>example_interfaces</depend>
+  <depend>perception_pcl</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>moveit_studio_vision_msgs</depend>
+  <depend>moveit_studio_vision</depend>
+  <depend>moveit_studio_behavior</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_clang_tidy</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/visualization_behaviors/src/example_publish_arrow_marker.cpp

@@ -0,0 +1,122 @@
+#include <example_behaviors/example_publish_arrow_marker.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <moveit_studio_behavior_interface/get_required_ports.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "../../../../moveit_studio/src/moveit_studio_vision/include/moveit_studio_vision/utils/tf_utils.hpp"
+
+namespace
+{
+// Port names for input and output ports.
+constexpr auto kPortIDStartPose = "start_pose";
+constexpr auto kPortIDEndPose = "end_pose";
+}  // namespace
+
+namespace example_behaviors
+{
+ExamplePublishArrowMarker::ExamplePublishArrowMarker(
+    const std::string& name, const BT::NodeConfiguration& config,
+    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
+  : moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>(name, config, shared_resources)
+{
+  marker_publisher_ = shared_resources_->node->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "visual_markers", rclcpp::SystemDefaultsQoS());
+}
+
+BT::PortsList ExamplePublishArrowMarker::providedPorts()
+{
+  // This node has no input or output ports
+  return BT::PortsList({
+      BT::InputPort<geometry_msgs::msg::PoseStamped>(kPortIDStartPose, "", "The start of the arrow"),
+      BT::InputPort<geometry_msgs::msg::PoseStamped>(kPortIDEndPose, "", "The end of the arrow."),
+  });
+}
+
+BT::KeyValueVector ExamplePublishArrowMarker::metadata()
+{
+  return { { "subcategory", "Example Behaviors" }, { "description", "Publish marker" } };
+}
+
+BT::NodeStatus ExamplePublishArrowMarker::tick()
+{
+  const auto ports =
+      moveit_studio::behaviors::getRequiredInputs(getInput<geometry_msgs::msg::PoseStamped>(kPortIDStartPose),
+                                                  getInput<geometry_msgs::msg::PoseStamped>(kPortIDEndPose));
+
+  // If a port was set incorrectly, log an error message and return FAILURE
+  if (!ports.has_value())
+  {
+    shared_resources_->logger->publishFailureMessage(
+        name(), fmt::format("Failed to get required value from input data port: {}", ports.error()));
+
+    return BT::NodeStatus::FAILURE;
+  }
+  const auto& [start_pose_stamped, end_pose_stamped] = ports.value();
+
+  geometry_msgs::msg::TransformStamped transform_end;
+  geometry_msgs::msg::TransformStamped transform_start;
+  try
+  {
+    transform_start = shared_resources_->transform_buffer_ptr->lookupTransform(
+        "world", start_pose_stamped.header.frame_id, tf2::TimePointZero);
+    transform_end = shared_resources_->transform_buffer_ptr->lookupTransform(
+        "world", start_pose_stamped.header.frame_id, tf2::TimePointZero);
+  }
+  catch (tf2::TransformException& ex)
+  {
+    shared_resources_->logger->publishFailureMessage(name(), ex.what());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  geometry_msgs::msg::PoseStamped pose_start;
+  geometry_msgs::msg::PoseStamped pose_end;
+  tf2::doTransform(start_pose_stamped, pose_start, transform_start);
+  tf2::doTransform(end_pose_stamped, pose_end, transform_end);
+
+  visualization_msgs::msg::Marker msg;
+  msg.pose = pose_start.pose;
+  msg.header.frame_id = "world";
+  msg.header.stamp = shared_resources_->node->get_clock()->now();
+  msg.ns = "arrow_marker";
+  msg.id = 0;
+  msg.type = visualization_msgs::msg::Marker::ARROW;
+  msg.action = visualization_msgs::msg::Marker::ADD;
+
+  // Define the start and end points of the arrow
+  geometry_msgs::msg::Point start;
+  start.x = 0;
+  start.y = 0;
+  start.z = 0;
+
+  Eigen::Isometry3d start_tform = tf2::poseToEigen(pose_start);
+  Eigen::Isometry3d end_tform = tf2::poseToEigen(pose_end);
+  end_tform = start_tform.inverse() * end_tform;
+  geometry_msgs::msg::Point end;
+  end.x = end_tform.translation().x();
+  end.y = end_tform.translation().y();
+  end.z = end_tform.translation().z();
+
+  msg.points.push_back(start);
+  msg.points.push_back(end);
+
+  // Set the scale of the arrow
+  msg.scale.x = 0.01;  // Shaft diameter
+  msg.scale.y = 0.04;  // Head diameter
+  msg.scale.z = 0.02;  // Head length (only if non-zero)
+
+  // Set the color (RGBA)
+  msg.color.r = 1.0f;
+  msg.color.g = 0.0f;
+  msg.color.b = 0.0f;
+  msg.color.a = 1.0f;  // Ensure alpha is non-zero
+
+  visualization_msgs::msg::MarkerArray msg_array;
+  msg_array.markers = { msg };
+
+  marker_publisher_->publish(msg_array);
+
+  return BT::NodeStatus::SUCCESS;
+}
+}  // namespace example_behaviors