〖ROS2 源码解析〗rclcpp_action/src/server_goal_handle.cpp

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

目录

一、简要介绍

这段代码展示了 ROS 2 中rclcpp_action命名空间下ServerGoalHandleBase类的成员函数实现。ServerGoalHandleBase在动作服务器中扮演着关键的角色，负责与目标交互并管理其状态。

1. 文件核心功能

析构函数
  • ServerGoalHandleBase::~ServerGoalHandleBase()：空的析构函数，在对象销毁时可能执行一些清理操作，但目前没有具体实现。
状态判断方法
  • is_canceling()：通过锁定互斥锁，获取目标的状态，并判断是否处于取消中状态。如果获取状态过程中出现错误，会抛出相应的异常。
  • is_active()：锁定互斥锁后，直接调用底层函数判断目标是否处于活跃状态。
  • is_executing()：与is_canceling()类似，获取目标状态并判断是否处于执行中状态。
状态改变方法
  • _abort()、_succeed()、_cancel_goal()、_canceled()、_execute()：这些方法分别用于改变目标的状态为中止、成功、请求取消、已取消和开始执行。每个方法都通过锁定互斥锁，调用底层的rcl_action函数来更新目标状态。如果更新过程中出现错误，会抛出异常。
尝试取消方法
  • try_canceling()：这个方法首先检查目标是否可取消，如果可取消则尝试将目标状态转换为取消中，然后再次获取状态判断是否成功取消。如果任何步骤出现错误，返回false。

2. 使用场景

在 ROS 2 的动作服务器中，ServerGoalHandleBase的这些成员函数被用于管理和操作目标的状态。例如，当需要判断目标是否正在被取消、是否活跃或是否执行中时，可以调用相应的状态判断方法。当需要改变目标的状态，如中止目标、标记目标成功或取消目标时，可以调用对应的状态改变方法。而try_canceling()方法则在需要尝试取消目标时使用，例如在对象销毁前如果目标未达到终端状态，可以尝试取消目标以避免资源泄漏。

3. 总结

这段代码实现了ServerGoalHandleBase类的关键成员函数，为 ROS 2 动作服务器提供了强大的目标状态管理功能。通过这些函数，动作服务器可以准确地判断目标的状态，并在需要时改变目标的状态，确保动作服务器的稳定运行和正确响应。

二、源码与解析

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

#include <memory>

#include "rcl_action/action_server.h"
#include "rcl_action/goal_handle.h"

#include "rclcpp_action/server_goal_handle.hpp"
#include "rclcpp/exceptions.hpp"

namespace rclcpp_action
{
ServerGoalHandleBase::~ServerGoalHandleBase()
{
}

bool
ServerGoalHandleBase::is_canceling() const
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
  }
  return GOAL_STATE_CANCELING == state;
}

bool
ServerGoalHandleBase::is_active() const
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  return rcl_action_goal_handle_is_active(rcl_handle_.get());
}

bool
ServerGoalHandleBase::is_executing() const
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
  if (RCL_RET_OK != ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
  }
  return GOAL_STATE_EXECUTING == state;
}

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);
  }
}

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);
  }
}

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

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);
  }
}

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);
  }
}

bool
ServerGoalHandleBase::try_canceling() noexcept
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret;
  // Check if the goal is cancelable
  const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get());
  if (is_cancelable) {
    // Transition to CANCELING
    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL);
    if (RCL_RET_OK != ret) {
      return false;
    }
  }

  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
  // Get the current state
  ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
  if (RCL_RET_OK != ret) {
    return false;
  }

  // If it's canceling, cancel it
  if (GOAL_STATE_CANCELING == state) {
    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
    return RCL_RET_OK == ret;
  }

  return false;
}
}  // namespace rclcpp_action

以下是对各个成员函数的进一步解析：

一、析构函数

ServerGoalHandleBase::~ServerGoalHandleBase()
{
}

这个析构函数目前为空，可能在未来的版本中会添加一些资源清理的操作。

二、状态判断方法

1. is_canceling()
bool
ServerGoalHandleBase::is_canceling() const
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
  if (RCL_RET_OK!= ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
  }
  return GOAL_STATE_CANCELING == state;
}

