Add reset behavior for UR (#320)

src/picknik_ur_site_config/CMakeLists.txt

@@ -2,6 +2,11 @@ cmake_minimum_required(VERSION 3.22)
 project(picknik_ur_site_config)
 
 find_package(ament_cmake REQUIRED)
+find_package(moveit_studio_common REQUIRED)
+moveit_studio_package()
+
+# add custom behaviors
+add_subdirectory(behaviors)
 
 install(
   DIRECTORY
@@ -20,4 +25,10 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 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 behaviors/trigger_pstop_reset_service_plugin_description.xml)
+
 ament_package()

src/picknik_ur_site_config/behaviors/CMakeLists.txt

@@ -0,0 +1,40 @@
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  moveit_studio_behavior_interface
+  pluginlib
+  std_srvs
+  )
+foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${package} REQUIRED)
+endforeach()
+
+add_library(
+        trigger_pstop_reset_service
+        SHARED
+        src/trigger_pstop_reset_service.cpp
+        src/register_behaviors.cpp)
+target_include_directories(
+        trigger_pstop_reset_service
+        PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(trigger_pstop_reset_service
+        ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+  # Add coverage flags behavior
+  target_compile_options(trigger_pstop_reset_service PRIVATE -fprofile-arcs -ftest-coverage)
+  target_link_options(trigger_pstop_reset_service PRIVATE -fprofile-arcs -ftest-coverage)
+endif()
+
+# Install Libraries
+install(
+        TARGETS trigger_pstop_reset_service
+        EXPORT trigger_pstop_reset_serviceTargets
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+        INCLUDES
+        DESTINATION include)
+
+ament_export_targets(trigger_pstop_reset_serviceTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})

src/picknik_ur_site_config/behaviors/include/trigger_pstop_reset_service/trigger_pstop_reset_service.hpp

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <moveit_studio_behavior_interface/service_client_behavior_base.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+namespace trigger_pstop_reset_service
+{
+/**
+ * @brief Resets the UR P-stop status by calling the reset service named /recover_from_protective_stop.
+ */
+using PStopService = std_srvs::srv::Trigger;
+class TriggerPStopResetService : public moveit_studio::behaviors::ServiceClientBehaviorBase<PStopService>
+{
+public:
+/**
+   * @brief Constructor for the trigger_pstop_reset_service 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 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.
+   */
+ TriggerPStopResetService(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 trigger_pstop_reset_service 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 trigger_pstop_reset_service does not use 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();
+
+private:
+ /**
+ * @brief Specify the name of the service that will be called by the behaviors
+ * @return Returns the name of the service. If not successful, returns an error message.
+ */
+ tl::expected<std::string, std::string> getServiceName() override;
+ /**
+ * @brief Sets the timeout for the service response.
+ * @details If the timeout expires before a response is received, the behavior will fail. A negative duration indicates no timeout.
+ * @return Returns the service response timeout duration.
+ */
+ tl::expected<std::chrono::duration<double>, std::string> getResponseTimeout() override;
+ /**
+ * @brief Create a service request.
+ * @return Returns a service request message. If not successful, returns an error message.
+ */
+ tl::expected<std_srvs::srv::Trigger::Request, std::string> createRequest() override;
+ /**
+ * @brief Determines if the service request succeeded or failed based on the response message.
+ * @param response Response message received from the service server.
+ * @return Returns true if the response indicates success. If not successful, returns an error message.
+ */
+ tl::expected<bool, std::string> processResponse(const std_srvs::srv::Trigger::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;
+
+ /**
+ * @brief Holds a copy of the service name specified by the input port.
+ */
+ std::string service_name_;
+
+ /**
+  * @brief Holds the result of calling the service asynchronously.
+  */
+ std::shared_future<tl::expected<bool, std::string>> future_;
+
+
+};
+}  // namespace trigger_pstop_reset_service

src/picknik_ur_site_config/behaviors/src/register_behaviors.cpp

@@ -0,0 +1,24 @@
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit_studio_behavior_interface/behavior_context.hpp>
+#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
+
+#include <trigger_pstop_reset_service/trigger_pstop_reset_service.hpp>
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace trigger_pstop_reset_service
+{
+class TriggerPStopResetServiceBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase
+{
+public:
+  void registerBehaviors(BT::BehaviorTreeFactory& factory,
+    [[maybe_unused]] const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources) override
+  {
+    moveit_studio::behaviors::registerBehavior<TriggerPStopResetService>(factory, "TriggerPStopResetService", shared_resources);
+
+  }
+};
+}  // namespace trigger_pstop_reset_service
+
+PLUGINLIB_EXPORT_CLASS(trigger_pstop_reset_service::TriggerPStopResetServiceBehaviorsLoader,
+                       moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

src/picknik_ur_site_config/behaviors/src/trigger_pstop_reset_service.cpp

@@ -0,0 +1,80 @@
+#include <moveit_studio_behavior_interface/check_for_error.hpp>
+#include <trigger_pstop_reset_service/trigger_pstop_reset_service.hpp>
+
+namespace
+{
+    constexpr auto kDefaultPStopServiceName = "/recover_from_protective_stop";
+    constexpr auto kPortServiceName = "service_name";
+}// namespace
+
+namespace trigger_pstop_reset_service
+{
+    TriggerPStopResetService::TriggerPStopResetService(const std::string& name, const BT::NodeConfiguration& config,
+                                                       const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>&
+                                                       shared_resources)
+        : ServiceClientBehaviorBase<PStopService>(name, config, shared_resources)
+    {
+    }
+
+    tl::expected<std::string, std::string> TriggerPStopResetService::getServiceName()
+    {
+        // Get service name from the input port.
+        const auto service_name = getInput<std::string>(kPortServiceName);
+        // Check that the port has a value on it, if not, return an error.
+        if (const auto error = moveit_studio::behaviors::maybe_error(service_name))
+        {
+            return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
+        }
+
+        service_name_ = service_name.value();
+        return service_name_;
+    }
+
+    tl::expected<std::chrono::duration<double>, std::string> TriggerPStopResetService::getResponseTimeout()
+    {
+        // Create timeout for service call.
+        return std::chrono::duration<double>{2.0};
+    }
+
+    tl::expected<std_srvs::srv::Trigger::Request, std::string> TriggerPStopResetService::createRequest()
+    {
+        // Create request message.
+        return PStopService::Request{};
+    }
+
+    tl::expected<bool, std::string> TriggerPStopResetService::processResponse(
+        const std_srvs::srv::Trigger::Response& trigger_response)
+    {
+        // If the service response could not be processed, returns an error message, otherwise, return true to indicate success.
+        return trigger_response.success
+                   ? tl::expected<bool, std::string>(true)
+                   : tl::make_unexpected(
+                       "Failed to call P-Stop service `" + service_name_ + "`: " + trigger_response.message);
+    }
+
+    std::shared_future<tl::expected<bool, std::string>>& TriggerPStopResetService::getFuture()
+    {
+        return future_;
+    }
+
+
+    BT::PortsList TriggerPStopResetService::providedPorts()
+    {
+        return {
+            BT::InputPort<std::string>(kPortServiceName, kDefaultPStopServiceName,
+                                       "Name of the service to send a request to.")
+        };
+    }
+
+    BT::KeyValueVector TriggerPStopResetService::metadata()
+    {
+        return {
+            {
+                "description",
+                std::string(
+                    "Resets the UR P-stop status by calling the reset service named service name specified by the input port named `")
+                .append(kPortServiceName).append("`.")
+            }
+        };
+    }
+} // namespace trigger_pstop_reset_service

src/picknik_ur_site_config/behaviors/test/CMakeLists.txt

@@ -0,0 +1,6 @@
+find_package(ament_cmake_auto REQUIRED)
+find_package(ament_cmake_gmock REQUIRED)
+find_package(ament_cmake_gtest REQUIRED)
+
+ament_auto_add_gtest(test_behavior_plugins test_behavior_plugins.cpp)
+ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS})

src/picknik_ur_site_config/behaviors/test/test_behavior_plugins.cpp

@@ -0,0 +1,37 @@
+#include <gtest/gtest.h>
+
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
+#include <pluginlib/class_loader.hpp>
+#include <rclcpp/node.hpp>
+
+/**
+ * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and
+ * instantiated by the behavior tree factory.
+ */
+TEST(BehaviorTests, test_load_behavior_plugins)
+{
+  pluginlib::ClassLoader<moveit_studio::behaviors::SharedResourcesNodeLoaderBase> class_loader(
+      "moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase");
+
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  auto shared_resources = std::make_shared<moveit_studio::behaviors::BehaviorContext>(node);
+
+  BT::BehaviorTreeFactory factory;
+  {
+    auto plugin_instance = class_loader.createUniqueInstance("trigger_pstop_reset_service::TriggerPStopResetServiceBehaviorsLoader");
+    ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
+  }
+
+  // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info.
+  EXPECT_NO_THROW(
+    (void)factory.instantiateTreeNode("test_behavior_name", "TriggerPStopResetService", BT::NodeConfiguration()));
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/picknik_ur_site_config/behaviors/trigger_pstop_reset_service_plugin_description.xml

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

src/picknik_ur_site_config/config/config.yaml

@@ -1,7 +1,9 @@
 based_on_package: "picknik_ur_base_config"
 
 objectives:
-  behavior_loader_plugins: {}
+  behavior_loader_plugins:
+    trigger_pstop_reset_service:
+      - "trigger_pstop_reset_service::TriggerPStopResetServiceBehaviorsLoader"
   objective_library_paths:
     # You must use a unique key for each package.
     # The picknik_ur_base_config uses "core"

src/picknik_ur_site_config/package.xml

@@ -11,6 +11,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <build_depend>moveit_studio_common</build_depend>
+
+  <depend>moveit_studio_behavior_interface</depend>
   <depend>picknik_ur_base_config</depend>
 
   <export>