〖ROS2 源码解析〗rclcpp/include/rclcpp/create_timer.hpp

沉浸式AI

已于 2024-09-14 21:07:38 修改

阅读量1k

点赞数 19

分类专栏：《ROS2系列教程》文章标签： linux ROS2

于 2024-08-30 19:06:26 首次发布

本文链接：https://blog.csdn.net/sinat_39783664/article/details/141725166

版权

《ROS2系列教程》专栏收录该内容

82 篇文章 8 订阅

订阅专栏

深入解析ROS 2的定时器创建：`create_timer` 函数逐行解析

概述

在 ROS 2 中，定时器是一种常用的工具，用于在特定的时间间隔内重复执行某个任务。为了简化定时器的创建和管理，ROS 2 提供了 create_timer 和 create_wall_timer 函数，这些函数封装了定时器的创建过程，并提供了多种选项来定制定时器的行为。本文将详细解析 create_timer 函数的实现，逐行解释其工作原理，并展示如何在 ROS 2 中创建一个定时器。

源码

首先，我们来看一下 create_timer 函数的完整源代码：

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

#ifndef RCLCPP__CREATE_TIMER_HPP_
#define RCLCPP__CREATE_TIMER_HPP_

#include <chrono>
#include <exception>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/duration.hpp"
#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
#include "rclcpp/node_interfaces/get_node_clock_interface.hpp"
#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_clock_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"

namespace rclcpp
{
namespace detail
{
/// 执行安全的定时器周期转换为纳秒单位
/**
 *
 * \tparam DurationRepT
 * \tparam DurationT
 * \param period 定时器回调的执行周期。此时长必须满足 0 <= period < nanoseconds::max()
 * \return 以纳秒为单位的周期
 * \throws std::invalid_argument 如果 period 为负数或过大
 */
template<typename DurationRepT, typename DurationT>
std::chrono::nanoseconds
safe_cast_to_period_in_ns(std::chrono::duration<DurationRepT, DurationT> period)
{
  if (period < std::chrono::duration<DurationRepT, DurationT>::zero()) {
    throw std::invalid_argument{"timer period cannot be negative"};
  }

  constexpr auto maximum_safe_cast_ns =
    std::chrono::nanoseconds::max() - std::chrono::duration<DurationRepT, DurationT>(1);

  constexpr auto ns_max_as_double =
    std::chrono::duration_cast<std::chrono::duration<double, std::chrono::nanoseconds::period>>(
    maximum_safe_cast_ns);
  if (period > ns_max_as_double) {
    throw std::invalid_argument{
            "timer period must be less than std::chrono::nanoseconds::max()"};
  }

  const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(period);
  if (period_ns < std::chrono::nanoseconds::zero()) {
    throw std::runtime_error{
            "Casting timer period to nanoseconds resulted in integer overflow."};
  }

  return period_ns;
}
}  // namespace detail

/// 使用给定时钟创建定时器
/// \internal
template<typename CallbackT>
typename rclcpp::TimerBase::SharedPtr
create_timer(
  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
  std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers,
  rclcpp::Clock::SharedPtr clock,
  rclcpp::Duration period,
  CallbackT && callback,
  rclcpp::CallbackGroup::SharedPtr group = nullptr,
  bool autostart = true)
{
  return create_timer(
    clock,
    period.to_chrono<std::chrono::nanoseconds>(),
    std::forward<CallbackT>(callback),
    group,
    node_base.get(),
    node_timers.get(),
    autostart);
}

/// 使用给定时钟创建定时器
template<typename NodeT, typename CallbackT>
typename rclcpp::TimerBase::SharedPtr
create_timer(
  NodeT node,
  rclcpp::Clock::SharedPtr clock,
  rclcpp::Duration period,
  CallbackT && callback,
  rclcpp::CallbackGroup::SharedPtr group = nullptr,
  bool autostart = true)
{
  return create_timer(
    clock,
    period.to_chrono<std::chrono::nanoseconds>(),
    std::forward<CallbackT>(callback),
    group,
    rclcpp::node_interfaces::get_node_base_interface(node).get(),
    rclcpp::node_interfaces::get_node_timers_interface(node).get(),
    autostart);
}

/// 创建通用定时器
/**
 *
 * \tparam DurationRepT
 * \tparam DurationT
 * \tparam CallbackT
 * \param clock 使用的时钟
 * \param period 定时器回调的执行周期。此时长必须满足 0 <= period < nanoseconds::max()
 * \param callback 定时器周期性执行的回调
 * \param group 回调组
 * \param node_base 节点基础接口
 * \param node_timers 节点计时器接口
 * \param autostart 定义定时器是否应在初始化时启动计时
 * \return 共享指针指向通用定时器
 * \throws std::invalid_argument 如果 clock, node_base 或 node_timers 为空，或 period 为负数或过大
 */
template<typename DurationRepT, typename DurationT, typename CallbackT>
typename rclcpp::GenericTimer<CallbackT>::SharedPtr
create_timer(
  rclcpp::Clock::SharedPtr clock,
  std::chrono::duration<DurationRepT, DurationT> period,
  CallbackT callback,
  rclcpp::CallbackGroup::SharedPtr group,
  node_interfaces::NodeBaseInterface * node_base,
  node_interfaces::NodeTimersInterface * node_timers,
  bool autostart = true)
{
  if (clock == nullptr) {
    throw std::invalid_argument{"clock cannot be null"};
  }
  if (node_base == nullptr) {
    throw std::invalid_argument{"input node_base cannot be null"};
  }
  if (node_timers == nullptr) {
    throw std::invalid_argument{"input node_timers cannot be null"};
  }

  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);

  auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
    std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart);
  node_timers->add_timer(timer, group);
  return timer;
}

/// 创建墙时间定时器
/**
 *
 * \tparam DurationRepT
 * \tparam DurationT
 * \tparam CallbackT
 * \param period 定时器回调的执行周期。此时长必须满足 0 <= period < nanoseconds::max()
 * \param callback 定时器周期性执行的回调
 * \param group 回调组
 * \param node_base 节点基础接口
 * \param node_timers 节点计时器接口
 * \return 共享指针指向墙时间定时器
 * \throws std::invalid_argument 如果 node_base 或 node_timers 为空，或 period 为负数或过大
 */
template<typename DurationRepT, typename DurationT, typename CallbackT>
typename rclcpp::WallTimer<CallbackT>::SharedPtr
create_wall_timer(
  std::chrono::duration<DurationRepT, DurationT> period,
  CallbackT callback,
  rclcpp::CallbackGroup::SharedPtr group,
  node_interfaces::NodeBaseInterface * node_base,
  node_interfaces::NodeTimersInterface * node_timers,
  bool autostart = true)
{
  if (node_base == nullptr) {
    throw std::invalid_argument{"input node_base cannot be null"};
  }

  if (node_timers == nullptr) {
    throw std::invalid_argument{"input node_timers cannot be null"};
  }

  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);

  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
    period_ns, std::move(callback), node_base->get_context(), autostart);
  node_timers->add_timer(timer, group);
  return timer;
}
}  // namespace rclcpp

#endif  // RCLCPP__CREATE_TIMER_HPP_

逐行解析

文件头部

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

解释：
- 这部分是版权声明和许可证信息，说明该代码由 Open Source Robotics Foundation, Inc. 开发，并且受 Apache License 2.0 约束。用户可以在遵循许可证的前提下自由使用、修改和分发该代码。

头文件保护

#ifndef RCLCPP__CREATE_TIMER_HPP_
#define RCLCPP__CREATE_TIMER_HPP_

解释：
- 这是头文件保护机制，防止头文件被多次包含。#ifndef 宏检查 RCLCPP__CREATE_TIMER_HPP_ 是否已经定义，如果没有定义，则继续定义它并包含文件内容。