这段代码是 ServerGoalHandleBase 类中的一个成员函数 is_canceling() 的实现。这个函数用于检查一个目标（goal）是否被请求取消。下面是对这段代码的详细解析：

函数签名

bool ServerGoalHandleBase::is_canceling() const
  • 返回类型: bool - 如果目标处于取消状态则返回 true，否则返回 false。
  • 函数名: is_canceling - 检查目标是否被请求取消。
  • 常量成员函数: (const) - 表示这个函数不会修改类的任何成员变量，只读取它们。

函数体

std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
if (RCL_RET_OK != ret) {
  rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
}
return GOAL_STATE_CANCELING == state;

  1. Mutex Locking:

    • std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
      • 这行代码使用了 std::lock_guard 来自动锁定互斥锁 rcl_handle_mutex_。std::lock_guard 是一个 RAII（Resource Acquisition Is Initialization）风格的对象，它在构造时锁定互斥锁，在析构时自动解锁互斥锁。这样可以确保即使发生异常，互斥锁也会被正确解锁。

  2. 获取目标状态:

    • rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
      • 初始化状态变量 state 为 GOAL_STATE_UNKNOWN。这通常是为了避免使用未初始化的数据。
    • rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
      • 调用 rcl_action_goal_handle_get_status 函数来获取目标的状态，并将结果存储在 state 变量中。这里使用了智能指针 rcl_handle_ 的 .get() 方法来获取底层的 rcl_action_goal_handle_t 对象指针。

  3. 错误处理:

    • if (RCL_RET_OK != ret) { ... }
      • 检查 rcl_action_goal_handle_get_status 的返回值 ret 是否等于 RCL_RET_OK。如果不等于 RCL_RET_OK，则表示获取状态失败。
    • rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
      • 如果获取状态失败，这里会抛出一个异常。rclcpp::exceptions::throw_from_rcl_error 函数会根据 ret 的值抛出相应的异常，并附带错误信息。

  4. 返回值:

    • return GOAL_STATE_CANCELING == state;
      • 如果 state 等于 GOAL_STATE_CANCELING，则返回 true；否则返回 false。这表示如果目标正处于取消状态，则返回 true。

成员变量

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

  • std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;

    • 一个智能指针，持有 rcl_action_goal_handle_t 类型的对象。这个对象是由 ROS 2 的底层库（rcl_action）管理的目标句柄，用于与目标进行交互。

  • mutable std::mutex rcl_handle_mutex_;

    • 一个互斥锁，用于保护对 rcl_action_goal_handle_t 对象的访问。由于 rcl_action_goal_handle_t 可能会被多个线程访问，因此需要使用互斥锁来保证数据的一致性和安全性。

  1. Mutex Locking:

    • 通过使用 std::lock_guard，函数在进入时自动锁定互斥锁 rcl_handle_mutex_，并在函数退出时自动解锁。这确保了在多线程环境下对 rcl_action_goal_handle_t 对象的访问是线程安全的。

  2. 获取目标状态:

    • 通过调用 rcl_action_goal_handle_get_status 函数来获取目标的状态。这个函数接受一个指向 rcl_action_goal_handle_t 的指针以及一个指向 rcl_action_goal_state_t 的指针来接收状态值。

  3. 错误处理:

    • 如果 rcl_action_goal_handle_get_status 函数返回非 RCL_RET_OK 的值，说明获取状态失败，此时会抛出一个异常。

  4. 返回值:

    • 根据获取到的状态值 state，判断是否等于 GOAL_STATE_CANCELING。如果是，则返回 true 表示目标已被请求取消；否则返回 false。

通过这种方式，is_canceling() 函数提供了一个安全、可靠的方法来检查目标是否被请求取消，同时确保了对底层 rcl_action_goal_handle_t 对象的线程安全访问。

附加解析1：

  template<typename _Mutex>
    class lock_guard
    {
    public:
      typedef _Mutex mutex_type;

      explicit lock_guard(mutex_type& __m) : _M_device(__m)
      { _M_device.lock(); }

      lock_guard(mutex_type& __m, adopt_lock_t) noexcept : _M_device(__m)
      { } // calling thread owns mutex

      ~lock_guard()
      { _M_device.unlock(); }

      lock_guard(const lock_guard&) = delete;
      lock_guard& operator=(const lock_guard&) = delete;

    private:
      mutex_type&  _M_device;
    };

