〖ROS2 源码解析〗rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

已于 2024-09-15 02:08:25 修改
阅读量237 收藏 6
点赞数 3
分类专栏： 《ROS2系列教程》 文章标签： linux ROS2
于 2024-08-20 21:56:28 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/sinat_39783664/article/details/141369460
版权

目录

一. 简要介绍

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp 文件是ROS 2 (Robot Operating System 2) 中用于处理Action服务器目标请求的核心部分。这个文件定义了一个基类 ServerGoalHandleBase 和一个模板类 ServerGoalHandle，它们共同构成了处理Action服务器目标请求的基础。

1. 文件概览

ServerGoalHandleBase 类
  • 作用：ServerGoalHandleBase 是一个基类，它提供了与Action服务器目标交互的基本功能。它不直接被用户使用，而是作为 ServerGoalHandle 的基础。
  • 成员函数：
    • is_canceling()：检查客户端是否已请求取消此目标。
    • is_active()：检查目标是否处于待处理或执行状态。
    • is_executing()：检查目标是否正在执行。
    • _abort(), _succeed(), _cancel_goal(), _canceled(), _execute(): 这些是内部使用的函数，用于改变目标的状态。
    • try_canceling()：尝试将目标转换为取消状态，如果目标尚未达到终端状态。
ServerGoalHandle 类
  • 作用：ServerGoalHandle 是一个模板类，它扩展了 ServerGoalHandleBase，并提供与特定类型的Action目标交互的功能。
  • 成员函数：
    • publish_feedback(): 发布关于目标进度的反馈消息。
    • abort(): 表明目标无法达成并已中止。
    • succeed(): 表明目标已成功完成。
    • canceled(): 表明目标已被取消。
    • execute(): 指示服务器开始执行目标。
    • get_goal(): 获取描述目标的消息。
    • get_goal_id(): 获取目标的唯一标识符。
  • 构造函数：
    • 接受一个rcl_action_goal_handle_t指针、目标的GoalUUID、目标的消息、回调函数等参数。
  • 析构函数：
    • 如果目标未达到终端状态，则尝试将其取消。

2. 使用场景

  • 目的：ServerGoalHandle 类允许Action服务器接收、处理和响应客户端发送的目标请求。
  • 用户交互：当服务器接受一个目标时，它会创建一个 ServerGoalHandle 实例，并通过用户的回调函数传递给用户。用户可以使用这个实例来监控目标的状态，发布反馈消息，以及通知系统目标是否已成功、中止或取消。
// Copyright 2018 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.

#ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_

#include <rcl_action/types.h>
#include <rcl_action/goal_handle.h>

#include <action_msgs/msg/goal_status.hpp>

#include <functional>
#include <memory>
#include <mutex>

#include "rclcpp_action/visibility_control.hpp"
#include "rclcpp_action/types.hpp"

namespace rclcpp_action
{

/// Base class to interact with goals on a server.
/// \internal
/**
 *
 * This class in not be used directly by users writing an action server.
 * Instead users will be given an instance of `rclcpp_action::ServerGoalHandle<>`.
 *
 * Internally, this class is responsible for interfacing with the `rcl_action` API.
 */
class ServerGoalHandleBase
{
public:
  /// Indicate if client has requested this goal be cancelled.
  /// \return true if a cancelation request has been accepted for this goal.
  RCLCPP_ACTION_PUBLIC
  bool
  is_canceling() const;

  /// Indicate if goal is pending or executing.
  /// \return false if goal has reached a terminal state.
  RCLCPP_ACTION_PUBLIC
  bool
  is_active() const;

  /// Indicate if goal is executing.
  /// \return true only if the goal is in an executing state.
  RCLCPP_ACTION_PUBLIC
  bool
  is_executing() const;

  RCLCPP_ACTION_PUBLIC
  virtual
  ~ServerGoalHandleBase();

protected:
  // -------------------------------------------------------------------------
  // API for communication between ServerGoalHandleBase and ServerGoalHandle<>

  /// \internal
  RCLCPP_ACTION_PUBLIC
  ServerGoalHandleBase(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle
  )
  : rcl_handle_(rcl_handle)
  {
  }

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  _abort();

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  _succeed();

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  _cancel_goal();

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  _canceled();

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  _execute();

  /// Transition the goal to canceled state if it never reached a terminal state.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  bool
  try_canceling() noexcept;

  // End API for communication between ServerGoalHandleBase and ServerGoalHandle<>
  // -----------------------------------------------------------------------------

private:
  std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;
  mutable std::mutex rcl_handle_mutex_;
};

// Forward declare server
template<typename ActionT>
class Server;

/// Class to interact with goals on a server.
/**
 * Use this class to check the status of a goal as well as set the result.
 *
 * This class is not meant to be created by a user, instead it is created when a goal has been
 * accepted.
 * A `Server` will create an instance and give it to the user in their `handle_accepted` callback.
 *
 * Internally, this class is responsible for coverting between the C++ action type and generic
 * types for `rclcpp_action::ServerGoalHandleBase`.
 */
template<typename ActionT>
class ServerGoalHandle : public ServerGoalHandleBase
{
public:
  /// Send an update about the progress of a goal.
  /**
   * This must only be called when the goal is executing.
   * If execution of a goal is deferred then `ServerGoalHandle::set_executing()` must be called
   * first.
   *
   * \throws std::runtime_error If the goal is in any state besides executing.
   *
   * \param[in] feedback_msg the message to publish to clients.
   */
  void
  publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
  {
    auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
    feedback_message->goal_id.uuid = uuid_;
    feedback_message->feedback = *feedback_msg;
    publish_feedback_(feedback_message);
  }

  /// Indicate that a goal could not be reached and has been aborted.
  /**
   * Only call this if the goal was executing but cannot be completed.
   * This is a terminal state, no more methods should be called on a goal handle after this is
   * called.
   *
   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
   *
   * \param[in] result_msg the final result to send to clients.
   */
  void
  abort(typename ActionT::Result::SharedPtr result_msg)
  {
    _abort();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }

  /// Indicate that a goal has succeeded.
  /**
   * Only call this if the goal is executing and has reached the desired final state.
   * This is a terminal state, no more methods should be called on a goal handle after this is
   * called.
   *
   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
   *
   * \param[in] result_msg the final result to send to clients.
   */
  void
  succeed(typename ActionT::Result::SharedPtr result_msg)
  {
    _succeed();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }

  /// Indicate that a goal has been canceled.
  /**
   * Only call this if the goal is executing or pending, but has been canceled.
   * This is a terminal state, no more methods should be called on a goal handle after this is
   * called.
   *
   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
   *
   * \param[in] result_msg the final result to send to clients.
   */
  void
  canceled(typename ActionT::Result::SharedPtr result_msg)
  {
    _canceled();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }

  /// Indicate that the server is starting to execute a goal.
  /**
   * Only call this if the goal is pending.
   *
   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
   */
  void
  execute()
  {
    _execute();
    on_executing_(uuid_);
  }

  /// Get the user provided message describing the goal.
  const std::shared_ptr<const typename ActionT::Goal>
  get_goal() const
  {
    return goal_;
  }

  /// Get the unique identifier of the goal
  const GoalUUID &
  get_goal_id() const
  {
    return uuid_;
  }

  virtual ~ServerGoalHandle()
  {
    // Cancel goal if handle was allowed to destruct without reaching a terminal state
    if (try_canceling()) {
      auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
      null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
      on_terminal_state_(uuid_, null_result);
    }
  }

protected:
  /// \internal
  ServerGoalHandle(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
    GoalUUID uuid,
    std::shared_ptr<const typename ActionT::Goal> goal,
    std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
    std::function<void(const GoalUUID &)> on_executing,
    std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback
  )
  : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
    on_terminal_state_(on_terminal_state), on_executing_(on_executing),
    publish_feedback_(publish_feedback)
  {
  }

