Robot Operating System——Action通信机制的服务端

最新推荐文章于 2024-09-08 17:41:31 发布

breaksoftware

最新推荐文章于 2024-09-08 17:41:31 发布

阅读量773

点赞数 20

分类专栏： Robot Operating System 文章标签： c++ 机器人

本文链接：https://blog.csdn.net/breaksoftware/article/details/140769304

版权

Robot Operating System 专栏收录该内容

69 篇文章 1 订阅

订阅专栏

大纲

回调
创建服务
完整代码
总结

在《Robot Operating System——Action通信机制概念及Client端》一文中，我们介绍了Action客户端的主要流程。本文我们将介绍Action服务端的编写。

回顾下Action的构成:

目标（Goal）：这是由Action客户端（Client）发送给Action服务器（Server）的请求，指示服务器需要执行的具体任务。例如，在控制一个机器人旋转的任务中，目标可能是指定机器人需要旋转的角度。
反馈（Feedback）：在执行目标的过程中，Action服务器会定期向客户端发送反馈，以报告当前的执行状态和进度。这种反馈机制使得客户端能够实时了解任务的执行情况，并在必要时进行干预或调整。
结果（Result）：当目标执行完成后，Action服务器会向客户端发送一个结果，表明任务的最终执行状态。这个结果可以是成功完成、失败或者部分完成等状态信息。

我们看到Action客户端通过设置与上述组成对应的回调函数来实现整体功能。

但是Action的服务端则不是如此。它也由三部分组成，分别是：

处理Client发送请求的回调。用于表示是“接受”还是“拒绝”该请求。
处理Client发送“终止”请求任务的回调。Action服务端在处理任务时，是可以接收客户端发送的“终止”指令的。这个回调可以“接受”还是“拒绝”该请求。
处理执行任务的回调。这个就是主要的处理逻辑。

我们通过action_tutorials/action_tutorials_cpp/src/fibonacci_action_server.cpp看下其具体实现。

回调

接受或者拒绝请求

class FibonacciActionServer : public rclcpp::Node
{
public:
  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  ACTION_TUTORIALS_CPP_PUBLIC
  explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("fibonacci_action_server", options)
  {
    using namespace std::placeholders;

    auto handle_goal = [this](
      const rclcpp_action::GoalUUID & uuid,
      std::shared_ptr<const Fibonacci::Goal> goal)
      {
        (void)uuid;
        RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
        // The Fibonacci action uses int32 for the return of sequences, which means it can only hold
        // 2^31-1 (2147483647) before wrapping negative in two's complement. Based on empirical
        // tests, that means that an order of > 46 will cause wrapping, so we don't allow that here.
        if (goal->order > 46) {
          return rclcpp_action::GoalResponse::REJECT;
        }
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
      };

本例中，handle_goal会根据客户端传递过来的order值来判断是否“接受”或“拒绝”处理该请求。
如果拒绝就返回rclcpp_action::GoalResponse::REJECT；如果接受，可以返回rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE或者rclcpp_action::GoalResponse::ACCEPT_AND_DEFER。
ACCEPT_AND_EXECUTE表示该回调后会立即调用执行任务的回调；ACCEPT_AND_DEFER表示过会儿再调用。

执行任务的回调

    auto handle_accepted = [this](
      const std::shared_ptr<GoalHandleFibonacci> goal_handle)
      {
        // this needs to return quickly to avoid blocking the executor,
        // so we declare a lambda function to be called inside a new thread
        auto execute_in_thread = [this, goal_handle]() {return this->execute(goal_handle);};
        std::thread{execute_in_thread}.detach();
      };

由于我们处理任务的回调执行时间很长（它内部要Sleep一段时间），所以为了让该回调可以立即返回，不阻碍后续流程，于是主流程会放在线程中执行。

  ACTION_TUTORIALS_CPP_LOCAL
  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    rclcpp::Rate loop_rate(1);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    auto & sequence = feedback->partial_sequence;
    sequence.push_back(0);
    sequence.push_back(1);
    auto result = std::make_shared<Fibonacci::Result>();

我们先构建Action客户端2个回调函数需要的参数：

feedback用于告知客户端FeedbackCallback回调结果。
result用于告知客户端ResultCallback回调结果。

    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
      // Check if there is a cancel request
      if (goal_handle->is_canceling()) {
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        return;
      }
      // Update sequence
      sequence.push_back(sequence[i] + sequence[i - 1]);
      // Publish feedback
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Publish feedback");

      loop_rate.sleep();
    }

然后For循环中会不停计算斐波那契数列。在计算过程中，通过GoalHandleFibonacci::is_canceling检查是否服务端接受了客户端“终止”任务的请求。如果终止，则会通过GoalHandleFibonacci::canceled返回Result。这样客户端的ResultCallback回调会收到结果，且状态是rclcpp_action::ResultCode::CANCELED。
在这里插入图片描述
如果没有终止，就会通过GoalHandleFibonacci::publish_feedback来将中间过程返回给客户端。这样客户端的FeedbackCallback回调会收到响应。

如果任务执行结束，就调用GoalHandleFibonacci::succeed来返回最终结果。