这段代码展示了C++标准库中的 std::lock_guard 类模板的一个简化版本。std::lock_guard 是一个用于管理互斥锁（mutex）的类模板，它实现了RAII（Resource Acquisition Is Initialization）模式，即资源的获取是在构造函数中进行的，而资源的释放是在析构函数中进行的。这有助于确保即使在异常情况下，锁也能被正确地释放，从而避免死锁。

类模板定义

template<typename _Mutex>
class lock_guard
{
public:
  typedef _Mutex mutex_type;
  • template<typename _Mutex>: lock_guard 是一个模板类，它接受一个类型参数 _Mutex，该类型通常是一个互斥锁类型，如 std::mutex。
  • typedef _Mutex mutex_type;: 定义了一个类型别名 mutex_type，它是指向模板参数 _Mutex 的类型别名。

构造函数

  explicit lock_guard(mutex_type& __m) : _M_device(__m)
  { _M_device.lock(); }
  • explicit lock_guard(mutex_type& __m) : _M_device(__m): 这是一个构造函数，它接受一个 mutex_type 的引用作为参数。explicit 关键字防止了隐式转换。构造函数初始化 lock_guard 类的 _M_device 成员变量，该成员变量是一个互斥锁的引用。
  • { _M_device.lock(); }: 构造函数体内的代码块中，调用了 _M_device 的 lock() 方法来获取互斥锁。

构造函数（采用锁）

  lock_guard(mutex_type& __m, adopt_lock_t) noexcept : _M_device(__m)
  { } // calling thread owns mutex
  • lock_guard(mutex_type& __m, adopt_lock_t) noexcept : _M_device(__m): 这个构造函数接受一个 mutex_type 的引用和一个 adopt_lock_t 标记作为参数。adopt_lock_t 是一个类型标签，用来表示当前线程已经拥有互斥锁的所有权，因此不需要在构造函数中再次获取锁。
  • noexcept: 表示构造函数不会抛出异常。
  • { }: 构造函数体为空，因为互斥锁已经在其他地方被锁定，所以不需要在这里再次锁定。

析构函数

  ~lock_guard()
  { _M_device.unlock(); }
  • ~lock_guard(): 析构函数负责释放互斥锁。当 lock_guard 实例超出作用域时，析构函数将被调用，从而调用 _M_device 的 unlock() 方法来释放互斥锁。

私有成员变量

private:
  mutex_type&  _M_device;
  • mutex_type& _M_device;: 这是一个私有成员变量，它是一个对 mutex_type 的引用。这个引用保持了对互斥锁的访问。

移除拷贝构造函数和赋值运算符

  lock_guard(const lock_guard&) = delete;
  lock_guard& operator=(const lock_guard&) = delete;
  • lock_guard(const lock_guard&) = delete;: 删除拷贝构造函数，意味着不能创建 lock_guard 的拷贝。这是因为拷贝构造函数会导致互斥锁的所有权变得模糊不清，可能导致程序错误。
  • lock_guard& operator=(const lock_guard&) = delete;: 删除赋值运算符，意味着不能将一个 lock_guard 对象赋值给另一个。同样，这也为了避免所有权问题。

附加解析2：

  explicit lock_guard(mutex_type& __m) : _M_device(__m)
  { _M_device.lock(); }
  
  std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;
  mutable std::mutex rcl_handle_mutex_;
  
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);

这段代码涉及到了 std::lock_guard 和 std::mutex 的使用。下面是针对这段代码的详细解析：

std::lock_guard 类模板的构造函数