  /// The user provided message describing the goal.
  const std::shared_ptr<const typename ActionT::Goal> goal_;

  /// A unique id for the goal request.
  const GoalUUID uuid_;

  friend Server<ActionT>;

  std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;
  std::function<void(const GoalUUID &)> on_executing_;
  std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;
};
}  // namespace rclcpp_action

#endif  // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_

以下是对ServerGoalHandleBase作用的进一步举例解析：

一、在动作服务器中的位置

想象一个 ROS 2 的动作服务器系统就像一个繁忙的工厂车间。在这个车间里，ServerGoalHandleBase扮演着车间主管的角色。它虽然不是直接在生产线上操作的工人（即不是直接被用户使用），但却负责协调和管理整个生产流程（与 rcl_action API 进行交互以管理动作服务器上的目标）。

二、处理目标状态

  1. 目标取消判断：

    • 方法is_canceling就像是一个传感器，不断检测是否有客户请求取消某个特定的目标。例如，在一个机器人导航的动作服务器中，当用户通过控制台发送取消当前导航任务的指令时，is_canceling方法可以快速判断这个导航目标是否正在被取消。
    • 如果这个方法返回true，就意味着车间里的某个特定生产任务（目标）被要求停止。这时候，系统可以采取相应的措施，比如停止分配资源给这个任务。

  2. 目标活跃状态判断：

    • is_active方法如同一个指示灯，告诉我们目标是否处于待处理或正在执行的状态。如果返回false，那就表示这个目标已经完成或者被取消等，处于终端状态，不再需要关注。
    • 继续以机器人导航为例，当导航任务开始后，这个方法会一直返回true，表示导航目标处于活跃状态。一旦导航完成或者被取消，它就会返回false。

  3. 目标执行状态判断：

    • is_executing方法则更加具体，专门判断目标是否正在执行。在机器人导航中，当机器人真正在移动朝着目标位置前进时，这个方法会返回true，否则返回false。

三、目标状态转换方法

  1. 取消目标：

    • _cancel_goal方法就像是按下了工厂车间里某个任务的紧急停止按钮。当调用这个方法时，系统会尝试取消一个特定的目标。
    • 例如，如果在导航过程中遇到了不可预见的障碍，动作服务器可以调用这个方法来取消当前的导航目标。

  2. 目标成功与失败处理：

    • _succeed和_abort方法分别用于标记目标的成功和失败状态。在工厂车间里，这就相当于完成了一个高质量的产品（成功）或者因为某些问题不得不废弃一个产品（失败）。
    • 对于机器人导航，如果机器人成功到达了目标位置，动作服务器可以调用_succeed方法来标记这个导航目标的成功完成。如果在导航过程中出现了严重错误无法继续，就可以调用_abort方法。

  3. 目标被取消后的处理：

    • _canceled方法在目标被取消后被调用，用于进行一些清理工作或者通知其他相关部分目标已被取消。
    • 比如在导航被取消后，可能需要释放一些资源或者通知用户界面显示导航已被取消。

  4. 开始执行目标：

    • _execute方法启动一个目标的执行。在工厂车间里，这就像是启动了一条新的生产线来处理一个特定的任务。
    • 对于机器人导航，当动作服务器决定开始处理一个导航目标时，就可以调用这个方法来启动导航过程。

四、资源管理与安全

  1. 资源管理：

    • rcl_handle_成员变量是与底层rcl_action API 进行交互的关键。它就像一个与工厂车间的控制系统进行通信的接口，通过它可以获取和更新目标的状态信息、发送指令等。
    • 例如，通过这个接口，动作服务器可以向底层系统查询当前有哪些目标正在处理中，以及它们的状态如何。

  2. 线程安全：

    • rcl_handle_mutex_互斥锁确保了对rcl_handle_的安全访问。在多线程环境下，就像工厂车间里有多个工人同时操作，这个互斥锁可以防止不同的线程同时对目标状态进行修改，从而避免数据混乱和不一致性。
    • 例如，当一个线程正在查询目标状态时，另一个线程不能同时修改这个目标的状态，必须等待查询完成后才能进行修改。

综上所述，ServerGoalHandleBase在 ROS 2 的动作服务器中起着至关重要的作用，它通过与底层 API 的交互，管理和协调目标的状态，确保动作服务器的稳定运行和正确响应。

二. 代码解析

1、ServerGoalHandleBase

/// Base class to interact with goals on a server.
/// \internal
/**
 *
 * This class is not to be used directly by users writing an action server.
 * Instead, users will be given an instance of `rclcpp_action::ServerGoalHandle<>`.
 *
 * Internally, this class is responsible for interfacing with the `rcl_action` API.
 */

ServerGoalHandleBase 是一个基类，专门用来管理和处理服务器上与目标（Goal）相关的操作。在 ROS 2 的动作服务器中，它扮演着一个中间管理者的角色，负责与底层的 rcl_action API 进行交互。

设计目的
  • 封装和抽象：这个类不直接供用户使用，而是被封装在用户可访问的 ServerGoalHandle<> 中，以提供一个简洁的接口。
  • 与 rcl_action API 的交互：它管理了与底层 API 的通信和状态管理，这意味着它承担了大量低级别的细节操作，确保用户不必关心这些复杂性。
举例

当你在实现一个机器人导航的动作服务器时，ServerGoalHandleBase 将帮助你管理各个目标的状态、处理取消请求以及决定目标是否成功或失败，而你只需使用更高层的 ServerGoalHandle<> 来与这些目标交互。

2、is_canceling

  /// Indicate if the client has requested this goal be cancelled.
  /// \return true if a cancelation request has been accepted for this goal.
  RCLCPP_ACTION_PUBLIC
  bool
  is_canceling() const;

is_canceling 方法用于判断当前目标是否接收到了取消请求。这是通过检查目标的状态来实现的，返回值为 true 表示目标已经被请求取消。

举例

在机器人执行一个任务的过程中，用户可能发送一个取消命令。此时，is_canceling 方法将用于检查该目标是否已经进入取消状态，进而决定是否继续执行或者终止任务。

3、is_active

  /// Indicate if the goal is pending or executing.
  /// \return false if the goal has reached a terminal state.
  RCLCPP_ACTION_PUBLIC
  bool
  is_active() const;

is_active 方法用于检查目标是否处于活动状态，包括待处理和执行中的状态。如果目标已经达到了终端状态（如完成、取消或失败），则返回 false。

举例

在一个持续的任务中，is_active 可以用来判断任务是否仍在进行，以决定是否继续分配资源或执行后续操作。

4、is_executing

  /// Indicate if the goal is executing.
  /// \return true only if the goal is in an executing state.
  RCLCPP_ACTION_PUBLIC
  bool
  is_executing() const;

is_executing 方法判断目标是否正在执行。它返回 true 表示目标已经进入执行阶段，否则返回 false。

举例

当机器人开始移动以达到某个目标位置时，is_executing 可以帮助你确认目标是否已开始执行，并根据情况触发相关的监控或反馈机制。

5、~ServerGoalHandleBase

  RCLCPP_ACTION_PUBLIC
  virtual
  ~ServerGoalHandleBase();

这是 ServerGoalHandleBase 类的析构函数，用于清理资源和执行对象销毁时所需的操作。它是一个虚函数，允许派生类进行自定义清理操作。

6、ServerGoalHandleBase

  RCLCPP_ACTION_PUBLIC
  ServerGoalHandleBase(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle
  )
  : rcl_handle_(rcl_handle)
  {
  }

这是 ServerGoalHandleBase 的构造函数，用于初始化目标句柄 rcl_handle_，它是与 rcl_action API 进行交互的核心对象。

举例

当一个新的目标被创建时，ServerGoalHandleBase 将持有一个指向该目标的句柄，以便在后续的执行和状态管理过程中使用。

7、_abort

  RCLCPP_ACTION_PUBLIC
  void
  _abort();

_abort 方法用于标记目标为失败状态。这是一个终端状态，意味着目标不能再进一步处理。

