Creating a Behavior to Call a Service

This tutorial shows how to create a custom Behavior that creates a service request, waits until it receives a response from the service server, and then sets the response it received as an output data port.

We will achieve this by specializing the ServiceClientBehaviorBase class for the service type of our choice.

For the purposes of this tutorial, we will target the example example_interfaces::srv::AddTwoInts service and create a AddTwoIntsServiceClient Behavior. An example service server using AddTwoInts is available as part of the ROS 2 examples.

Technical Background

From a C++ development perspective, we will create a new class that inherits from the ServiceClientBehaviorBase class and specializes it for a particular message type.

The API documentation for the ServiceClientBehaviorBase class is here.

The ServiceClientBehaviorBase class itself inherits from AsyncBehaviorBase, which has some virtual functions which must be implemented in the new class as well:

• getServiceName is used to get the name of the service when initializing the service client.
• createRequest is used to to create the service request. In this example, we will make the request from data specified from input ports.
• getResponseTimeout is an optional function used to set the timeout used when waiting for the service response. This is left as default in this example (no timeout).
• processResponse is an optional function used to process the service response after the service has finished. In this example, we will make the response available on an output port.
• getFuture must be implemented for all classes derived from AsyncBehaviorBase. It returns a shared_future class member.

The API documentation for the AsyncBehaviorBase class is here.

Implementing the AddTwoIntsServiceClient Behavior

Custom Behaviors in MoveIt Pro are built within ROS packages, so the new Behavior must be either created within a new package or added to an existing package.

This public repo contains some demo Behaviors to show an example of this.

The package containing the new Behavior must be within the workspace which is loaded by the MoveIt Pro Docker container on startup, which by default is located at ~/moveit_pro/moveit_studio_ur_ws.

Within the example_behaviors package, first create a header file at example_behaviors/include/example_behaviors/add_two_ints_service_client.hpp and add this code to that file:

#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 CallAddTwoIntsService Behavior. */
  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<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

Next, create a source file at example_behaviors/src/add_two_ints_service_client.cpp and add this code to that file:

#include <example_behaviors/add_two_ints_service_client.hpp>

// Include the template implementation for ServiceClientBehaviorBase<T>.
#include <moveit_studio_behavior_interface/impl/service_client_behavior_base_impl.hpp>

namespace example_behaviors
{
AddTwoIntsServiceClient::AddTwoIntsServiceClient(
    const std::string& name, const BT::NodeConfiguration& config,
    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
  : ServiceClientBehaviorBase<example_interfaces::srv::AddTwoInts>(name, config, shared_resources)
{
}

BT::PortsList AddTwoIntsServiceClient::providedPorts()
{
  // This node has three input ports and one output port
  return BT::PortsList({
    BT::InputPort<std::string>("service_name"),
    BT::InputPort<int>("addend1"),
    BT::InputPort<int>("addend2"),
    BT::OutputPort<int>("result"),
  });
}

BT::KeyValueVector AddTwoIntsServiceClient::metadata()
{
  return { { "subcategory", "Example" },
           { "description", "Calls a service to add two integers and makes the result available on an output port." } };
}

tl::expected<std::string, std::string> AddTwoIntsServiceClient::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<AddTwoInts::Request, std::string> AddTwoIntsServiceClient::createRequest(){
  const auto a = getInput<int>("addend1");
  const auto b = getInput<int>("addend2");
  if (const auto error = moveit_studio::behaviors::maybe_error(a, b))
  {
    return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
  }
  return example_interfaces::build<AddTwoInts::Request>().a(a.value()).b(b.value());
}

tl::expected<bool, std::string> AddTwoIntsServiceClient::processResponse(const AddTwoInts::Response& response){
  setOutput<int>("result", response.sum);
  return { true };
}
}  // namespace example_behaviors

After that, add the new source file to the example_behaviors CMake library target in CMakeLists.txt following the same pattern used by the other Behaviors. It should look something like this:

add_library(
  example_behaviors SHARED
  src/hello_world.cpp
  src/delayed_message.cpp
  src/setup_mtc_wave_hand.cpp
  src/register_behaviors.cpp
  src/add_two_ints_service_client.cpp
)

Next, add a dependency for the service definition by adding the requisite package. The AddTwoInts service definition is provided by the example_interfaces package. Add this package to THIS_PACKAGE_INCLUDES_DEPENDS along with the other dependencies.

set(
  THIS_PACKAGE_INCLUDE_DEPENDS
  moveit_studio_behavior_interface
  pluginlib
  example_interfaces
)

Also, add the example_interfaces package as a dependency in the package.xml alongside the other dependencies.

<depend>moveit_studio_behavior_interface</depend>
<depend>example_interfaces</depend>

Add a line to include your new Behavior for registration using the same pattern as the lines for the other example Behaviors.

#include <example_behaviors/hello_world.hpp>
#include <example_behaviors/delayed_message.hpp>
#include <example_behaviors/setup_mtc_wave_hand.hpp>
#include <example_behaviors/add_two_ints_service_client.hpp>

Then, add a line to register the new Behavior with the package's Behavior loader plugin, following the same pattern as the lines for the other example Behaviors.

void registerBehaviors(BT::BehaviorTreeFactory& factory,
                   const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources) override
{
  // Other Behaviors registered above
  moveit_studio::behaviors::registerBehavior<AddTwoIntsServiceClient>(factory, "AddTwoIntsServiceClient", shared_resources);

}

This service client sends a request to the add_two_ints service server. The service name, ports, request construction, and response handling can be modified for your custom service.

For the purposes of this example, if you'd like to test the service client you just created, you can use the add_two_ints service server implementation from the ROS 2 examples. This can be installed and run like this if you have ROS installed on the host and configured to use the same DDS configuration as MoveIt Pro (see Configuring DDS for MoveIt Pro):

sudo apt install ros-humble-examples-rclcpp-minimal-service
ros2 run examples_rclcpp_minimal_service service_main