explicit lock_guard(mutex_type& __m) : _M_device(__m)
{ _M_device.lock(); }

  1. explicit 关键字:

    • explicit 关键字用于构造函数或转换运算符，它禁止隐式类型转换。这意味着你不能使用 lock_guard 的构造函数来进行隐式的类型转换。例如，你不能将一个 std::mutex 直接传递给一个期望 lock_guard 的函数，必须显式地创建 lock_guard 实例。

  2. 构造函数:

    • lock_guard 的构造函数接受一个 mutex_type 类型的引用 __m。这里的 mutex_type 是 std::lock_guard 模板的一个类型参数，通常会被指定为 std::mutex 或者其他互斥锁类型。
    • lock_guard 的构造函数初始化了一个名为 _M_device 的成员变量，该成员变量是一个对 __m 的引用。
    • 构造函数体内调用 _M_device.lock() 来锁定互斥锁。这是通过 std::lock_guard 的 RAII（Resource Acquisition Is Initialization）机制来完成的，确保互斥锁在 lock_guard 实例创建时被锁定。

std::mutex 和 std::lock_guard 的使用

mutable std::mutex rcl_handle_mutex_;
std::lock_guard<std::mutex> lock(rcl_handle_mutex_);

  1. std::mutex:

    • rcl_handle_mutex_ 是一个 std::mutex 类型的私有成员变量。它用于保护对 rcl_action_goal_handle_t 对象的访问，确保在多线程环境下对这个对象的访问是线程安全的。
    • mutable 关键字允许在 const 成员函数中修改这个成员变量。这是因为 std::lock_guard 需要锁定和解锁互斥锁，这通常涉及到修改互斥锁的状态。在 const 成员函数中使用 mutable 允许我们在不改变对象状态的情况下锁定和解锁互斥锁。

  2. std::lock_guard 的实例化:

    • std::lock_guard<std::mutex> lock(rcl_handle_mutex_); 创建了一个 std::lock_guard 实例，名为 lock，它接受 rcl_handle_mutex_ 作为参数。
    • 当 lock 实例被创建时，rcl_handle_mutex_ 会被锁定。这确保了在 lock 实例的作用域内，rcl_handle_mutex_ 是被锁定的，不允许其他线程同时访问。
    • 当 lock 实例超出作用域时，它的析构函数会被自动调用，从而解锁 rcl_handle_mutex_。

综合解析

在 ServerGoalHandleBase::is_canceling() 函数中，std::lock_guard 的使用如下：

  1. 保护代码块:

    • std::lock_guard<std::mutex> lock(rcl_handle_mutex_); 保护了整个函数体内的代码块。这意味着在函数体执行期间，rcl_handle_mutex_ 是被锁定的，确保了对 rcl_action_goal_handle_t 对象的线程安全访问。
    • 即使函数体中抛出了异常，std::lock_guard 的析构函数也会确保 rcl_handle_mutex_ 被解锁。

  2. 线程安全:

    • 通过使用 std::lock_guard，我们无需显式地调用 lock() 和 unlock() 方法来管理互斥锁。这有助于避免死锁和其他并发问题。
    • std::lock_guard 的使用还简化了代码，使得锁的管理更加自动化和可靠。

总结来说，std::lock_guard 和 std::mutex 的组合使用提供了线程安全的互斥锁管理机制，简化了多线程编程，并提高了代码的安全性和可维护性。

2. is_active()
bool
ServerGoalHandleBase::is_active() const
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  return rcl_action_goal_handle_is_active(rcl_handle_.get());
}

这段代码是 ServerGoalHandleBase 类中的一个成员函数 is_active() 的实现。这个函数用于检查一个目标（goal）是否处于活动状态。下面是对这段代码的详细解析：

函数签名

bool ServerGoalHandleBase::is_active() const
  • 返回类型: bool - 如果目标处于活动状态则返回 true，否则返回 false。
  • 函数名: is_active - 检查目标是否处于活动状态。
  • 常量成员函数: (const) - 表示这个函数不会修改类的任何成员变量，只读取它们。

函数体

std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
return rcl_action_goal_handle_is_active(rcl_handle_.get());

  1. Mutex Locking:

    • std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
      • 这行代码使用了 std::lock_guard 来自动锁定互斥锁 rcl_handle_mutex_。std::lock_guard 是一个 RAII（Resource Acquisition Is Initialization）风格的对象，它在构造时锁定互斥锁，在析构时自动解锁互斥锁。这样可以确保即使发生异常，互斥锁也会被正确解锁。

  2. 检查目标状态:

    • return rcl_action_goal_handle_is_active(rcl_handle_.get());
      • 调用 rcl_action_goal_handle_is_active 函数来检查目标是否处于活动状态。这里使用了智能指针 rcl_handle_ 的 .get() 方法来获取底层的 rcl_action_goal_handle_t 对象指针。
      • 这个函数返回一个布尔值，指示目标是否处于活动状态。