8、_succeed

  RCLCPP_ACTION_PUBLIC
  void
  _succeed();

_succeed 方法用于标记目标为成功状态。这是一个终端状态，表示目标已成功完成。

9、_cancel_goal

  RCLCPP_ACTION_PUBLIC
  void
  _cancel_goal();

_cancel_goal 方法用于处理目标的取消请求，标记目标进入取消状态。

10、_canceled

  RCLCPP_ACTION_PUBLIC
  void
  _canceled();

_canceled 方法在目标被取消后调用，完成取消过程并进行必要的清理操作。

11、_execute

  RCLCPP_ACTION_PUBLIC
  void
  _execute();

_execute 方法用于启动目标的执行。它将目标状态转换为执行中，并开始实际的任务执行过程。

12、_succeed

（已在第8条中解释）

13、try_canceling

  RCLCPP_ACTION_PUBLIC
  bool
  try_canceling() noexcept;

try_canceling 方法尝试取消一个目标。如果目标未达到终端状态，则将其标记为取消。

14、rcl_handle_

  std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;

rcl_handle_ 是一个指向 rcl_action API 目标句柄的共享指针，负责管理目标状态和与底层 API 的交互。

15、rcl_handle_mutex_

  mutable std::mutex rcl_handle_mutex_;

rcl_handle_mutex_ 是一个互斥锁，用于保护 rcl_handle_ 以确保多线程环境下的线程安全。

16、ServerGoalHandle

/// Class to interact with goals on a server.
/**
 * Use this class to check the status of a goal as well as set the result.
 *
 * This class is not meant to be created by a user, instead it is created when a goal has been
 * accepted.
 * A `Server` will create an instance and give it to the user in their `handle_accepted` callback.
 *
 * Internally, this class is responsible for converting between the C++ action type and generic
 * types for `rclcpp_action::ServerGoalHandleBase`.
 */
template<typename ActionT>
class ServerGoalHandle : public ServerGoalHandleBase

这段注释描述了 ServerGoalHandle 类的用途和设计意图。下面是对这段注释和代码的详细解析：

注释解析

/// Class to interact with goals on a server.
/**
 * Use this class to check the status of a goal as well as set the result.
 *
 * This class is not meant to be created by a user, instead it is created when a goal has been
 * accepted.
 * A `Server` will create an instance and give it to the user in their `handle_accepted` callback.
 *
 * Internally, this class is responsible for converting between the C++ action type and generic
 * types for `rclcpp_action::ServerGoalHandleBase`.
 */

  1. Class Purpose:

    • ServerGoalHandle 类用于与服务器上的目标（goals）进行交互。
    • 用户可以使用这个类来检查目标的状态以及设置结果。

  2. Creation and Usage:

    • 这个类不是由用户直接创建的，而是当目标被接受时由服务器创建的。
    • 服务器（Server）会在用户的 handle_accepted 回调函数中创建一个 ServerGoalHandle 实例并将其传递给用户。

  3. Internal Responsibilities:

    • 内部地，这个类负责在 C++ 动作类型和 rclcpp_action::ServerGoalHandleBase 的通用类型之间进行转换。
    • 这意味着 ServerGoalHandle 类需要处理特定动作类型的细节，而 ServerGoalHandleBase 类则是更通用的基类。

代码解析

template<typename ActionT>
class ServerGoalHandle : public ServerGoalHandleBase

  1. Template Definition:

    • ServerGoalHandle 是一个模板类，它接受一个类型参数 ActionT，代表具体的动作类型。
    • ActionT 应该是继承自 rclcpp_action::Action 的具体动作类型的实例。

  2. Inheritance:

    • ServerGoalHandle 继承自 ServerGoalHandleBase 类。
    • 这意味着 ServerGoalHandle 可以重写或扩展 ServerGoalHandleBase 中定义的行为，同时保留其基本功能。

  3. Purpose:

    • ServerGoalHandle 类的目的是提供一个特定于动作类型的接口，以便用户可以与特定动作的目标进行交互。
    • 它封装了与特定动作类型相关的逻辑，例如检查目标的状态和设置结果等。

总结

ServerGoalHandle 类是一个模板类，用于与服务器上的特定动作目标进行交互。它继承自 ServerGoalHandleBase 类，并负责处理特定动作类型的细节。用户不直接创建 ServerGoalHandle 实例，而是由服务器在目标被接受时创建并传递给用户。这个类的内部职责包括在 C++ 动作类型和 rclcpp_action::ServerGoalHandleBase 的通用类型之间进行转换。

17、publish_feedback

  /// Send an update about the progress of a goal.
  /**
   * This must only be called when the goal is executing.
   * If execution of a goal is deferred then `ServerGoalHandle::set_executing()` must be called
   * first.
   *
   * \throws std::runtime_error If the goal is in any state besides executing.
   *
   * \param[in] feedback_msg the message to publish to clients.
   */
  void
  publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
  {
    auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
    feedback_message->goal_id.uuid = uuid_;
    feedback_message->feedback = *feedback_msg;
    publish_feedback_(feedback_message);
  }
  std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;

这段代码是 ServerGoalHandle 类中的一个成员函数 publish_feedback() 的实现，以及一个私有成员变量 publish_feedback_ 的声明。下面是对这段代码的详细解析：

函数签名

void ServerGoalHandle<ActionT>::publish_feedback(
  std::shared_ptr<typename ActionT::Feedback> feedback_msg)
  • 返回类型: void - 这个函数没有返回值。
  • 函数名: publish_feedback - 发布目标的反馈信息。
  • 参数:
    • feedback_msg: std::shared_ptr<typename ActionT::Feedback> 类型的参数，表示要发布的反馈消息。

函数体

auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
feedback_message->goal_id.uuid = uuid_;
feedback_message->feedback = *feedback_msg;
publish_feedback_(feedback_message);

  1. 创建反馈消息:

    • auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
      • 创建一个 typename ActionT::Impl::FeedbackMessage 类型的智能指针 feedback_message。这里使用了 typename 关键字来明确指出 ActionT::Impl::FeedbackMessage 是一个类型。
      • typename ActionT::Impl::FeedbackMessage 表示与特定动作类型 ActionT 相关的反馈消息类型。

  2. 填充反馈消息:

    • feedback_message->goal_id.uuid = uuid_;
      • 设置反馈消息中的 goal_id 字段，使用 uuid_ 成员变量来标识目标。
      • uuid_ 是一个 rcl_uuid_t 类型的成员变量，用于唯一标识一个目标。
    • feedback_message->feedback = *feedback_msg;
      • 设置反馈消息中的 feedback 字段，使用传入的 feedback_msg 参数的值。
      • 这里使用了 *feedback_msg 来解引用智能指针，获取实际的反馈消息。

  3. 发布反馈消息:

    • publish_feedback_(feedback_message);
      • 调用 publish_feedback_ 成员函数来发布反馈消息。这里使用了下划线 _ 后缀来区分这是一个私有成员函数。

成员变量定义

private:
  /// A unique id for the goal request.
  const GoalUUID uuid_;
using GoalUUID = std::array<uint8_t, UUID_SIZE>;

  std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;

成员变量定义解析：

1. uuid_ 成员变量

const GoalUUID uuid_;
  • 类型: const GoalUUID
  • 名称: uuid_
  • 描述: uuid_ 是一个常量（const）成员变量，用于唯一标识每个动作请求（goal request）。
  • 用途: 在动作服务器中，每个动作请求都需要一个唯一标识符来区分不同的请求。uuid_ 提供了这种唯一性。

2. GoalUUID 类型定义

using GoalUUID = std::array<uint8_t, UUID_SIZE>;
  • 类型定义: GoalUUID
  • 描述: GoalUUID 是一个类型别名，定义为一个固定大小为 UUID_SIZE 的 std::array，每个元素是 uint8_t 类型。
  • 用途: 使用 std::array<uint8_t, UUID_SIZE> 来表示一个固定长度的 UUID（通用唯一识别码），通常用于唯一标识对象，如在分布式系统中唯一标识一个动作请求。UUID_SIZE 应该是一个预定义的常量，表示 UUID 的字节长度。