    // Check if goal is done
    if (rclcpp::ok()) {
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
    }
  }

终止任务回调

    auto handle_cancel = [this](
      const std::shared_ptr<GoalHandleFibonacci> goal_handle)
      {
        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
      };

本例中，我们直接接受了客户端的“终止”请求，返回了rclcpp_action::CancelResponse::ACCEPT。现实中，我们还可以通过rclcpp_action::CancelResponse::REJECT来拒绝“终止”请求，让任务继续进行。

如果这个回调我们返回了rclcpp_action::CancelResponse::ACCEPT，则GoalHandleFibonacci::is_canceling会返回true。这样主处理流程会做相应的动作来“终止”任务。

创建服务

最后我们只要将上述3个回调传递给rclcpp_action::create_server来构造对象即可。

    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
      this,
      "fibonacci",
      handle_goal,
      handle_cancel,
      handle_accepted);
  }

private:
  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

完整代码

// Copyright 2019 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.

#include <memory>

#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

#include "action_tutorials_cpp/visibility_control.h"

namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  ACTION_TUTORIALS_CPP_PUBLIC
  explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("fibonacci_action_server", options)
  {
    using namespace std::placeholders;

    auto handle_goal = [this](
      const rclcpp_action::GoalUUID & uuid,
      std::shared_ptr<const Fibonacci::Goal> goal)
      {
        (void)uuid;
        RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
        // The Fibonacci action uses int32 for the return of sequences, which means it can only hold
        // 2^31-1 (2147483647) before wrapping negative in two's complement. Based on empirical
        // tests, that means that an order of > 46 will cause wrapping, so we don't allow that here.
        if (goal->order > 46) {
          return rclcpp_action::GoalResponse::REJECT;
        }
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
      };

    auto handle_cancel = [this](
      const std::shared_ptr<GoalHandleFibonacci> goal_handle)
      {
        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
      };

    auto handle_accepted = [this](
      const std::shared_ptr<GoalHandleFibonacci> goal_handle)
      {
        // this needs to return quickly to avoid blocking the executor,
        // so we declare a lambda function to be called inside a new thread
        auto execute_in_thread = [this, goal_handle]() {return this->execute(goal_handle);};
        std::thread{execute_in_thread}.detach();
      };

    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
      this,
      "fibonacci",
      handle_goal,
      handle_cancel,
      handle_accepted);
  }

private:
  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

  ACTION_TUTORIALS_CPP_LOCAL
  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    rclcpp::Rate loop_rate(1);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    auto & sequence = feedback->partial_sequence;
    sequence.push_back(0);
    sequence.push_back(1);
    auto result = std::make_shared<Fibonacci::Result>();

    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
      // Check if there is a cancel request
      if (goal_handle->is_canceling()) {
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        return;
      }
      // Update sequence
      sequence.push_back(sequence[i] + sequence[i - 1]);
      // Publish feedback
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Publish feedback");

      loop_rate.sleep();
    }

    // Check if goal is done
    if (rclcpp::ok()) {
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
    }
  }
};  // class FibonacciActionServer

}  // namespace action_tutorials_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)

总结

Action服务端主要由3个回调构成：
- 是否接受执行任务请求的handle_goal；
- 是否接受终止任务请求的handle_cancel；
- handle_goal中返回rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE或者rclcpp_action::GoalResponse::ACCEPT_AND_DEFER后执行的handle_accepted。
Action客户端主要由3个回调构成：
- 请求Action服务端handle_goal回调返回结果的goal_response_callback。用于知晓服务端是接受了还是拒绝了该任务请求。
- 用于接收Action服务端在handle_accepted中通过调用GoalHandleFibonacci::publish_feedback回传过程的 feedback_callback。
- 用于接收接收Action服务端在handle_accepted中通过调用GoalHandleFibonacci::canceled或者GoalHandleFibonacci::succeed的result_callback。
Action服务端的handle_goal返回结果会决定了handle_accepted是否执行。
Action服务端的handle_cancel返回结果会决定GoalHandleFibonacci::is_canceling的返回结果，进而可以干涉主任务流程的处理逻辑。

breaksoftware

关注

20
点赞
踩
20

收藏

觉得还不错? 一键收藏
打赏
0
评论
Robot Operating System——Action通信机制的服务端

Action服务端主要由3个回调构成：是否接受执行任务请求的handle_goal；是否接受终止任务请求的handle_cancel；handle_goal中返回rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE或者rclcpp_action::GoalResponse::ACCEPT_AND_DEFER后执行的handle_accepted。
复制链接

扫一扫