成员变量

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

  • std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;

    • 一个智能指针，持有 rcl_action_goal_handle_t 类型的对象。这个对象是由 ROS 2 的底层库（rcl_action）管理的目标句柄，用于与目标进行交互。

  • mutable std::mutex rcl_handle_mutex_;

    • 一个互斥锁，用于保护对 rcl_action_goal_handle_t 对象的访问。由于 rcl_action_goal_handle_t 可能会被多个线程访问，因此需要使用互斥锁来保证数据的一致性和安全性。

总结

这段代码的主要功能是安全地检查目标是否处于活动状态。通过使用 std::lock_guard 和 std::mutex 来确保线程安全，具体解析如下：

  1. Mutex Locking:

    • 通过使用 std::lock_guard，函数在进入时自动锁定互斥锁 rcl_handle_mutex_，并在函数退出时自动解锁。这确保了在多线程环境下对 rcl_action_goal_handle_t 对象的访问是线程安全的。

  2. 检查目标状态:

    • 通过调用 rcl_action_goal_handle_is_active 函数来检查目标是否处于活动状态。这个函数接受一个指向 rcl_action_goal_handle_t 的指针，并返回一个布尔值来指示目标的状态。

  3. 返回值:

    • 根据 rcl_action_goal_handle_is_active 函数的返回值直接返回 true 或 false。如果目标处于活动状态，则返回 true；否则返回 false。

通过这种方式，is_active() 函数提供了一个安全、可靠的方法来检查目标是否处于活动状态，同时确保了对底层 rcl_action_goal_handle_t 对象的线程安全访问。

3. is_executing()
bool
ServerGoalHandleBase::is_executing() const
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
  if (RCL_RET_OK!= ret) {
    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
  }
  return GOAL_STATE_EXECUTING == state;
}

与is_canceling()类似，获取目标状态并判断是否处于执行中状态。

三、状态改变方法

1. _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);
  }
}

这段代码是 ServerGoalHandleBase 类中的一个成员函数 _abort() 的实现。这个函数用于向目标（goal）发送一个终止（abort）事件。下面是对这段代码的详细解析：

函数签名

void ServerGoalHandleBase::_abort()
  • 返回类型: void - 这个函数没有返回值。
  • 函数名: _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);
}

  1. Mutex Locking:

    • std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
      • 这行代码使用了 std::lock_guard 来自动锁定互斥锁 rcl_handle_mutex_。std::lock_guard 是一个 RAII（Resource Acquisition Is Initialization）风格的对象，它在构造时锁定互斥锁，在析构时自动解锁互斥锁。这样可以确保即使发生异常，互斥锁也会被正确解锁。

  2. 更新目标状态:

    • rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT);
      • 调用 rcl_action_update_goal_state 函数来向目标发送一个终止事件。这里使用了智能指针 rcl_handle_ 的 .get() 方法来获取底层的 rcl_action_goal_handle_t 对象指针。
      • 函数接受两个参数：目标句柄的指针和一个事件类型，这里是 GOAL_EVENT_ABORT，表示终止事件。
      • 函数返回一个 rcl_ret_t 类型的值，表示操作的结果。

  3. 错误处理:

    • if (RCL_RET_OK != ret) { ... }
      • 检查 rcl_action_update_goal_state 的返回值 ret 是否等于 RCL_RET_OK。如果不等于 RCL_RET_OK，则表示操作失败。
    • rclcpp::exceptions::throw_from_rcl_error(ret);
      • 如果操作失败，这里会抛出一个异常。rclcpp::exceptions::throw_from_rcl_error 函数会根据 ret 的值抛出相应的异常。
2. _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);
  }
}

将目标状态更新为成功状态。

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

请求取消目标。

4. _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);
  }
}

