重复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 函数；

参考链接：

posted @ 2024-01-19 18:21  逍遥鱼儿叹大海  阅读(409)  评论(0)    收藏  举报