3. publish_feedback_ 成员变量

std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;

  • 类型: std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)>

  • 名称: publish_feedback_

  • 描述: publish_feedback_ 是一个 std::function 类型的成员变量，用于存储一个函数或可调用对象，该对象接受一个 std::shared_ptr<typename ActionT::Impl::FeedbackMessage> 参数并返回 void。

  • 用途: 此成员变量用于发布反馈消息。它的主要功能是允许动作服务器在执行动作时向客户端发送中间反馈，通知动作的进度或状态。通过使用 std::function，它可以被设置为任何符合签名的可调用对象（如函数指针、lambda 表达式等）。

    为了更好地理解 std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_ 的作用，我们先简单介绍一下 std::function 的用途。

    什么是 std::function：

    std::function 是 C++ 标准库中的一个模板类，用于存储和调用任何可调用对象，如普通函数、成员函数、函数对象（functor）或 lambda 表达式。它可以灵活地封装不同类型的可调用对象，而不必知道它们的具体类型。它通常用于需要动态更改函数的场景。

    std::function 的使用场景：

    考虑在不同的场景下，我们可能希望对同一类型的事件（如发布反馈消息）进行不同的处理。std::function 可以用来存储这种处理逻辑，并在需要时调用。

    publish_feedback_ 示例解释：

    在你的代码中，publish_feedback_ 被定义为：

    std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;

    这是一个 std::function 类型的成员变量，它能保存任何返回类型为 void，且接受一个 std::shared_ptr<typename ActionT::Impl::FeedbackMessage> 参数的函数或可调用对象。

    假设我们有一个动作服务器，在不同的状态下，它需要将反馈信息发送给不同的客户端或处理系统。我们可以通过设置 publish_feedback_ 来动态改变发布反馈消息的方式。

    示例代码：

    #include <iostream>
#include <memory>
#include <functional>

// 定义一个假设的 FeedbackMessage 类型
struct FeedbackMessage {
    int data;
};

// 动作服务器中的某个类（假设为 ActionServer）
class ActionServer {
public:
    // 定义 publish_feedback_ 成员变量，保存发布反馈消息的函数
    std::function<void(std::shared_ptr<FeedbackMessage>)> publish_feedback_;

    // 发送反馈消息的方法
    void publish_feedback(std::shared_ptr<FeedbackMessage> feedback_msg) {
        if (publish_feedback_) {  // 检查是否有设置的函数
            publish_feedback_(feedback_msg);  // 调用设置的函数
        } else {
            std::cerr << "No feedback function set!" << std::endl;
        }
    }
};

int main() {
    ActionServer server;

    // 设置 publish_feedback_ 为一个打印函数
    server.publish_feedback_ = [](std::shared_ptr<FeedbackMessage> msg) {
        std::cout << "Feedback data: " << msg->data << std::endl;
    };

    // 创建一个反馈消息
    auto feedback_msg = std::make_shared<FeedbackMessage>();
    feedback_msg->data = 42;

    // 调用发布反馈方法，这将调用之前设置的 lambda 函数
    server.publish_feedback(feedback_msg);

    // 现在改变 publish_feedback_ 为一个不同的函数
    server.publish_feedback_ = [](std::shared_ptr<FeedbackMessage> msg) {
        std::cout << "Different feedback handler. Data: " << msg->data << std::endl;
    };

    // 再次调用发布反馈方法，这次将调用新的 lambda 函数
    server.publish_feedback(feedback_msg);

    return 0;
}

    解释代码：

    1. 定义 FeedbackMessage 结构: 一个简单的结构体，用于示例，包含一个整数数据。

    2. ActionServer 类: 这是一个包含 publish_feedback_ 成员变量的类，publish_feedback_ 是一个 std::function，用于存储任何接受 std::shared_ptr<FeedbackMessage> 参数且返回 void 的函数。

    3. publish_feedback 方法: 调用 publish_feedback_ 成员变量中存储的函数。这个方法首先检查 publish_feedback_ 是否被设置，然后调用它。

    4. 主函数 (main):

      • 创建一个 ActionServer 实例 server。
      • 使用 lambda 表达式为 publish_feedback_ 设置一个简单的打印函数。
      • 创建一个 FeedbackMessage 对象，并调用 publish_feedback 方法，这将输出 “Feedback data: 42”。
      • 然后，publish_feedback_ 被设置为另一个不同的 lambda 函数。
      • 再次调用 publish_feedback，这次将输出 “Different feedback handler. Data: 42”。

注释解析

/// Send an update about the progress of a goal.
/**
 * This must only be called when the goal is executing.
 * If execution of a goal is deferred then `ServerGoalHandle::set_executing()` must be called
 * first.
 *
 * \throws std::runtime_error If the goal is in any state besides executing.
 *
 * \param[in] feedback_msg the message to publish to clients.
 */

  1. Function Description:

    • 这个函数用于发送关于目标进度的更新。
    • 它只能在目标正在执行时调用。
    • 如果目标的执行被延迟，则必须先调用 ServerGoalHandle::set_executing()。
    • 如果目标处于除了执行以外的任何状态，则会抛出 std::runtime_error 异常。

  2. Parameter Documentation:

    • feedback_msg: 输入参数，表示要发布给客户端的消息。

18、abort

  /// Indicate that a goal could not be reached and has been aborted.
  /**
   * Only call this if the goal was executing but cannot be completed.
   * This is a terminal state, no more methods should be called on a goal handle after this is
   * called.
   *
   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
   *
   * \param[in] result_msg the final result to send to clients.
   */
  void
  abort(typename ActionT::Result::SharedPtr result_msg)
  {
    _abort();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }

关联代码：

void
ServerGoalHandleBase::_abort()
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}

  std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;

翻译注释：

  /// 指示一个目标无法达到并且已经中止。
  /**
   * 只有当目标正在执行但无法完成时才调用此函数。
   * 这是一个终止状态，在调用此函数后，不应再对目标句柄调用其他方法。
   *
   * \throws rclcpp::exceptions::RCLError 如果目标处于执行以外的任何状态。
   *
   * \param[in] result_msg 要发送给客户端的最终结果。
   */

函数逐行解析：

以下是 abort 函数的逐行解析：

void
abort(typename ActionT::Result::SharedPtr result_msg)
  • 函数定义: 这是 abort 函数的定义，它接受一个共享指针 result_msg 作为参数，表示要发送给客户端的最终结果消息。该函数的目的是中止一个正在执行的目标（goal）。
{
    _abort();
  • 调用 _abort(): 这个内部函数 _abort() 用于更新目标的状态，确保其状态被标记为中止。详细的功能如下：
void
ServerGoalHandleBase::_abort()
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}
  • 锁定互斥锁: std::lock_guard<std::mutex> lock(rcl_handle_mutex_); 确保在函数执行期间不会有其他线程修改 rcl_handle_。
  • 更新目标状态: rcl_action_update_goal_state 函数被调用以更新目标状态为“中止”（GOAL_EVENT_ABORT）。
  • 错误检查: 如果状态更新失败（ret 不是 RCL_RET_OK），则抛出一个 rclcpp::exceptions::throw_from_rcl_error 异常，表示操作失败。
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
  • 创建响应对象: 使用 std::make_shared 创建一个新的 GetResultService::Response 对象。这个对象将被用来存储目标中止的状态信息以及结果。
    response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
  • 设置状态: 将响应对象的 status 字段设置为 STATUS_ABORTED，表示目标已经被中止。
    response->result = *result_msg;
  • 设置结果: 将传入的 result_msg（中止时的最终结果）解引用，并将其存储在响应对象的 result 字段中。
    on_terminal_state_(uuid_, response);
}
  • 调用终止状态处理函数: 调用 on_terminal_state_ 函数，通知系统目标已达到终止状态并传递相关数据。on_terminal_state_ 是一个 std::function 类型的成员变量，它之前已经被设置为一个特定的函数或回调，用于处理目标达到终止状态的情况。

