重复spin问题

ROS2 重复spin问题

报错描述：
在执行回调函数时，报错 terminate called after throwing an instance of 'std::runtime_error'what(): Node '/workflow_control_node' has already been added to an executor.[ros2run]: Aborted ;

原因

在回调函数中又执行了 rclcpp::spin 监听函数；

举例说明

#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <cinttypes>
#include <memory>

using namespace std::chrono_literals;
using AddTwoInts = example_interfaces::srv::AddTwoInts;

class Client {
public:
  Client(std::shared_ptr<rclcpp::Node> node) : node_(node) {
    client_ = node_->create_client<AddTwoInts>("add_two_ints");
    timer_ =
        node_->create_wall_timer(1s, std::bind(&Client::timer_callback, this));
  }
  ~Client() {}

private:
  std::shared_ptr<rclcpp::Node> node_ = nullptr;
  rclcpp::TimerBase::SharedPtr timer_ = nullptr;
  ::rclcpp::Client<AddTwoInts>::SharedPtr client_ = nullptr;

  void timer_callback() {
    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(node_->get_logger(),
                     "client interrupted while waiting for service to appear.")
        return;
      }
      RCLCPP_INFO(node_->get_logger(), "waiting for service to appear...")
    }
    auto request = std::make_shared<AddTwoInts::Request>();
    request->a = 41;
    request->b = 1;
    auto result_future = client_->async_send_request(request);
    if (rclcpp::spin_until_future_complete(node_, result_future) !=
        rclcpp::executor::FutureReturnCode::SUCCESS) {
      RCLCPP_ERROR(node_->get_logger(), "service call failed :(")
      return;
    }
    auto result = result_future.get();
    RCLCPP_INFO(node_->get_logger(),
                "result of %" PRId64 " + %" PRId64 " = %" PRId64, request->a,
                request->b, result->sum);
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("minimal_client");
  Client client(node);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

在 main() 函数中，已经通过 rclcpp::spin(node) 调用了 spin() 函数；然后在类的回调函数 timer_callback 中再一次通过 spin_until_future_complete 使当前节点去监听回调函数，所以会报错；

我们可以查看 spin_until_future_complete 函数，会发现在一层一层嵌套调用的背后，有这样一个函数：

/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
/**
 * \param[in] executor The executor which will spin the node.
 * \param[in] node_ptr The node to spin.
 * \param[in] future The future to wait on. If `SUCCESS`, the future is safe to
 *   access after this function
 * \param[in] timeout Optional timeout parameter, which gets passed to
 *   Executor::spin_node_once.
 *   `-1` is block forever, `0` is non-blocking.
 *   If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code.
 * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
 */
template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
rclcpp::FutureReturnCode
spin_node_until_future_complete(
  rclcpp::Executor & executor,
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
  const FutureT & future,
  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
  // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
  // inside a callback executed by an executor.
  executor.add_node(node_ptr);
  auto retcode = executor.spin_until_future_complete(future, timeout);
  executor.remove_node(node_ptr);
  return retcode;
}

所以，ROS2 中的节点回调都是先将当前节点加入到 rclcpp::executors::SingleThreadedExecutor executor 中，等待 future 成功获得后，再就将当前节点从 executor 中删除掉，才可以进入下一个回调函数；如果当前节点还在 executor 中，就会报出上述错误；

解决方法

在回调函数中，不要使用 spin() 相关的监听功能；如果是 service 通信的话，可以使用绑定回调函数的方法，避免使用 spin_until_future_complete 函数；

例如将上述代码修改为：

#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <cinttypes>
#include <memory>

using namespace std::chrono_literals;
using namespace std::placeholders;
using AddTwoInts = example_interfaces::srv::AddTwoInts;

class Client {
public:
  Client(std::shared_ptr<rclcpp::Node> node) : node_(node) {
    client_ = node_->create_client<AddTwoInts>("add_two_ints");
    timer_ =
        node_->create_wall_timer(1s, std::bind(&Client::timer_callback, this));
  }
  ~Client() {}

  void client_callback(const rclcpp::Client<AddTwoInts>::SharedFuture future){
    const auto& response = future.get();
    RCLCPP_INFO(node_->get_logger(),"result is " PRId64, result->sum);
    return;
  }

private:
  std::shared_ptr<rclcpp::Node> node_ = nullptr;
  rclcpp::TimerBase::SharedPtr timer_ = nullptr;
  ::rclcpp::Client<AddTwoInts>::SharedPtr client_ = nullptr;

  void timer_callback() {
    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(node_->get_logger(),
                     "client interrupted while waiting for service to appear.")
        return;
      }
      RCLCPP_INFO(node_->get_logger(), "waiting for service to appear...")
    }
    auto request = std::make_shared<AddTwoInts::Request>();
    request->a = 41;
    request->b = 1;
    client_->async_send_request(request, std::bind(&Client::client_callback, this, _1));
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("minimal_client");
  Client client(node);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

或者是，在创建类的时候并不是将 rclcpp::Node 作为参数传入类中，而是将类设置为从 rclcpp::Node 继承过来，便可以在回调函数中使用 spin_until_future_complete 函数；

参考链接：

• https://github.com/ros2/rclcpp/issues/206
• https://github.com/ros2/ros2/issues/605