包含的头文件

#include <chrono>
#include <exception>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/duration.hpp"
#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
#include "rclcpp/node_interfaces/get_node_clock_interface.hpp"
#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_clock_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"

解释：
- 这些头文件提供了创建和管理定时器所需的基础设施：
  - <chrono>: 用于处理时间相关的操作。
  - <exception>: 用于异常处理。
  - <memory>: 用于智能指针管理。
  - <string>: 用于字符串处理。
  - <utility>: 提供了如 std::move 之类的实用函数。
  - rclcpp 相关头文件：这些头文件定义了 ROS 2 中的持续时间、节点接口、定时器接口等相关功能。

命名空间声明

namespace rclcpp
{

解释：
- 这行代码声明了 rclcpp 命名空间。rclcpp 是 ROS 2 的核心命名空间，包含了与客户端、服务、节点、发布者、订阅者、定时器等相关的功能。

`detail::safe_cast_to_period_in_ns` 函数

namespace detail
{
/// 执行安全的定时器周期转换为纳秒单位
/**
 *
 * \tparam DurationRepT
 * \tparam DurationT
 * \param period 定时器回调的执行周期。此时长必须满足 0 <= period < nanoseconds::max()
 * \return 以纳秒为单位的周期
 * \throws std::invalid_argument 如果 period 为负数或过大
 */
template<typename DurationRepT, typename DurationT>
std::chrono::nanoseconds
safe_cast_to_period_in_ns(std::chrono::duration<DurationRepT, DurationT> period)
{
  if (period < std::chrono::duration<DurationRepT, DurationT>::zero()) {
    throw std::invalid_argument{"timer period cannot be negative"};
  }

函数头解析

函数声明：
- 这是一个模板函数，用于将定时器周期转换为纳秒单位的安全操作。它返回一个 std::chrono::nanoseconds 类型的对象，表示周期的纳秒值。
参数解析：
- std::chrono::duration<DurationRepT, DurationT> period: 定时器周期，表示为 std::chrono::duration 类型。
异常处理：
- 如果周期为负数，则抛出 std::invalid_argument 异常，表示定时器周期不能为负。

检查周期是否超过最大值

  constexpr auto maximum_safe_cast_ns =
    std::chrono::nanoseconds::max() - std::chrono::duration<DurationRepT, DurationT>(1);

  constexpr auto ns_max_as_double =
    std::chrono::duration_cast<std::chrono::duration<double, std::chrono::nanoseconds::period>>(
    maximum_safe_cast_ns);
  if (period > ns_max_as_double) {
    throw std::invalid_argument{
            "timer period must be less than std::chrono::nanoseconds::max()"};
  }

解释：
- 这里检查周期是否超过 std::chrono::nanoseconds::max()，这是为了防止周期值过大导致转换时的溢出。
- 使用 maximum_safe_cast_ns 作为安全边界，将周期值与其进行比较。如果周期值超过了该安全边界，则抛出 std::invalid_argument 异常，表示定时器周期必须小于 std::chrono::nanoseconds::max()。

周期转换与溢出检查

  const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(period);
  if (period_ns < std::chrono::nanoseconds::zero()) {
    throw std::runtime_error{
            "Casting timer period to nanoseconds resulted in integer overflow."};
  }

  return period_ns;
}
}  // namespace detail

解释：
- 将周期转换为 std::chrono::nanoseconds 类型，并检查转换后的值是否为负。如果转换后周期值为负，可能意味着发生了整数溢出，此时抛出 std::runtime_error 异常。
- 最终返回转换后的纳秒值 period_ns。

`create_timer` 函数

/// 使用给定时钟创建定时器
/// \internal
template<typename CallbackT>
typename rclcpp::TimerBase::SharedPtr
create_timer(
  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
  std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers,
  rclcpp::Clock::SharedPtr clock,
  rclcpp::Duration period,
  CallbackT && callback,
  rclcpp::CallbackGroup::SharedPtr group = nullptr,
  bool autostart = true)
{
  return create_timer(
    clock,
    period.to_chrono<std::chrono::nanoseconds>(),
    std::forward<CallbackT>(callback),
    group,
    node_base.get(),
    node_timers.get(),
    autostart);
}

函数头解析

函数声明：
- 这是一个模板函数，用于创建定时器。它返回一个指向 rclcpp::TimerBase 对象的共享指针。
参数解析：
- std::shared_ptr<node_interfaces::NodeBaseInterface> node_base: 节点基础接口的共享指针，用于管理节点资源。
- std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers: 节点计时器接口的共享指针，用于管理定时器。
- rclcpp::Clock::SharedPtr clock: 使用的时钟的共享指针。
- rclcpp::Duration period: 定时器周期，表示回调函数的执行间隔。
- CallbackT && callback: 定时器的回调函数，当定时器触发时执行。
- rclcpp::CallbackGroup::SharedPtr group = nullptr: 回调组的共享指针，默认为空。
- bool autostart = true: 是否在创建定时器时自动启动，默认为 true。

内部调用重载的 `create_timer` 函数

解释：
- 该函数内部调用了另一个 create_timer 函数，并传递了转换后的周期、回调函数、回调组、节点基础接口、节点计时器接口以及自动启动选项。

重载的 `create_timer` 函数

/// 使用给定时钟创建定时器
template<typename NodeT, typename CallbackT>
typename rclcpp::TimerBase::SharedPtr
create_timer(
  NodeT node,
  rclcpp::Clock::SharedPtr clock,
  rclcpp::Duration period,
  CallbackT && callback,
  rclcpp::CallbackGroup::SharedPtr group = nullptr,
  bool autostart = true)
{
  return create_timer(
    clock,
    period.to_chrono<std::chrono::nanoseconds>(),
    std::forward<CallbackT>(callback),
    group,
    rclcpp::node_interfaces::get_node_base_interface(node).get(),
    rclcpp::node_interfaces::get_node_timers_interface(node).get(),
    autostart);
}

解释：
- 这个重载版本适用于 NodeT 类型的节点，它提供了 get_node_base_interface 和 get_node_timers_interface 方法，用于获取节点的基础接口和计时器接口。
- 调用时，传入节点对象、时钟、周期、回调函数、回调组以及自动启动选项，内部依旧调用通用的 create_timer 函数。

通用定时器创建函数

template<typename DurationRepT, typename DurationT, typename CallbackT>
typename rclcpp::GenericTimer<CallbackT>::SharedPtr
create_timer(
  rclcpp::Clock::SharedPtr clock,
  std::chrono::duration<DurationRepT, DurationT> period,
  CallbackT callback,
  rclcpp::CallbackGroup::SharedPtr group,
  node_interfaces::NodeBaseInterface * node_base,
  node_interfaces::NodeTimersInterface * node_timers,
  bool autostart = true)
{
  if (clock == nullptr) {
    throw std::invalid_argument{"clock cannot be null"};
  }
  if (node_base == nullptr) {
    throw std::invalid_argument{"input node_base cannot be null"};
  }
  if (node_timers == nullptr) {
    throw std::invalid_argument{"input node_timers cannot be null"};
  }

  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);