std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_; 是一个函数对象的定义，用于存储和调用一个特定的函数或回调，该函数有两个参数：

  1. const GoalUUID &: 这是一个常量引用，指向 GoalUUID 类型的对象。GoalUUID 是一个唯一标识目标（goal）的标识符，在这里用于指示哪个目标已经达到了终止状态。

  2. std::shared_ptr<void>: 这是一个指向 void 的 std::shared_ptr，代表一个泛型的、类型不确定的共享指针。void 指针在 C++ 中表示一个“无类型”指针，可以指向任何类型的数据。使用 std::shared_ptr<void> 可以实现对任意类型的对象的共享所有权管理，而不限定对象的具体类型。

解析 std::shared_ptr<void>：

std::shared_ptr<void> 的主要作用是提供一种通用的方式来管理和传递任意类型的对象。它的使用通常与回调函数和事件系统有关，允许函数或方法在不知道具体数据类型的情况下仍能处理不同类型的数据。这种做法在需要统一处理不同类型的数据或回调时非常有用。

在这个上下文中，std::shared_ptr<void> 被用来传递与目标（goal）终止状态相关的数据，但由于这个数据类型是不确定的，使用 void 可以灵活处理任何类型的数据。

举例：

假设 on_terminal_state_ 需要处理不同的目标结果类型。使用 std::shared_ptr<void> 可以避免模板化或者多重重载，这样可以用一个统一的接口来处理所有不同类型的结果。例如：

void my_terminal_state_callback(const GoalUUID &uuid, std::shared_ptr<void> result)
{
    // 假设我们知道 result 是某种类型的 shared_ptr，例如 MyResultType
    auto specific_result = std::static_pointer_cast<MyResultType>(result);
    // 现在我们可以安全地使用 specific_result 了
    if (specific_result) {
        // 使用 specific_result 做一些操作
        std::cout << "Goal " << uuid << " has terminated with result: " << specific_result->some_member << std::endl;
    } else {
        std::cerr << "Failed to cast result to MyResultType!" << std::endl;
    }
}

在这个例子中，我们定义了一个 my_terminal_state_callback 函数，它接受一个 GoalUUID 和一个 std::shared_ptr<void> 类型的参数。我们使用 std::static_pointer_cast 将 void 类型的共享指针转换为我们期望的具体类型 MyResultType。这样做的好处是，我们在接口上保持了灵活性，可以处理任意类型的数据，同时在实现中能够处理具体类型的对象。

19、succeed

/// Indicate that a goal has succeeded.
  /**
   * Only call this if the goal is executing and has reached the desired final state.
   * This is a terminal state, no more methods should be called on a goal handle after this is
   * called.
   *
   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
   *
   * \param[in] result_msg the final result to send to clients.
   */
  void
  succeed(typename ActionT::Result::SharedPtr result_msg)
  {
    _succeed();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }

关联代码：

void
ServerGoalHandleBase::_succeed()
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SUCCEED);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}

  std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;

注释翻译：

/// 指示一个目标（goal）已经成功完成。
/**
 * 只有在目标正在执行并且已经达到了期望的最终状态时才调用此函数。
 * 这是一个终止状态，调用此函数后不应再对目标句柄调用任何其他方法。
 *
 * \throws rclcpp::exceptions::RCLError 如果目标的状态不是执行中，则抛出该异常。
 *
 * \param[in] result_msg 要发送给客户端的最终结果消息。
 */

代码逐行解析：

函数 succeed
void
succeed(typename ActionT::Result::SharedPtr result_msg)
  • 作用: 该函数用于标记一个目标（goal）已成功完成，并将最终结果发送给客户端。
  • 参数: result_msg 是一个共享指针，指向 ActionT::Result 类型的对象，表示目标成功的结果信息。
_succeed();
  • 作用: 调用内部函数 _succeed()，它更新内部状态以反映目标已经成功完成。这是一个辅助函数，负责处理底层逻辑，如线程安全性和状态更新。
auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
  • 作用: 创建一个 Response 对象的共享指针，类型为 ActionT::Impl::GetResultService::Response。这是用于响应客户端请求的消息。
response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
  • 作用: 设置 response 的状态为 STATUS_SUCCEEDED，表明目标已成功完成。这是通过 action_msgs::msg::GoalStatus::STATUS_SUCCEEDED 来指示的。
response->result = *result_msg;
  • 作用: 将 result_msg 中包含的最终结果复制到 response 的 result 成员中。这样客户端就可以接收到该目标的结果数据。
on_terminal_state_(uuid_, response);
  • 作用: 调用函数对象 on_terminal_state_，传递目标的唯一标识符（uuid_）和响应消息（response）。on_terminal_state_ 是一个回调函数，用于在目标达到终止状态（例如成功、失败、被取消）时执行相应操作。

关联代码 _succeed：

void
ServerGoalHandleBase::_succeed()
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SUCCEED);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}
  • 作用: _succeed 函数用于更新目标状态，以反映其成功完成。

  1. 线程安全操作:

    std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
    • 使用 std::lock_guard 确保在函数执行期间，目标状态的更新操作是线程安全的。rcl_handle_mutex_ 是一个互斥锁，用于保护共享资源 rcl_handle_，防止多个线程同时修改状态。

  2. 更新目标状态:

    rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SUCCEED);
    • 调用 rcl_action_update_goal_state 函数更新目标的状态为成功（GOAL_EVENT_SUCCEED）。rcl_handle_ 是一个指向低层 ROS 2 目标管理结构的指针。rcl_action_update_goal_state 函数将目标状态设置为成功。

  3. 错误处理:

    if (RCL_RET_OK != ret) {
  rclcpp::exceptions::throw_from_rcl_error(ret);
}
    • 检查状态更新的返回值 ret，如果不等于 RCL_RET_OK（表示操作成功），则抛出异常 rclcpp::exceptions::throw_from_rcl_error(ret)，通知调用者目标状态更新失败。

on_terminal_state_ 成员变量：

std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;
  • 作用: on_terminal_state_ 是一个函数对象（回调），用于处理目标到达终止状态（例如成功、失败、被取消）后的操作。
  • 参数:
    • const GoalUUID &: 目标的唯一标识符。
    • std::shared_ptr<void>: 一个泛型共享指针，指向包含终止状态信息的数据。由于使用了 void，它可以指向任意类型的数据，这使得函数更加通用。

总结：

succeed 函数用于将目标状态更新为“成功”并通知客户端结果。该函数确保在执行更新操作时的线程安全性，并通过 _succeed 函数处理底层状态管理。on_terminal_state_ 函数对象作为回调，用于处理所有目标的终止状态，提供了一种通用的机制来通知目标状态的变化。

20、canceled

  /// Indicate that a goal has been canceled.
  /**
   * Only call this if the goal is canceling.
   * This is a terminal state, no more methods should be called on a goal handle after this is
   * called.
   *
   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
   *
   * \param[in] result_msg the final result to send to clients.
   */
  void
  canceled(typename ActionT::Result::SharedPtr result_msg)
  {
    _canceled();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }

关联代码：

void
ServerGoalHandleBase::_canceled()
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}

  std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;

注释翻译

/// 表示一个目标（goal）已经被取消。
/**
 * 只有在目标处于取消状态时才调用此函数。
 * 这是一个终止状态，调用此函数后，不应再对目标句柄调用任何其他方法。
 *
 * \throws rclcpp::exceptions::RCLError 如果目标的状态不是正在执行中（executing），则抛出此异常。
 *
 * \param[in] result_msg 要发送给客户端的最终结果消息。
 */

函数 canceled 解析

