Add training behaviors (#331)

* add examples

Co-authored-by: Sebastian Jahr <[email protected]>
Co-authored-by: Michael Wrock <[email protected]>

src/example_behaviors/call_my_service_solution/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.22)
+project(call_my_service CXX)
+
+find_package(moveit_studio_common REQUIRED)
+moveit_studio_package()
+
+find_package(moveit_studio_behavior_interface REQUIRED)
+find_package(service_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface service_interface pluginlib)
+
+add_library(
+  call_my_service
+  SHARED
+  src/call_my_service.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+  call_my_service
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(call_my_service
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Install Libraries
+install(
+  TARGETS call_my_service
+  EXPORT call_my_serviceTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  add_subdirectory(test)
+  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 call_my_service_plugin_description.xml)
+
+ament_export_targets(call_my_serviceTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/example_behaviors/call_my_service_solution/behavior_plugin.yaml

@@ -0,0 +1,4 @@
+objectives:
+  behavior_loader_plugins:
+    call_my_service:
+      - "call_my_service::CallMyServiceBehaviorsLoader"

src/example_behaviors/call_my_service_solution/call_my_service_plugin_description.xml

@@ -0,0 +1,5 @@
+<library path="call_my_service">
+  <class type="call_my_service::CallMyServiceBehaviorsLoader"
+         base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase">
+  </class>
+</library>

src/example_behaviors/call_my_service_solution/config/tree_nodes_model.xml

@@ -0,0 +1,5 @@
+<root>
+    <TreeNodesModel>
+        <!-- Include additional SubTree metadata in this file. -->
+    </TreeNodesModel>
+</root>

src/example_behaviors/call_my_service_solution/include/call_my_service/call_my_service.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <moveit_studio_behavior_interface/service_client_behavior_base.hpp>
+#include <service_interface/srv/service_interface.hpp>
+
+using moveit_studio::behaviors::BehaviorContext;
+using moveit_studio::behaviors::ServiceClientBehaviorBase;
+using ServiceInterface = service_interface::srv::ServiceInterface;
+
+namespace call_my_service
+{
+class CallMyService final : public ServiceClientBehaviorBase<ServiceInterface>
+{
+public:
+  CallMyService(const std::string& name, const BT::NodeConfiguration& config,
+            const std::shared_ptr<BehaviorContext>& shared_resources);
+
+  static BT::KeyValueVector metadata();
+
+  static BT::PortsList providedPorts();
+
+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. */
+  tl::expected<ServiceInterface::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 ServiceInterface::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 call_my_service

src/example_behaviors/call_my_service_solution/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>call_my_service</name>
+  <version>6.0.0</version>
+  <description>Example of a Behavior that calls a ROS 2 Service</description>
+
+  <maintainer email="[email protected]">MoveIt Pro Maintainer</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>moveit_studio_common</build_depend>
+  <build_depend>service_interface</build_depend>
+
+  <depend>moveit_studio_behavior_interface</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/example_behaviors/call_my_service_solution/src/call_my_service.cpp

@@ -0,0 +1,48 @@
+#include <call_my_service/call_my_service.hpp>
+
+// Include the template implementation for ServiceClientBehaviorBase<T>.
+#include <moveit_studio_behavior_interface/impl/service_client_behavior_base_impl.hpp>
+
+namespace call_my_service
+{
+CallMyService::CallMyService(
+    const std::string& name, const BT::NodeConfiguration& config,
+    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
+  : ServiceClientBehaviorBase<service_interface::srv::ServiceInterface>(name, config, shared_resources)
+{
+}
+
+BT::PortsList CallMyService::providedPorts()
+{
+  // This node has three input ports and one output port
+  return BT::PortsList({
+    BT::InputPort<std::string>("service_name"),
+    BT::OutputPort<bool>("result"),
+  });
+}
+
+BT::KeyValueVector CallMyService::metadata()
+{
+  return { { "subcategory", "Example" },
+           { "description", "Example of calling a ROS2 service." } };
+}
+
+tl::expected<std::string, std::string> CallMyService::getServiceName()
+{
+  const auto service_name = getInput<std::string>("service_name");
+  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());
+  }
+  return service_name.value();
+}
+
+tl::expected<ServiceInterface::Request, std::string> CallMyService::createRequest(){
+  return service_interface::build<ServiceInterface::Request>();
+}
+
+tl::expected<bool, std::string> CallMyService::processResponse(const ServiceInterface::Response& response){
+  setOutput<bool>("result", response.success);
+  return { response.success };
+}
+}  // namespace call_my_service

src/example_behaviors/call_my_service_solution/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 <call_my_service/call_my_service.hpp>
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace call_my_service
+{
+class CallMyServiceBehaviorsLoader : 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<CallMyService>(factory, "CallMyService", shared_resources);
+
+  }
+};
+}  // namespace call_my_service
+
+PLUGINLIB_EXPORT_CLASS(call_my_service::CallMyServiceBehaviorsLoader,
+                       moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

src/example_behaviors/call_my_service_solution/test/CMakeLists.txt

@@ -0,0 +1,4 @@
+find_package(ament_cmake_gtest REQUIRED)
+
+ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp)
+ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS})

src/example_behaviors/call_my_service_solution/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("call_my_service::CallMyServiceBehaviorsLoader");
+    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", "CallMyService", BT::NodeConfiguration()));
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/example_behaviors/service_example/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>service_example</name>
+  <version>6.0.0</version>
+  <description>A basic example exposing a ROS 2 Service</description>
+
+  <maintainer email="[email protected]">MoveIt Pro Maintainer</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <depend>rclpy</depend>
+  <depend>service_interface</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/example_behaviors/service_example/service_example/service.py

@@ -0,0 +1,32 @@
+from service_interface.srv import ServiceInterface
+
+import rclpy
+from rclpy.node import Node
+
+
+class ExampleService(Node):
+    def __init__(self):
+        super().__init__("example_service")
+        self.srv = self.create_service(
+            ServiceInterface, "example_service", self.callback
+        )
+
+    def callback(self, request, response):
+        response.success = True
+        self.get_logger().info("ExampleService called and returned true")
+
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    example_service = ExampleService()
+
+    rclpy.spin(example_service)
+
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/example_behaviors/service_example/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/service_example
+[install]
+install_scripts=$base/lib/service_example

src/example_behaviors/service_example/setup.py

@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = "service_example"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="marioprats",
+    maintainer_email="[email protected]",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "service = service_example.service:main",
+        ],
+    },
+)

src/example_behaviors/service_example/test/test_copyright.py

@@ -0,0 +1,27 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"

src/example_behaviors/service_example/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)