  // 添加一个新的通用定时器。
  auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
    std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart);
  node_timers->add_timer(timer, group);
  return timer;
}

函数解析

函数声明：
- 这是一个模板函数，用于创建通用的定时器。它返回一个指向 rclcpp::GenericTimer<CallbackT> 对象的共享指针。
参数解析：
- rclcpp::Clock::SharedPtr clock: 使用的时钟的共享指针。
- std::chrono::duration<DurationRepT, DurationT> period: 定时器周期，表示回调函数的执行间隔。
- CallbackT callback: 定时器的回调函数。
- rclcpp::CallbackGroup::SharedPtr group: 回调组的共享指针。
- node_interfaces::NodeBaseInterface * node_base: 节点基础接口的指针。
- node_interfaces::NodeTimersInterface * node_timers: 节点计时器接口的指针。
- bool autostart: 定义定时器是否应在初始化时启动，默认为 true。

检查参数的有效性

解释：
- 该函数首先检查 clock、node_base 和 node_timers 是否为空。如果任一参数为空，则抛出 std::invalid_argument 异常，表明输入参数无效。

周期转换与定时器创建

解释：
- 使用 detail::safe_cast_to_period_in_ns 函数将传入的周期转换为纳秒单位。
- 然后，创建一个 GenericTimer 对象，传入转换后的周期、回调函数、节点上下文以及自动启动选项。
- 最后，将创建的定时器添加到节点的计时器接口中，并返回定时器的共享指针。