void
canceled(typename ActionT::Result::SharedPtr result_msg)

  • 作用:

    • 该函数用于标记一个目标（goal）已经被取消，并将最终结果发送给客户端。
    • 一旦一个目标的状态被标记为“已取消”，就不应该再对这个目标句柄进行任何操作，因为这是一个终止状态。

  • 参数:

    • result_msg 是一个共享指针，指向 ActionT::Result 类型的对象，它包含了目标被取消时需要发送给客户端的结果信息。
_canceled();
  • 作用:
    • 调用内部函数 _canceled()，这会更新目标的内部状态以反映其已被取消。
    • 这个函数确保在多线程环境下的线程安全性，并通过锁保护更新状态操作。
    • 它还通过底层 ROS 2 API 更新目标状态为已取消（GOAL_EVENT_CANCELED）。
auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
  • 作用:
    • 创建一个 Response 对象的共享指针，类型为 ActionT::Impl::GetResultService::Response。
    • 这个响应对象将被用来发送给客户端，以通知他们目标的取消状态。
response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
  • 作用:
    • 设置 response 的状态为 STATUS_CANCELED，表明目标已经被取消。
    • 这里的 STATUS_CANCELED 是一个常量，来自于 action_msgs::msg::GoalStatus，用于表示目标的终止状态为“已取消”。
response->result = *result_msg;
  • 作用:
    • 将 result_msg 中包含的最终结果数据复制到 response 对象的 result 成员中。
    • 这样，客户端将接收到目标取消后的结果数据。
on_terminal_state_(uuid_, response);
  • 作用:
    • 调用函数对象 on_terminal_state_，传递目标的唯一标识符 uuid_ 和响应消息 response。
    • on_terminal_state_ 是一个回调函数，用于处理目标达到终止状态（如成功、失败、取消）后的操作。
    • 这一步通知系统和客户端，目标已经达到终止状态，当前操作已经完成。

关联代码 _canceled 解析

void
ServerGoalHandleBase::_canceled()
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}
  • 作用:
    • _canceled 函数用于将目标状态更新为“已取消”。
    • 它是 canceled 函数的辅助函数，处理低层次的目标状态管理。

  1. 线程安全操作:

    std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
    • 使用 std::lock_guard 来确保在函数执行期间，对共享资源 rcl_handle_ 的访问是线程安全的。
    • rcl_handle_mutex_ 是一个互斥锁，用于保护对 rcl_handle_ 的操作，防止多个线程同时修改状态。

  2. 更新目标状态:

    rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
    • 调用 rcl_action_update_goal_state 函数将目标状态设置为取消（GOAL_EVENT_CANCELED）。
    • rcl_handle_ 是一个指向低层 ROS 2 目标管理结构的指针。

  3. 错误处理:

    if (RCL_RET_OK != ret) {
  rclcpp::exceptions::throw_from_rcl_error(ret);
}
    • 检查状态更新的返回值 ret，如果返回值不是 RCL_RET_OK（表示操作成功），则抛出异常 rclcpp::exceptions::throw_from_rcl_error(ret)，通知调用者目标状态更新失败。

on_terminal_state_ 成员变量

std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;

  • 作用:

    • on_terminal_state_ 是一个函数对象（或回调），用于在目标到达终止状态（例如成功、失败、被取消）后执行相关操作。
    • 通过这个回调函数，系统可以统一处理目标终止状态的通知和相关后续操作。

  • 参数:

    • const GoalUUID &: 目标的唯一标识符（UUID），用于标识具体的目标实例。
    • std::shared_ptr<void>: 一个泛型共享指针，指向包含终止状态信息的数据。由于使用了 void 类型，指针可以指向任意类型的数据，使得函数更加通用和灵活。

总结

canceled 函数用于将目标状态更新为“已取消”并通知客户端结果。该函数确保在执行更新操作时的线程安全性，并通过 _canceled 函数处理底层状态管理。on_terminal_state_ 函数对象作为回调，用于处理所有目标的终止状态，提供了一种通用的机制来通知目标状态的变化。

21、execute

  /// Indicate that the server is starting to execute a goal.
  /**
   * Only call this if the goal is pending.
   *
   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
   */
  void
  execute()
  {
    _execute();
    on_executing_(uuid_);
  }

注释翻译

/// 表示服务器开始执行一个目标（goal）。
/**
 * 只有在目标处于等待状态（pending）时才调用此函数。
 *
 * \throws rclcpp::exceptions::RCLError 如果目标不在执行状态中（executing），则抛出此异常。
 */

execute 函数解析

void execute()

  • 作用:

    • execute 函数用于将一个目标的状态从“等待”（pending）更新为“执行中”（executing）。
    • 在执行目标的具体逻辑之前，这个状态更新是必要的，因为它通知系统目标已经从等待状态进入了执行状态。

  • 使用场景:

    • 该函数应在确定目标准备好执行并且确实处于等待状态（pending）时调用。
    • 如果目标不在等待状态，则调用此函数会导致异常抛出。

函数调用步骤解析

  1. 内部状态更新:

    _execute();
    • 调用 _execute() 函数，这个内部函数用于执行底层状态更新操作，将目标状态从“等待”更新为“执行中”。

  2. 执行状态回调:

    on_executing_(uuid_);
    • 调用 on_executing_ 回调函数，这个函数是一个用户定义的回调，用于处理目标状态转为执行状态后的逻辑。
    • uuid_ 是目标的唯一标识符，用于通知具体哪个目标进入了执行状态。

关联代码 _execute 解析

void
ServerGoalHandleBase::_execute()
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}
  • 作用:
    • _execute 函数负责将目标的状态从“等待”更新为“执行中”。
    • 这个函数是 execute 函数的辅助函数，用于处理底层的状态管理。

  1. 线程安全操作:

    std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
    • 使用 std::lock_guard 来确保在函数执行期间，对共享资源 rcl_handle_ 的访问是线程安全的。
    • 通过锁定互斥锁 rcl_handle_mutex_，防止多个线程同时修改目标状态。

  2. 更新目标状态:

    rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE);
    • 调用 rcl_action_update_goal_state 函数将目标状态更新为执行状态（GOAL_EVENT_EXECUTE）。
    • rcl_handle_ 是一个指向底层 ROS 2 目标管理结构的指针，GOAL_EVENT_EXECUTE 是一个常量，用于表示目标状态已更改为“执行中”。

  3. 错误处理:

    if (RCL_RET_OK != ret) {
  rclcpp::exceptions::throw_from_rcl_error(ret);
}
    • 检查状态更新的返回值 ret，如果返回值不是 RCL_RET_OK（表示操作成功），则抛出异常 rclcpp::exceptions::throw_from_rcl_error(ret)，通知调用者目标状态更新失败。

on_executing_ 成员变量

std::function<void(const GoalUUID &)> on_executing_;

  • 作用:

    • on_executing_ 是一个函数对象（或回调），用于在目标从等待状态转换为执行状态时执行相关操作。
    • 它提供了一种机制，让用户可以在目标开始执行时执行自定义逻辑。

  • 参数:

    • const GoalUUID &: 目标的唯一标识符（UUID），用于标识具体的目标实例。

总结

execute 函数用于将一个目标状态从“等待”更新为“执行中”，并通知系统和用户目标已经开始执行。该函数确保在执行更新操作时的线程安全性，并通过 _execute 函数处理底层状态管理。on_executing_ 函数对象作为回调，用于处理目标进入执行状态后的操作，为用户提供了一种灵活的方式来实现目标状态转变的处理逻辑。

22、get_goal

成员函数定义：

/// Get the user provided message describing the goal.
const std::shared_ptr<const typename ActionT::Goal>
get_goal() const
{
  return goal_;
}

  1. 函数签名:

    • 返回类型: const std::shared_ptr<const typename ActionT::Goal> - 返回一个指向目标消息的智能指针，目标消息是只读的。
    • 函数名: get_goal - 获取用户提供的描述目标的消息。
    • 参数: 无。
    • 函数修饰符: const - 表明这个函数不会修改 ServerGoalHandle 对象的状态。

  2. 函数体:

    • return goal_; - 返回私有成员变量 goal_，这是一个指向目标消息的智能指针。