将目标状态更新为已取消状态。

5. _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);
  }
}

启动目标的执行。

四、尝试取消方法

bool
ServerGoalHandleBase::try_canceling() noexcept
{
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret;
  // Check if the goal is cancelable
  const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get());
  if (is_cancelable) {
    // Transition to CANCELING
    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL);
    if (RCL_RET_OK!= ret) {
      return false;
    }
  }

  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
  // Get the current state
  ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
  if (RCL_RET_OK!= ret) {
    return false;
  }

  // If it's canceling, cancel it
  if (GOAL_STATE_CANCELING == state) {
    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
    return RCL_RET_OK == ret;
  }

  return false;
}

这段代码是 ServerGoalHandleBase 类中的一个成员函数 try_canceling() 的实现。这个函数尝试取消一个目标（goal），如果目标是可以取消的，则将其状态设置为取消中（canceling），并且如果目标已经是取消中状态，则尝试将其设置为已取消（canceled）。下面是对这段代码的详细解析：

函数签名

bool ServerGoalHandleBase::try_canceling() noexcept
  • 返回类型: bool - 如果成功尝试取消目标，则返回 true；否则返回 false。
  • 函数名: try_canceling - 尝试取消目标。
  • 无参数 - 这个函数不接受任何参数。
  • noexcept - 表明这个函数不会抛出异常。

函数体

std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
rcl_ret_t ret;
// Check if the goal is cancelable
const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get());
if (is_cancelable) {
  // Transition to CANCELING
  ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL);
  if (RCL_RET_OK != ret) {
    return false;
  }
}

rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
// Get the current state
ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
if (RCL_RET_OK != ret) {
  return false;
}

// If it's canceling, cancel it
if (GOAL_STATE_CANCELING == state) {
  ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
  return RCL_RET_OK == ret;
}

return false;

  1. Mutex Locking:

    • std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
      • 这行代码使用了 std::lock_guard 来自动锁定互斥锁 rcl_handle_mutex_。std::lock_guard 是一个 RAII（Resource Acquisition Is Initialization）风格的对象，它在构造时锁定互斥锁，在析构时自动解锁互斥锁。这样可以确保即使发生异常，互斥锁也会被正确解锁。

  2. 检查目标是否可取消:

    • const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get());
      • 调用 rcl_action_goal_handle_is_cancelable 函数来检查目标是否处于可取消状态。这里使用了智能指针 rcl_handle_ 的 .get() 方法来获取底层的 rcl_action_goal_handle_t 对象指针。
      • 函数返回一个布尔值，指示目标是否可取消。

  3. 尝试设置为取消中状态:

    • 如果目标是可取消的，则尝试将其状态设置为取消中。
    • ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL);
      • 调用 rcl_action_update_goal_state 函数来向目标发送一个取消事件。这里使用了智能指针 rcl_handle_ 的 .get() 方法来获取底层的 rcl_action_goal_handle_t 对象指针。
      • 函数接受两个参数：目标句柄的指针和一个事件类型 GOAL_EVENT_CANCEL_GOAL。
      • 函数返回一个 rcl_ret_t 类型的值，表示操作的结果。
    • 如果设置失败，则返回 false。

  4. 获取当前状态:

    • rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
      • 初始化状态变量 state 为 GOAL_STATE_UNKNOWN。
    • ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
      • 调用 rcl_action_goal_handle_get_status 函数来获取目标的状态，并将结果存储在 state 变量中。这里使用了智能指针 rcl_handle_ 的 .get() 方法来获取底层的 rcl_action_goal_handle_t 对象指针。
      • 函数返回一个 rcl_ret_t 类型的值，表示操作的结果。
    • 如果获取状态失败，则返回 false。

  5. 尝试设置为已取消状态:

    • 如果目标当前状态为取消中，则尝试将其设置为已取消。
    • ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
      • 调用 rcl_action_update_goal_state 函数来向目标发送一个已取消事件。这里使用了智能指针 rcl_handle_ 的 .get() 方法来获取底层的 rcl_action_goal_handle_t 对象指针。
      • 函数接受两个参数：目标句柄的指针和一个事件类型 GOAL_EVENT_CANCELED。
      • 函数返回一个 rcl_ret_t 类型的值，表示操作的结果。
    • 如果设置成功，则返回 true；否则返回 false。

  6. 返回值:

    • 如果目标不是可取消状态，或者在尝试设置状态时遇到错误，则返回 false。