`create_wall_timer` 函数

/// 创建墙时间定时器
/**
 *
 * \tparam DurationRepT
 * \tparam DurationT
 * \tparam CallbackT
 * \param period 定时器回调的执行周期。此时长必须满足 0 <= period < nanoseconds::max()
 * \param callback 定时器周期性执行的回调
 * \param group 回调组
 * \param node_base 节点基础接口
 * \param node_timers 节点计时器接口
 * \return 共享指针指向墙时间定时器
 * \throws std::invalid_argument 如果 node_base 或 node_timers 为空，或 period 为负数或过大
 */
template<typename DurationRepT, typename DurationT, typename CallbackT>
typename rclcpp::WallTimer<CallbackT>::SharedPtr
create_wall_timer(
  std::chrono::duration<DurationRepT, DurationT> period,
  CallbackT callback,
  rclcpp::CallbackGroup::SharedPtr group,
  node_interfaces::NodeBaseInterface * node_base,
  node_interfaces::NodeTimersInterface * node_timers,
  bool autostart = true)
{
  if (node_base == nullptr) {
    throw std::invalid_argument{"input node_base cannot be null"};
  }

  if (node_timers == nullptr) {
    throw std::invalid_argument{"input node_timers cannot be null"};
  }

  const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period);

  // 添加一个新的墙时间定时器。
  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
    period_ns, std::move(callback), node_base->get_context(), autostart);
  node_timers->add_timer(timer, group);
  return timer;
}

函数解析

函数声明：
- 这是一个模板函数，用于创建墙时间定时器。它返回一个指向 rclcpp::WallTimer<CallbackT> 对象的共享指针。
参数解析：
- std::chrono::duration<DurationRepT, DurationT> period: 定时器周期，表示回调函数的执行间隔。
- CallbackT callback: 定时器的回调函数。
- rclcpp::CallbackGroup::SharedPtr group: 回调组的共享指针。
- node_interfaces::NodeBaseInterface * node_base: 节点基础接口的指针。
- node_interfaces::NodeTimersInterface * node_timers: 节点计时器接口的指针。
- bool autostart: 定义定时器是否应在初始化时启动，默认为 true。

检查参数的有效性与定时器创建

解释：
- 该函数首先检查 node_base 和 node_timers 是否为空。如果任一参数为空，则抛出 std::invalid_argument 异常。
- 然后，使用 detail::safe_cast_to_period_in_ns 函数将传入的周期转换为纳秒单位。
- 最后，创建一个 WallTimer 对象，并将其添加到节点的计时器接口中，返回定时器的共享指针。

命名空间结束与头文件保护结束

}  // namespace rclcpp

#endif  // RCLCPP__CREATE_TIMER_HPP_

解释：
- 这部分代码关闭了 rclcpp 命名空间，并结束了头文件的防重复包含保护。

总结

create_timer 和 create_wall_timer 函数为 ROS 2 提供了强大且灵活的定时器创建功能。通过多个重载版本，用户可以根据具体需求选择最合适的方式创建定时器。无论是处理周期性任务，还是需要精确控制定时器行为，这些函数都能够有效地简化开发过程，同时确保代码的可读性和可维护性。