成员变量定义：

/// The user provided message describing the goal.
const std::shared_ptr<const typename ActionT::Goal> goal_;

  1. 成员变量类型:

    • const std::shared_ptr<const typename ActionT::Goal> - 这是一个指向 typename ActionT::Goal 类型的智能指针，其中目标消息本身是只读的。
    • typename ActionT::Goal - 表示与特定动作类型 ActionT 相关的目标消息类型。

  2. 成员变量修饰符:

    • const - 表明这个成员变量是不可变的，一旦在构造函数中初始化后就不能更改。

成员变量作用：

  1. goal_ 成员变量的作用:
    • 存储用户提供的描述目标的消息。
    • 在整个目标处理过程中保持不变，用于描述目标的具体内容。
    • 有助于在后续处理中访问目标消息而不需重新获取或创建。

成员函数作用

  1. get_goal() 成员函数的作用:
    • 提供了一个接口来获取目标消息。
    • 通过返回指向目标消息的智能指针，使得其他部分的代码可以访问目标消息而无需知道具体的实现细节。

示例：

假设我们有一个名为 MyAction 的动作类型，它有一个名为 Goal 的目标消息类型。下面是具体的代码示例：

#include <memory>

// 假设这是 MyAction 的目标消息类型
template<typename T>
struct Goal {
    int target_value;
    // 其他成员变量和方法
};

// 动作类型
template<typename T>
class MyAction {
public:
    using GoalType = Goal<T>;

    // 构造函数
    MyAction() {
        // 初始化 goal_ 成员变量
        std::shared_ptr<const GoalType> goal(new GoalType());
        goal->target_value = 42;
        goal_ = goal;
    }

    // 获取目标消息的函数
    const std::shared_ptr<const GoalType>
    get_goal() const
    {
        return goal_;
    }

private:
    const std::shared_ptr<const GoalType> goal_;
};

int main() {
    MyAction<int> action;

    // 获取目标消息
    const std::shared_ptr<const typename MyAction<int>::GoalType> goal = action.get_goal();

    // 访问目标消息
    std::cout << "Target value: " << goal->target_value << std::endl;

    return 0;
}

解释：

  1. 定义目标消息类型:

    • 我们定义了一个名为 Goal 的模板类，它有一个 target_value 成员变量。

  2. 定义动作类型:

    • 我们定义了一个名为 MyAction 的模板类，它有一个名为 GoalType 的类型别名，指向 Goal 类型。

  3. 构造函数初始化:

    • 在构造函数中，我们初始化了 goal_ 成员变量，设置了一个指向 GoalType 类型的智能指针，并为其 target_value 成员赋值。

  4. 成员函数:

    • get_goal() 成员函数返回 goal_ 成员变量。

  5. 主函数:

    • 在 main 函数中，我们创建了一个 MyAction 类的实例，并通过调用 get_goal() 函数来获取目标消息，然后访问目标消息中的 target_value 成员。

总结：

这段代码定义了一个 ServerGoalHandle 类中的成员函数 get_goal() 和一个私有成员变量 goal_。goal_ 存储了用户提供的描述目标的消息，而 get_goal() 函数提供了一个接口来获取这个消息。这种方式确保了目标消息的一致性和可访问性。

23、get_goal_id

成员函数定义

/// Get the unique identifier of the goal
const GoalUUID &
get_goal_id() const
{
  return uuid_;
}

  1. 函数签名:

    • 返回类型: const GoalUUID & - 返回一个指向目标唯一标识符的引用，目标唯一标识符是只读的。
    • 函数名: get_goal_id - 获取目标的唯一标识符。
    • 参数: 无。
    • 函数修饰符: const - 表明这个函数不会修改 ServerGoalHandle 对象的状态。

  2. 函数体:

    • return uuid_; - 返回私有成员变量 uuid_，这是一个 GoalUUID 类型的引用。

成员变量定义

/// A unique id for the goal request.
const GoalUUID uuid_;
using GoalUUID = std::array<uint8_t, UUID_SIZE>;

  1. 成员变量类型:

    • const GoalUUID - 这是一个 GoalUUID 类型的常量成员变量，用于存储目标请求的唯一标识符。
    • GoalUUID - 表示目标请求的唯一标识符类型，它是一个固定大小为 UUID_SIZE 的 uint8_t 类型数组。

  2. 成员变量修饰符:

    • const - 表明这个成员变量是不可变的，一旦在构造函数中初始化后就不能更改。

  3. 类型定义:

    • using GoalUUID = std::array<uint8_t, UUID_SIZE>; - 这是一个类型别名定义，定义了一个名为 GoalUUID 的类型，它是一个固定大小的数组，用于存储目标的唯一标识符。
    • UUID_SIZE - 应该是一个预定义的常量，表示 UUID 的大小。在 ROS 2 中，UUID_SIZE 通常是 16 字节。

成员变量作用

  1. uuid_ 成员变量的作用:
    • 存储目标请求的唯一标识符。
    • 在整个目标处理过程中保持不变，用于唯一标识一个目标请求。
    • 有助于跟踪和管理不同的目标请求。

成员函数作用

  1. get_goal_id() 成员函数的作用:
    • 提供了一个接口来获取目标的唯一标识符。
    • 通过返回指向目标唯一标识符的引用，使得其他部分的代码可以访问目标唯一标识符而不需重新获取或创建。

示例

假设 UUID_SIZE 的值为 16，我们可以这样定义 GoalUUID 类型：

constexpr std::size_t UUID_SIZE = 16; // 假设 UUID 的大小为 16 字节

using GoalUUID = std::array<uint8_t, UUID_SIZE>;

下面是具体的代码示例：

#include <array>

// 假设 UUID_SIZE 的值为 16
constexpr std::size_t UUID_SIZE = 16;

using GoalUUID = std::array<uint8_t, UUID_SIZE>;

// 动作类型
template<typename T>
class MyAction {
public:
    using GoalUUIDType = GoalUUID;

    // 构造函数
    MyAction() {
        // 初始化 uuid_ 成员变量
        std::array<uint8_t, UUID_SIZE> uuid;
        for (std::size_t i = 0; i < UUID_SIZE; ++i) {
            uuid[i] = static_cast<uint8_t>(i); // 仅用于示例，实际应用中应该使用随机生成的 UUID
        }
        uuid_ = uuid;
    }

    // 获取目标唯一标识符的函数
    const GoalUUIDType &
    get_goal_id() const
    {
        return uuid_;
    }

private:
    const GoalUUIDType uuid_;
};

int main() {
    MyAction<int> action;

    // 获取目标唯一标识符
    const GoalUUIDType &uuid = action.get_goal_id();

    // 输出 UUID 的内容
    for (const auto& byte : uuid) {
        std::cout << static_cast<int>(byte) << " ";
    }

    return 0;
}

解释

  1. 定义目标唯一标识符类型:

    • 我们定义了一个名为 GoalUUID 的类型别名，它是一个包含 16 个 uint8_t 类型元素的数组。

  2. 定义动作类型:

    • 我们定义了一个名为 MyAction 的模板类，它有一个名为 GoalUUIDType 的类型别名，指向 GoalUUID 类型。

  3. 构造函数初始化:

    • 在构造函数中，我们初始化了 uuid_ 成员变量，设置了一个 GoalUUIDType 类型的数组，并填充了一些数据（这里仅为示例，实际应用中应该使用随机生成的 UUID）。

  4. 成员函数:

    • get_goal_id() 成员函数返回 uuid_ 成员变量的引用。

  5. 主函数:

    • 在 main 函数中，我们创建了一个 MyAction 类的实例，并通过调用 get_goal_id() 函数来获取目标唯一标识符，然后输出 UUID 的内容。

总结