沉浸式AI
关注
专栏目录
ROS2底层机制&源码分析(含rcl and dds)
John_ToStr的博客
06-12 362
ROS2底层机制&源码分析
ROS之rviz显示（visualization_msgs/Marker）
BigDavid123的博客
10-31 9606
（1）在ROS中，odombase_link和map是常见的坐标系（frame）用于机器人定位和导航。odom坐标系是机器人的里程计坐标系，相对于起始位置的运动。base_link坐标系是机器人的本体坐标系，与机器人的移动无关。map坐标系是机器人所在的全局地图坐标系，提供了固定的参考框架用于定位和导航机器人在全局环境中的位置。（2）是ROS中的消息类型，用于在RViz中发布多个Marker的数组消息由以下字段组成：markers（）：包含多个对象的数组。
ros2 代码结构及源码解析
liu-yonggang的专栏
01-09 6955
一、ros2 软件架构 二、ros2 命令代码解析 1.ros2 action 源码解析 2.ros2 bag 源码解析 3.ros2 run 源码解析 4.ros2 launch 源码解析 三、ros2 相关模块介绍 fastrtsp 源码解析
ROS 2（Robot Operating System 2）示例项目代码
weixin_47695827的博客
08-26 470
一个简单的ROS 2（Robot Operating System 2）示例项目代码，这个项目将包括两个节点：一个发布者（Publisher）节点和一个订阅者（Subscriber）节点。在这个例子中，发布者节点将周期性地发布一个字符串消息，而订阅者节点将接收这个消息并打印出来。
ros2 topic 源码解析
liu-yonggang的专栏
04-01 3165
ros2 命令加载过程 ros2 topic代码位置：ros2\src\ros2cli\ros2topic 最终运作在ros2topic文件中 setup.py 定义挂载点,加载命令的执行函数 from setuptools import find_packages from setuptools import setup package_name = 'ros2topic' setup( name=package_name, version='0.13.1', packages=f
ros程序源码分析2--service篇
antang7858的博客
08-09 210
服务器篇--客户端程序 1 int main(int argc, char *argv[]) 2 { 3 ros::init(argc, argv, "add_tow_ints_client"); //节点初始化，节点名称 4 5 if (argc != 3) 6 { 7 ROS_INFO("usage:add_two_ints...
ROS2RUN源码流程分析，解决所有ROS2RUN报找不到问题
生活不止眼前的苟且
05-30 815
RunCommand类依赖于两个重要的函数，它们分别是和。# 选择完全匹配的可执行文件# 检查PATHEXT中列出的扩展名以进行匹配（无扩展名）# 选择有已知扩展名的匹配项函数用于查找指定ROS包内的可执行文件的路径。它会检查与匹配的所有可执行文件，并根据操作系统和扩展名来选择最合适的可执行文件。# 在Windows上，Python脚本通过解释器调用try:# 子进程也会收到信号并应该关闭# 因此我们继续，直到进程完成pass= 0:# 负值 -N 表示子进程由信号 N 终止。
Ros2学习梳理_汇总（赵虚左老师）
m0_51759289的博客
05-30 1543
本篇博客对学习Ros2内容做一个记录，以便随时观看
[ROS2 知识] [Action综合] （2）C++实现action服务编写
gongdiwudu的专栏
03-12 5942
​Action知识有三部曲，Action定义、C++服务实现、客户端实现。本篇是C++实现action服务-客户，对于更基础的知识，可以参阅文章：[ROS2 知识] [Action综合-1] 定义一个action类型 ​
【ROS】ROS2编程示例：动作-C++版
郭老二
06-06 1035
ROS2 动作通信方式的C++编程示例
ROS navigation分析：move_base分析
总有错别字
06-10 4423
    move_base package可以说是整个navigation stack里最重要的部分了。它的功能是给定一个目标点，尝试产生一系列能让机器人到达这个目标点的控制指令。简单来说，就是输入是一个goal(geometry_msgs/PoseStamped)，输出是一系列command(geometry_msgs/Twist)。详细一点来说，就是在已知环境地图和机器人在地图中位置的情况下，...
开源机器人之Gazebo和ROS2入门源码包dolly-dashing.zip
09-04
开源机器人之Gazebo和ROS2入门源码包 Dolly是虚拟机器人小车，可作为Gazebo和ROS 2的实用介绍。 机器人操作系统的最新版本ROS 2提供了熟悉的工具和功能，同时扩展到新的用例。 Gazebo是一个功能强大的机器人模拟器，供工业界和学术界使用，可以计算物理，生成传感器数据并提供方便的界面。 开源软件正在降低进入门槛并加快机器人技术的进步。 世界各地的机器人专家正在利用Gazebo和ROS的应用，从人形机器人和无人机到仓库机器人和自动驾驶汽车。
ROS2:学习ROS2
03-25
ROS2 在AIIT-PKU学习LAIR的ROS2和ROS2工作区
[转]ROS2 源码解析与实践 - Node
陈显森的博客
02-23 2910
转载说明: 原文链接https://floodshao.github.io/2020/03/06/ros2-源码解析与实践-Node/ 感谢原作者分享！如有侵权，请联系我删除，谢谢！ 文章目录Node定义1.1 ROS1与ROS2的定义区别1.2 Node.hpp代码2. 相关C++11的语法：2.1 继承自std::enable_shared_from_this2.2 SharedPtr成员变量来自哪里？2.3 using关键字2.4 删除拷贝构造函数2.5 \__attribute__2.6 exp.
说实话，ROS2源码和我想象的不太一样！ROS2 源码编译安装
生活不止眼前的苟且
05-28 962
大家好，我是爱吃鱼香肉丝的小鱼，前两天吃饭点了个鱼香肉丝，发现居然没有胡萝卜，问老板说他这个是最正宗的，小鱼有点懵，不知道你吃过的鱼香肉丝里都有什么？回归正题，今天要讲的是ROS2的源码和编译安装的方法。
[转] ROS2源码编译和安装
aozhimen9773的博客
10-16 690
机器人开源操作系统软件ROS在10年后， 终于推出全新架构的ROS2，代号“ardent”。中文意思为“热心的美洲鳖”，看来ROS2要从ROS1的“海龟”变成“土鳖”系列了。与此同时，ROS2的编译系统也换成全新的ament。之前熟悉ROS的知道，ROS最初使用rosbuild，后来换成catkin，换成catkin可以理解，比较比起rosbuild在很多方面要先进很多的，比如支持外部构...
ROS源码分析--子话题-catkin
sukha的专栏
09-07 8064
catkincatkin是当前ROS的官方编译工具，每天都要接触到，因此有必要单独说说。
AMCL源码阅读（ros2版本）
adayimaxiga
07-31 2029
阅读amcl_ros2版本源码 源码git地址：https://github.com/ros2/navigation.git 本人注释的git地址：https://github.com/adayimaxiga/cartographer_migrate_to_ros2/tree/master/amcl 这个amcl执行过程与ros1版本没啥区别 首先阅读amcl_node.cpp 主函数调...
〖ROS2 源码解析〗rclcpp/include/rclcpp/node.hpp
最新发布
sinat_39783664的博客
08-28 761
ROS2 源码解析:rclcpp/include/rclcpp/node.hpp
/home/ros/catkin_ctr/src/precise_land/src/traj_pub.cpp:2:10: fatal error: home/ros/catkin_ctr/src/ego-planner-swarm/src/uav_simulator/Utils/quadrotor_msgs/PositionCommand.h: 没有那个文件或目录 2 | #include <home/ros/catkin_ctr/src/ego-planner-swarm/src/uav_simulator/Utils/quadrotor_msgs/PositionCommand.h>
06-11
在头文件中使用了绝对路径`home/ros/catkin_ctr/src/ego-planner-swarm/src/uav_simulator/Utils/quadrotor_msgs/PositionCommand.h`来引用`PositionCommand.h`文件，这是不正确的做法。正确的方式是使用相对路径...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值