这段代码定义了一个 ServerGoalHandle 类中的成员函数 get_goal_id() 和一个私有成员变量 uuid_。uuid_ 存储了目标请求的唯一标识符，而 get_goal_id() 函数提供了一个接口来获取这个唯一标识符。这种方式确保了目标唯一标识符的一致性和可访问性。

24、~ServerGoalHandle

析构函数定义

virtual ~ServerGoalHandle()
{
  // Cancel goal if handle was allowed to destruct without reaching a terminal state
  if (try_canceling()) {
    auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
    on_terminal_state_(uuid_, null_result);
  }
}

  1. 析构函数签名:

    • virtual ~ServerGoalHandle() - 定义了一个虚析构函数，允许子类覆盖并正确清理资源。

  2. 函数体:

    • if (try_canceling()) { ... } - 检查是否可以取消当前的目标。如果可以取消，则执行后续操作。
    • auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>(); - 创建一个响应对象的智能指针，类型为 ActionT::Impl::GetResultService::Response，并将其状态设置为已取消。
    • null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; - 设置响应对象的状态为 STATUS_CANCELED，表明目标已被取消。
    • on_terminal_state_(uuid_, null_result); - 调用 on_terminal_state_ 成员变量所代表的回调函数，传递目标的唯一标识符 uuid_ 和取消响应对象 null_result。

成员变量定义

std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;

  1. 成员变量类型:

    • std::function<void(const GoalUUID &, std::shared_ptr<void>)> - 这是一个 std::function 类型的成员变量，它可以存储任何接受两个参数的可调用对象。
      • 第一个参数是 const GoalUUID &，即目标请求的唯一标识符。
      • 第二个参数是 std::shared_ptr<void>，一个指向 void 类型的智能指针，通常用于传递泛型响应对象。

  2. 成员变量用途:

    • on_terminal_state_ - 用于存储一个回调函数，在目标达到终止状态时被调用，用来通知系统目标的最终状态。

总结

当 ServerGoalHandle 对象被销毁时，如果目标还没有到达终止状态（例如完成、取消或失败），则会尝试取消该目标。如果取消成功，会创建一个结果响应对象，其状态设置为 STATUS_CANCELED，然后调用 on_terminal_state_ 回调函数来通知系统目标已被取消。

这种方式确保了即使目标在没有正常完成的情况下被销毁，也能正确地处理目标的状态变化，避免了潜在的内存泄漏和其他问题。此外，使用 std::function 可以方便地替换或指定不同的回调行为，提供了灵活性。

25、ServerGoalHandle

构造函数定义

/// \internal
ServerGoalHandle(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
    GoalUUID uuid,
    std::shared_ptr<const typename ActionT::Goal> goal,
    std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
    std::function<void(const GoalUUID &)> on_executing,
    std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback
)
: ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
  on_terminal_state_(on_terminal_state), on_executing_(on_executing),
  publish_feedback_(publish_feedback)
{
}

  1. 构造函数参数:

    • std::shared_ptr<rcl_action_goal_handle_t> rcl_handle - RCL 层面的目标句柄。
    • GoalUUID uuid - 目标请求的唯一标识符。
    • std::shared_ptr<const typename ActionT::Goal> goal - 用户提供的描述目标的消息。
    • std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state - 当目标达到终止状态时的回调函数。
    • std::function<void(const GoalUUID &)> on_executing - 当目标开始执行时的回调函数。
    • std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback - 发布反馈消息的回调函数。

  2. 构造函数初始化列表:

    • ServerGoalHandleBase(rcl_handle) - 调用基类 ServerGoalHandleBase 的构造函数，传递 RCL 层面的目标句柄。
    • goal_(goal) - 初始化 goal_ 成员变量。
    • uuid_(uuid) - 初始化 uuid_ 成员变量。
    • on_terminal_state_(on_terminal_state) - 初始化 on_terminal_state_ 成员变量。
    • on_executing_(on_executing) - 初始化 on_executing_ 成员变量。
    • publish_feedback_(publish_feedback) - 初始化 publish_feedback_ 成员变量。

成员变量定义

  1. goal_ 成员变量:

    • const std::shared_ptr<const typename ActionT::Goal> goal_; - 用户提供的描述目标的消息。

  2. uuid_ 成员变量:

    • const GoalUUID uuid_; - 目标请求的唯一标识符。

  3. on_terminal_state_ 成员变量:

    • std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_; - 当目标达到终止状态时的回调函数。

  4. on_executing_ 成员变量:

    • std::function<void(const GoalUUID &)> on_executing_; - 当目标开始执行时的回调函数。

  5. publish_feedback_ 成员变量:

    • std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_; - 发布反馈消息的回调函数。

总结

这段代码定义了 ServerGoalHandle 类的构造函数和相关成员变量。构造函数接收多个参数，包括 RCL 层面的目标句柄、目标请求的唯一标识符、目标消息、终止状态回调、开始执行回调以及发布反馈消息的回调。这些参数被用于初始化相应的成员变量，从而构建了一个 ServerGoalHandle 对象。这些成员变量和构造函数一起确保了目标请求的正确处理，包括目标的唯一标识、消息的存储、状态变化的通知以及反馈消息的发布。

26、goal_

const std::shared_ptr<const typename ActionT::Goal> goal_;

goal_ 是一个指向目标的共享指针，包含了用户定义的目标消息。它保存了客户端发送的关于目标的详细信息，通常用于在执行过程中获取目标的参数和配置。

举例

假设我们在实现一个机器人导航的动作服务器，goal_ 可能会包含导航的目标位置坐标。当目标被接收后，可以通过 goal_ 来获取这个位置信息，并引导机器人移动到该位置。

27、uuid_

const GoalUUID uuid_;

uuid_ 是目标的唯一标识符（UUID），用于唯一标识每一个目标。在系统中，每个目标都有一个唯一的 uuid_，以便在执行、取消或完成目标时能够准确地识别和处理。

举例

当有多个导航任务同时进行时，uuid_ 能帮助动作服务器区分每个导航任务。即使多个任务的目标位置相同，它们的 uuid_ 也会不同，从而确保任务处理的准确性。

28、Server

 friend class Server<ActionT>;

Server 是 ServerGoalHandle 的友元类，这意味着 Server 类可以访问 ServerGoalHandle 类的私有和保护成员。这样设计是为了使 Server 类能够直接操作 ServerGoalHandle 的内部状态，便于管理和协调目标的生命周期。

举例

在动作服务器中，Server 类可以通过访问 ServerGoalHandle 的私有成员，来更新目标的执行状态、发送反馈信息或取消目标等。这种设计提高了类之间的协作效率。

29、on_terminal_state_

std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;

on_terminal_state_ 是一个函数对象，负责在目标达到终端状态时执行特定的操作。终端状态包括目标成功完成、取消或失败。该函数接收目标的 uuid_ 和终端状态的结果数据。

举例

当导航任务完成后，on_terminal_state_ 函数会被调用，通知系统该任务已经完成，并将最终结果发送给客户端。这有助于动作服务器管理和跟踪每个目标的状态。

30、on_executing_

std::function<void(const GoalUUID &)> on_executing_;

on_executing_ 是一个函数对象，在目标进入执行状态时调用。它主要用于通知系统或其他相关组件，某个目标已经从待处理状态转变为正在执行。

举例

当机器人开始执行导航任务时，on_executing_ 函数会被调用，通知系统该任务已进入执行阶段。系统可能会根据这一通知启动相关的监控或资源分配操作。

31、publish_feedback_

std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;

publish_feedback_ 是一个函数对象，用于发送反馈信息给客户端。反馈信息通常是在目标执行过程中生成的，能够告知客户端当前的执行进度或状态。

举例

在机器人导航过程中，publish_feedback_ 函数会周期性地发送机器人当前位置的更新信息给客户端。这使得用户能够实时监控任务的进展，并根据需要做出调整。

沉浸式AI
关注
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值