介绍
ROS 2 引入了受管节点的概念,也称为 LifecycleNode
。在下面的教程中,我们将解释这些节点的用途、它们与常规节点的不同之处以及它们如何遵守生命周期管理。受管节点包含具有一组预定义状态的状态机。这些状态可以通过调用指示后续连续状态的转换 ID 来更改。状态机的实现如 ROS 2 设计页面 https://design.ros2.org/articles/node_lifecycle.html 中所述。
我们的实现区分 Primary States
和 Transition States
。主要状态应该是稳定状态,其中任何节点都可以执行相关任务。另一方面,过渡状态是指附属于过渡的临时中间状态。这些中间状态的结果用于指示两个主要状态之间的转换是否被视为成功。因此,任何被管节点都可以处于以下状态之一:
主要状态(稳态):
unconfigured 未配置
inactive 不活跃
active 活跃状态
shutdown 关机
过渡状态(中间状态):
configuring 配置
activating 启动
deactivating 停用
cleaningup 清理中
shuttingdown 关机
可能要调用的转换是:
configure 配置
activate 启动
deactivate 禁用
cleanup 清理
shutdown 关机
为了更详细地解释所应用的状态机,我们参考了设计页面,该页面详细解释了每个状态和转换。
演示
发生了什么
演示被分为三个独立的应用程序:
lifecycle_talker 生命周期讲述者
lifecycle_listener 生命周期监听器
lifecycle_service_client 生命周期服务客户端
lifecycle_talker
表示一个受控节点,并根据节点的状态进行发布。我们将讲述节点的任务分成几个部分,并按如下方式执行:
配置:我们初始化了发布器和计时器
激活:我们激活发布者和计时器以便进行发布
我们停止发布者和计时器
清理:我们销毁了发布者和计时器
#include <chrono> // 包含用于处理时间的库
#include <memory> // 包含智能指针库
#include <string> // 包含字符串库
#include <thread> // 包含线程库
#include <utility> // 包含实用工具库
#include "lifecycle_msgs/msg/transition.hpp" // 包含生命周期消息中的转换消息
#include "rclcpp/rclcpp.hpp" // 包含ROS 2客户端库
#include "rclcpp/publisher.hpp" // 包含ROS 2的发布者库
#include "rclcpp_lifecycle/lifecycle_node.hpp" // 包含生命周期节点库
#include "rclcpp_lifecycle/lifecycle_publisher.hpp" // 包含生命周期发布者库
#include "std_msgs/msg/string.hpp" // 包含标准消息中的字符串消息
using namespace std::chrono_literals; // 使用chrono库中的字面量表示法
/// LifecycleTalker类继承自rclcpp_lifecycle::LifecycleNode
/**
* LifecycleTalker类并不像常规的“talker”节点那样继承自Node,而是继承自LifecycleNode。
* 这带来了一组回调函数,根据节点的当前状态被调用。
* 每个生命周期节点都有一组服务附加到它上面,这些服务使其可以从外部控制并触发状态变化。
* Beta1版本中可用的服务:
* - <node_name>__get_state
* - <node_name>__change_state
* - <node_name>__get_available_states
* - <node_name>__get_available_transitions
* 另外,还创建了一个用于状态变化通知的发布者:
* - <node_name>__transition_event
*/
class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode
{
public:
/// LifecycleTalker构造函数
/**
* LifecycleTalker/LifecycleNode构造函数与常规节点的构造函数相同。
*/
explicit LifecycleTalker(const std::string & node_name, bool intra_process_comms = false)
: rclcpp_lifecycle::LifecycleNode(node_name,
rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)) // 初始化父类LifecycleNode
{}
/// 用于定时器回调以发布消息的函数
/**
* 用于定时器回调的函数。此函数由定时器调用并执行发布操作。
* 在此演示中,我们询问节点的当前状态。如果生命周期发布者未激活,我们仍然调用发布,但通信被阻止,因此实际上没有消息被传输。
*/
void
publish()
{
static size_t count = 0; // 静态计数器
auto msg = std::make_unique<std_msgs::msg::String>(); // 创建一个唯一指针的字符串消息
msg->data = "Lifecycle HelloWorld #" + std::to_string(++count); // 设置消息内容
// 打印当前状态以供演示
if (!pub_->is_activated()) {
RCLCPP_INFO(
get_logger(), "Lifecycle publisher is currently inactive. Messages are not published."); // 如果发布者未激活,打印信息
} else {
RCLCPP_INFO(
get_logger(), "Lifecycle publisher is active. Publishing: [%s]", msg->data.c_str()); // 如果发布者已激活,打印并发布信息
}
// 我们在任何状态下都调用生命周期发布者的发布函数。
// 只有在发布者处于激活状态时,消息传输才会启用并且消息实际发布。
pub_->publish(std::move(msg)); // 发布消息
}
/// 用于配置状态的过渡回调函数
/**
* on_configure回调函数在生命周期节点进入“configuring”状态时调用。
* 根据此函数的返回值,状态机要么触发到“inactive”状态的过渡,要么保持在“unconfigured”状态。
* TRANSITION_CALLBACK_SUCCESS 过渡到“inactive”
* TRANSITION_CALLBACK_FAILURE 过渡到“unconfigured”
* TRANSITION_CALLBACK_ERROR 或任何未捕获的异常则过渡到“errorprocessing”
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_configure(const rclcpp_lifecycle::State &)
{
// 此回调用于初始化和配置。
// 因此我们初始化和配置我们的发布者和定时器。
// 生命周期节点API返回生命周期组件,例如生命周期发布者。这些实体遵循生命周期并能遵守节点的当前状态。
// 在beta版本中,目前只有一个生命周期发布者可用。
pub_ = this->create_publisher<std_msgs::msg::String>("lifecycle_chatter", 10); // 创建生命周期发布者
timer_ = this->create_wall_timer(
1s, [this]() {return this->publish();}); // 创建定时器
RCLCPP_INFO(get_logger(), "on_configure() is called."); // 打印配置信息
// 我们返回成功并因此触发到下一个步骤:“inactive”的过渡。
// 如果我们返回TRANSITION_CALLBACK_FAILURE,状态机将保持在“unconfigured”状态。
// 如果在此回调中发生TRANSITION_CALLBACK_ERROR或任何抛出的异常,状态机将过渡到“errorprocessing”状态。
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
}
/// 用于激活状态的过渡回调函数
/**
* on_activate回调函数在生命周期节点进入“activating”状态时调用。
* 根据此函数的返回值,状态机要么触发到“active”状态的过渡,要么保持在“inactive”状态。
* TRANSITION_CALLBACK_SUCCESS 过渡到“active”
* TRANSITION_CALLBACK_FAILURE 过渡到“inactive”
* TRANSITION_CALLBACK_ERROR 或任何未捕获的异常则过渡到“errorprocessing”
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_activate(const rclcpp_lifecycle::State & state)
{
// 父类方法自动在托管实体(目前是LifecyclePublisher)上过渡。
// 这里也可以手动调用pub_->on_activate()。
// 重写此方法是可选的,很多时候默认方法就足够了。
LifecycleNode::on_activate(state);
RCLCPP_INFO(get_logger(), "on_activate() is called."); // 打印激活信息
// 睡眠2秒。
// 我们模拟在激活阶段进行重要工作。
std::this_thread::sleep_for(2s); // 休眠2秒
// 我们返回成功并因此触发到下一个步骤:“active”的过渡。
// 如果我们返回TRANSITION_CALLBACK_FAILURE,状态机将保持在“inactive”状态。
// 如果在此回调中发生TRANSITION_CALLBACK_ERROR或任何抛出的异常,状态机将过渡到“errorprocessing”状态。
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
}
/// 用于停用状态的过渡回调函数
/**
* on_deactivate回调函数在生命周期节点进入“deactivating”状态时调用。
* 根据此函数的返回值,状态机要么触发到“inactive”状态的过渡,要么保持在“active”状态。
* TRANSITION_CALLBACK_SUCCESS 过渡到“inactive”
* TRANSITION_CALLBACK_FAILURE 过渡到“active”
* TRANSITION_CALLBACK_ERROR 或任何未捕获的异常则过渡到“errorprocessing”
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State & state)
{
// 父类方法自动在托管实体(目前是LifecyclePublisher)上过渡。
// 这里也可以手动调用pub_->on_deactivate()。
// 重写此方法是可选的,很多时候默认方法就足够了。
LifecycleNode::on_deactivate(state);
RCLCPP_INFO(get_logger(), "on_deactivate() is called."); // 打印停用信息
// 我们返回成功并因此触发到下一个步骤:“inactive”的过渡。
// 如果我们返回TRANSITION_CALLBACK_FAILURE,状态机将保持在“active”状态。
// 如果在此回调中发生TRANSITION_CALLBACK_ERROR或任何抛出的异常,状态机将过渡到“errorprocessing”状态。
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
}
/// 用于清理状态的过渡回调函数
/**
* on_cleanup回调函数在生命周期节点进入“cleaningup”状态时调用。
* 根据此函数的返回值,状态机要么触发到“unconfigured”状态的过渡,要么保持在“inactive”状态。
* TRANSITION_CALLBACK_SUCCESS 过渡到“unconfigured”
* TRANSITION_CALLBACK_FAILURE 过渡到“inactive”
* TRANSITION_CALLBACK_ERROR 或任何未捕获的异常则过渡到“errorprocessing”
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State &)
{
// 在我们的清理阶段,我们释放定时器和发布者的共享指针。这些实体不再可用,我们的节点是“干净”的。
timer_.reset();
pub_.reset();
RCLCPP_INFO(get_logger(), "on cleanup is called."); // 打印清理信息
// 我们返回成功并因此触发到下一个步骤:“unconfigured”的过渡。
// 如果我们返回TRANSITION_CALLBACK_FAILURE,状态机将保持在“inactive”状态。
// 如果在此回调中发生TRANSITION_CALLBACK_ERROR或任何抛出的异常,状态机将过渡到“errorprocessing”状态。
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
}
/// 用于关闭状态的过渡回调函数
/**
* on_shutdown回调函数在生命周期节点进入“shuttingdown”状态时调用。
* 根据此函数的返回值,状态机要么触发到“finalized”状态的过渡,要么保持在当前状态。
* TRANSITION_CALLBACK_SUCCESS 过渡到“finalized”
* TRANSITION_CALLBACK_FAILURE 过渡到当前状态
* TRANSITION_CALLBACK_ERROR 或任何未捕获的异常则过渡到“errorprocessing”
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_shutdown(const rclcpp_lifecycle::State & state)
{
// 在我们的关闭阶段,我们释放定时器和发布者的共享指针。这些实体不再可用,我们的节点是“干净”的。
timer_.reset();
pub_.reset();
RCLCPP_INFO(get_logger(), "on shutdown is called from state %s.", state.label().c_str()); // 打印关闭信息
// 我们返回成功并因此触发到下一个步骤:“finalized”的过渡。
// 如果我们返回TRANSITION_CALLBACK_FAILURE,状态机将保持在当前状态。
// 如果在此回调中发生TRANSITION_CALLBACK_ERROR或任何抛出的异常,状态机将过渡到“errorprocessing”状态。
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
}
private:
// 我们持有一个生命周期发布者的实例。根据生命周期节点所处的状态,此生命周期发布者可以被激活或停用。
// 默认情况下,生命周期发布者在创建时是未激活的,必须激活才能将消息发布到ROS世界。
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> pub_;
// 我们持有一个定时器实例,该定时器定期触发发布函数。
// 在beta版本中,这是一个常规定时器。在未来版本中,将创建一个生命周期定时器,它遵循与生命周期发布者相同的生命周期管理。
std::shared_ptr<rclcpp::TimerBase> timer_;
};
/**
* 生命周期节点具有与常规节点相同的节点API。
* 这意味着我们可以生成一个节点,给它一个名称并将其添加到执行器中。
*/
int main(int argc, char * argv[])
{
// 强制刷新stdout缓冲区。
// 这确保了所有打印输出的正确同步,即使在启动文件中同时执行时也是如此。
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv); // 初始化ROS 2
rclcpp::executors::SingleThreadedExecutor exe; // 创建单线程执行器
std::shared_ptr<LifecycleTalker> lc_node =
std::make_shared<LifecycleTalker>("lc_talker"); // 创建LifecycleTalker节点
exe.add_node(lc_node->get_node_base_interface()); // 将节点添加到执行器
exe.spin(); // 开始执行循环
rclcpp::shutdown(); // 关闭ROS 2
return 0; // 返回
}
该演示展示了一对典型的说话者/听者节点。然而,想象一个真实的场景,附加的硬件可能需要相当长的启动时间,例如激光器或相机。可以想象在配置状态下启动设备驱动程序,仅在活动/非活动状态下启动和停止设备数据的发布,并且仅在清理/关闭状态下实际关闭设备。
lifecycle_listener
是一个简单的监听器,展示了生命周期对话者的特征。对话者仅在活动状态下发布消息,因此监听器仅在对话者处于活动状态时接收消息。
#include <functional> // 包含功能库
#include <memory> // 包含智能指针库
#include <string> // 包含字符串库
#include "lifecycle_msgs/msg/transition_event.hpp" // 包含生命周期消息中的转换事件消息
#include "rclcpp/rclcpp.hpp" // 包含ROS 2客户端库
#include "std_msgs/msg/string.hpp" // 包含标准消息中的字符串消息
/// LifecycleListener 类作为一个简单的监听节点
/**
* 我们订阅了两个主题:
* - lifecycle_chatter:来自 talker 节点的数据主题
* - lc_talker__transition_event:发布关于 lc_talker 节点状态变化通知的主题
*/
class LifecycleListener : public rclcpp::Node
{
public:
/// LifecycleListener 构造函数
/**
* 构造函数,用于初始化订阅两个主题的监听节点。
*/
explicit LifecycleListener(const std::string & node_name)
: Node(node_name) // 初始化父类 Node
{
// 从 lc_talker 节点接收数据的主题
sub_data_ = this->create_subscription<std_msgs::msg::String>(
"lifecycle_chatter", 10, // 创建订阅者,订阅主题“lifecycle_chatter”,队列大小为10
[this](std_msgs::msg::String::ConstSharedPtr msg) {return this->data_callback(msg);}); // 设置回调函数
// 通知事件主题。所有状态变化都发布在这里,作为 TransitionEvent
// 包含表示转换的起始状态和目标状态
sub_notification_ = this->create_subscription<lifecycle_msgs::msg::TransitionEvent>(
"/lc_talker/transition_event", // 订阅主题“/lc_talker/transition_event”
10, // 队列大小为10
[this](lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg) {
return this->notification_callback(msg); // 设置回调函数
}
);
}
/// 数据回调函数
/**
* 处理订阅到的数据主题的消息
*/
void data_callback(std_msgs::msg::String::ConstSharedPtr msg)
{
RCLCPP_INFO(get_logger(), "data_callback: %s", msg->data.c_str()); // 打印接收到的数据消息
}
/// 通知回调函数
/**
* 处理订阅到的转换事件主题的消息
*/
void notification_callback(lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg)
{
RCLCPP_INFO(
get_logger(), "notify callback: Transition from state %s to %s", // 打印转换事件的起始状态和目标状态
msg->start_state.label.c_str(), msg->goal_state.label.c_str());
}
private:
// 声明数据主题的订阅者
std::shared_ptr<rclcpp::Subscription<std_msgs::msg::String>> sub_data_;
// 声明通知事件主题的订阅者
std::shared_ptr<rclcpp::Subscription<lifecycle_msgs::msg::TransitionEvent>>
sub_notification_;
};
/// 主函数
int main(int argc, char ** argv)
{
// 强制刷新 stdout 缓冲区
// 确保在启动文件中同时执行时所有打印输出的正确同步
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv); // 初始化ROS 2
auto lc_listener = std::make_shared<LifecycleListener>("lc_listener"); // 创建LifecycleListener节点
rclcpp::spin(lc_listener); // 使节点进入循环
rclcpp::shutdown(); // 关闭ROS 2
return 0; // 返回
}
lifecycle_service_client
是一个在 lifecycle_talker
上调用不同转换的脚本。这意味着外部用户控制节点的生命周期。
#include <chrono> // 包含时间库
#include <future> // 包含异步任务库
#include <memory> // 包含智能指针库
#include <string> // 包含字符串库
#include <thread> // 包含线程库
#include "lifecycle_msgs/msg/state.hpp" // 包含生命周期消息中的状态消息
#include "lifecycle_msgs/msg/transition.hpp" // 包含生命周期消息中的转换消息
#include "lifecycle_msgs/srv/change_state.hpp" // 包含生命周期服务中的更改状态服务
#include "lifecycle_msgs/srv/get_state.hpp" // 包含生命周期服务中的获取状态服务
#include "rclcpp/rclcpp.hpp" // 包含ROS 2客户端库
using namespace std::chrono_literals; // 使用时间字面量
// 要处理的节点
static constexpr char const * lifecycle_node = "lc_talker";
// 每个生命周期节点都有多个服务
// 按照惯例,我们使用 <节点名>/<服务名> 的格式。
// 在这个示例中,我们使用 get_state 和 change_state
// 因此两个服务主题是:
// lc_talker/get_state
// lc_talker/change_state
static constexpr char const * node_get_state_topic = "lc_talker/get_state";
static constexpr char const * node_change_state_topic = "lc_talker/change_state";
// 等待结果函数模板
template<typename FutureT, typename WaitTimeT>
std::future_status
wait_for_result(
FutureT & future, // 异步任务
WaitTimeT time_to_wait) // 等待时间
{
auto end = std::chrono::steady_clock::now() + time_to_wait; // 计算结束时间
std::chrono::milliseconds wait_period(100); // 每次等待100毫秒
std::future_status status = std::future_status::timeout; // 初始状态设为超时
do {
auto now = std::chrono::steady_clock::now(); // 获取当前时间
auto time_left = end - now; // 计算剩余时间
if (time_left <= std::chrono::seconds(0)) {break;} // 如果剩余时间小于等于0,跳出循环
status = future.wait_for((time_left < wait_period) ? time_left : wait_period); // 等待结果
} while (rclcpp::ok() && status != std::future_status::ready); // 如果ROS正常运行且状态不是就绪,继续等待
return status; // 返回最终状态
}
// 生命周期服务客户端类
class LifecycleServiceClient : public rclcpp::Node
{
public:
explicit LifecycleServiceClient(const std::string & node_name)
: Node(node_name) // 初始化父类 Node
{}
// 初始化函数
void
init()
{
// 每个生命周期节点自动生成几个服务
// 允许外部与这些节点进行交互
// 主要的两个服务是 GetState 和 ChangeState
client_get_state_ = this->create_client<lifecycle_msgs::srv::GetState>(
node_get_state_topic); // 创建获取状态服务客户端
client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>(
node_change_state_topic); // 创建更改状态服务客户端
}
// 请求节点的当前状态
/**
* 在这个函数中,我们发送服务请求
* 询问节点 lc_talker 的当前状态。
* 如果在给定的超时时间内没有返回,
* 我们返回节点的未知状态,否则返回当前状态。
* \param time_out 等待响应的超时时间(秒)
*/
unsigned int
get_state(std::chrono::seconds time_out = 3s)
{
auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>(); // 创建请求
if (!client_get_state_->wait_for_service(time_out)) { // 等待服务
RCLCPP_ERROR(
get_logger(),
"Service %s is not available.",
client_get_state_->get_service_name()); // 打印错误信息
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; // 返回未知状态
}
// 发送服务请求以询问 lc_talker 节点的当前状态
auto future_result = client_get_state_->async_send_request(request).future.share();
// 等待节点的响应
// 如果请求超时,返回未知状态
auto future_status = wait_for_result(future_result, time_out);
if (future_status != std::future_status::ready) { // 如果状态不是就绪
RCLCPP_ERROR(
get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; // 返回未知状态
}
// 成功接收到响应,打印当前状态
if (future_result.get()) {
RCLCPP_INFO(
get_logger(), "Node %s has current state %s.",
lifecycle_node, future_result.get()->current_state.label.c_str()); // 打印状态信息
return future_result.get()->current_state.id; // 返回当前状态ID
} else {
RCLCPP_ERROR(
get_logger(), "Failed to get current state for node %s", lifecycle_node); // 打印错误信息
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; // 返回未知状态
}
}
// 调用转换函数
/**
* 我们发送一个服务请求,表示
* 我们希望调用 id 为 "transition" 的转换。
* 默认情况下,这些转换是
* - configure(配置)
* - activate(激活)
* - cleanup(清理)
* - shutdown(关闭)
* \param transition 表示要调用的转换的ID
* \param time_out 等待响应的超时时间(秒)
*/
bool
change_state(std::uint8_t transition, std::chrono::seconds time_out = 3s)
{
auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>(); // 创建请求
request->transition.id = transition; // 设置转换ID
if (!client_change_state_->wait_for_service(time_out)) { // 等待服务
RCLCPP_ERROR(
get_logger(),
"Service %s is not available.",
client_change_state_->get_service_name()); // 打印错误信息
return false;
}
// 发送请求以调用转换
auto future_result = client_change_state_->async_send_request(request).future.share();
// 等待节点的响应
// 如果请求超时,返回失败
auto future_status = wait_for_result(future_result, time_out);
if (future_status != std::future_status::ready) { // 如果状态不是就绪
RCLCPP_ERROR(
get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
return false;
}
// 成功接收到响应,打印转换成功信息
if (future_result.get()->success) {
RCLCPP_INFO(
get_logger(), "Transition %d successfully triggered.", static_cast<int>(transition)); // 打印成功信息
return true;
} else {
RCLCPP_WARN(
get_logger(), "Failed to trigger transition %u", static_cast<unsigned int>(transition)); // 打印警告信息
return false;
}
}
private:
std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_state_; // 获取状态服务客户端
std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_; // 更改状态服务客户端
};
/**
* 这是一个小的独立脚本,
* 触发节点的默认生命周期。
* 它从 configure 开始,接着是 activate、
* deactivate,再次 activate、deactivate,
* 然后是 cleanup,最后是 shutdown。
*/
void
callee_script(std::shared_ptr<LifecycleServiceClient> lc_client)
{
rclcpp::WallRate time_between_state_changes(0.1); // 状态变化之间的时间间隔为0.1秒
using Transition = lifecycle_msgs::msg::Transition; // 使用生命周期消息中的 Transition
// configure
{
if (!lc_client->change_state(Transition::TRANSITION_CONFIGURE)) { // 触发 configure 转换
return; // 如果失败则退出
}
if (!lc_client->get_state()) { // 获取当前状态
return; // 如果获取失败则退出
}
}
// activate
{
time_between_state_changes.sleep(); // 等待
if (!rclcpp::ok()) { // 如果 ROS 2 已关闭,退出
return;
}
if (!lc_client->change_state(Transition::TRANSITION_ACTIVATE)) { // 触发 activate 转换
return; // 如果失败则退出
}
if (!lc_client->get_state()) { // 获取当前状态
return; // 如果获取失败则退出
}
}
// deactivate
{
time_between_state_changes.sleep(); // 等待
if (!rclcpp::ok()) { // 如果 ROS 2 已关闭,退出
return;
}
if (!lc_client->change_state(Transition::TRANSITION_DEACTIVATE)) { // 触发 deactivate 转换
return; // 如果失败则退出
}
if (!lc_client->get_state()) { // 获取当前状态
return; // 如果获取失败则退出
}
}
// 再次激活
{
time_between_state_changes.sleep(); // 等待
if (!rclcpp::ok()) { // 如果 ROS 2 已关闭,退出
return;
}
if (!lc_client->change_state(Transition::TRANSITION_ACTIVATE)) { // 触发 activate 转换
return; // 如果失败则退出
}
if (!lc_client->get_state()) { // 获取当前状态
return; // 如果获取失败则退出
}
}
// 再次停用
{
time_between_state_changes.sleep(); // 等待
if (!rclcpp::ok()) { // 如果 ROS 2 已关闭,退出
return;
}
if (!lc_client->change_state(Transition::TRANSITION_DEACTIVATE)) { // 触发 deactivate 转换
return; // 如果失败则退出
}
if (!lc_client->get_state()) { // 获取当前状态
return; // 如果获取失败则退出
}
}
// 进行清理
{
time_between_state_changes.sleep(); // 等待
if (!rclcpp::ok()) { // 如果 ROS 2 已关闭,退出
return;
}
if (!lc_client->change_state(Transition::TRANSITION_CLEANUP)) { // 触发 cleanup 转换
return; // 如果失败则退出
}
if (!lc_client->get_state()) { // 获取当前状态
return; // 如果获取失败则退出
}
}
// 最后关闭
// 注意:在调用 shutdown 转换ID 时,我们必须准确。
// 我们当前处于未配置状态,因此必须调用
// TRANSITION_UNCONFIGURED_SHUTDOWN
{
time_between_state_changes.sleep(); // 等待
if (!rclcpp::ok()) { // 如果 ROS 2 已关闭,退出
return;
}
if (!lc_client->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)) { // 触发 shutdown 转换
return; // 如果失败则退出
}
if (!lc_client->get_state()) { // 获取当前状态
return; // 如果获取失败则退出
}
}
}
void
wake_executor(std::shared_future<void> future, rclcpp::executors::SingleThreadedExecutor & exec)
{
future.wait(); // 等待脚本完成
// 当脚本完成时唤醒执行器
// https://github.com/ros2/rclcpp/issues/1916
exec.cancel(); // 取消执行器
}
int main(int argc, char ** argv)
{
// 强制刷新 stdout 缓冲区。
// 确保在启动文件中同时执行时所有打印输出的正确同步。
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv); // 初始化 ROS 2
auto lc_client = std::make_shared<LifecycleServiceClient>("lc_client"); // 创建 LifecycleServiceClient 节点
lc_client->init(); // 初始化节点
rclcpp::executors::SingleThreadedExecutor exe; // 创建单线程执行器
exe.add_node(lc_client); // 将节点添加到执行器
std::shared_future<void> script = std::async(
std::launch::async,
callee_script,
lc_client // 启动脚本
);
auto wake_exec = std::async(
std::launch::async,
wake_executor,
script,
std::ref(exe) // 启动唤醒执行器
);
exe.spin_until_future_complete(script); // 执行直到脚本完成
rclcpp::shutdown(); // 关闭 ROS 2
return 0;
}
运行示例
为了运行这个演示,我们打开三个终端,并从二进制发行版或我们从源代码编译的工作区中加载 ROS 2 环境变量。
生命周期讲述者 | 生命周期监听器 | 生命周期服务客户端 |
---|---|---|
$ ros2 run lifecycle lifecycle_talker | $ ros2 run lifecycle lifecycle_listener | $ ros2 run lifecycle lifecycle_service_client |
cxy@cxy-Ubuntu2404:~$ ros2 run lifecycle lifecycle_service_client
[INFO] [1721622029.248486621] [lc_client]: Transition 1 successfully triggered.
[INFO] [1721622029.249219857] [lc_client]: Node lc_talker has current state inactive.
[INFO] [1721622041.247756146] [lc_client]: Transition 3 successfully triggered.
[INFO] [1721622041.248743872] [lc_client]: Node lc_talker has current state active.
[INFO] [1721622049.247651152] [lc_client]: Transition 4 successfully triggered.
[INFO] [1721622049.248932501] [lc_client]: Node lc_talker has current state inactive.
[INFO] [1721622061.247750975] [lc_client]: Transition 3 successfully triggered.
[INFO] [1721622061.249117121] [lc_client]: Node lc_talker has current state active.
[INFO] [1721622069.247679262] [lc_client]: Transition 4 successfully triggered.
[INFO] [1721622069.248990375] [lc_client]: Node lc_talker has current state inactive.
[INFO] [1721622079.247619695] [lc_client]: Transition 2 successfully triggered.
[INFO] [1721622079.249757541] [lc_client]: Node lc_talker has current state unconfigured.
[INFO] [1721622089.247976786] [lc_client]: Transition 5 successfully triggered.
[INFO] [1721622089.249151815] [lc_client]: Node lc_talker has current state finalized.
或者,可以使用启动文件在同一个终端中同时运行这三个程序:
ros2 launch lifecycle lifecycle_demo_launch.py
# 导入LaunchDescription模块,该模块用于描述启动配置
from launch import LaunchDescription
# 导入Shutdown模块,该模块用于在节点退出时关闭其他节点
from launch.actions import Shutdown
# 导入LifecycleNode模块,该模块用于创建生命周期节点
from launch_ros.actions import LifecycleNode
# 导入Node模块,该模块用于创建节点
from launch_ros.actions import Node
# 定义一个函数,该函数生成并返回一个启动描述
def generate_launch_description():
# 返回一个启动描述,该描述包含以下节点:
return LaunchDescription([
# 创建一个生命周期节点,包名为'lifecycle',可执行文件为'lifecycle_talker',
# 节点名为'lc_talker',命名空间为空,输出到屏幕
LifecycleNode(package='lifecycle', executable='lifecycle_talker',
name='lc_talker', namespace='', output='screen'),
# 创建一个节点,包名为'lifecycle',可执行文件为'lifecycle_listener',输出到屏幕
Node(package='lifecycle', executable='lifecycle_listener', output='screen'),
# 创建一个节点,包名为'lifecycle',可执行文件为'lifecycle_service_client',输出到屏幕,
# 当该节点退出时,执行Shutdown操作
Node(package='lifecycle', executable='lifecycle_service_client', output='screen',
on_exit=Shutdown()),
])
如果我们查看 lifecycle_talker
的输出,我们会注意到似乎没有任何事情发生。这是有道理的,因为每个节点都以 unconfigured
开始。lifecycle_talker 尚未配置,在我们的示例中,还没有创建发布者和计时器。对于 lifecycle_listener
,也可以看到相同的行为,这并不令人惊讶,因为此时没有可用的发布者。有趣的部分从第三个终端开始。在那里,我们启动了负责更改 lifecycle_talker
状态的 lifecycle_service_client
。
配置触发转换 1
[lc_client] Transition 1 successfully triggered.
[lc_client] Node lc_talker has current state inactive.
使生命周期对话器的状态变为不活动。不活动状态意味着所有发布者和计时器都已创建和配置,但节点仍未激活,因此不会发布任何消息。
[lc_talker] on_configure() is called.
Lifecycle publisher is currently inactive. Messages are not published.
...
同时,生命周期监听器会收到通知,因为它监听生命周期对话者的每个状态变化通知。实际上,监听器会收到两个连续的通知。一个是从主要状态“未配置”更改为“配置中”,另一个是从“配置中”更改为“非活动”(因为对话者的配置步骤成功)。
[lc_listener] notify callback: Transition from state unconfigured to configuring
[lc_listener] notify callback: Transition from state configuring to inactive
激活转换 2
[lc_client] Transition 2 successfully triggered.
[lc_client] Node lc_talker has current state active.
使生命周期对话者变为活动状态。这意味着所有发布者和计时器都已激活,消息也正在发布。
[lc_talker] on_activate() is called.
[lc_talker] Lifecycle publisher is active. Publishing: [Lifecycle HelloWorld #11]
[lc_talker] Lifecycle publisher is active. Publishing: [Lifecycle HelloWorld #12]
...
生命周期监听器接收到与之前相同的一组通知。生命周期对话器的状态从不活动变为活动。
[lc_listener]: notify callback: Transition from state inactive to activating
[lc_listener]: notify callback: Transition from state activating to active
与之前的转换事件不同的是,我们的监听器现在也接收到了实际发布的数据。
[lc_listener] data_callback: Lifecycle HelloWorld #11
[lc_listener] data_callback: Lifecycle HelloWorld #12
...
请注意,已发布消息的索引已达到 11。此演示旨在展示即使我们在生命周期的每个状态下调用 publish
,消息也仅在状态处于活动时才会发布。
在演示的其余部分中,您将看到类似的输出,因为我们会停用和激活生命周期对话器,最后将其关闭。
示例代码
生命周期对话器,生命周期监听器和生命周期服务客户端
如果我们看看代码,与常规的 talker 相比,生命周期 talker 有一个显著的变化。我们的节点不是继承自常规的 rclcpp::node::Node ,而是继承自 rclcpp_lifecycle::LifecycleNode 。
class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode
每个 LifecycleNodes 的子节点都有一组回调函数。这些回调函数与附加的状态机一起使用。这些回调函数包括:
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_configure(const rclcpp_lifecycle::State & previous_state)
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_activate(const rclcpp_lifecycle::State & previous_state)
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State & previous_state)
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State & previous_state)
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_shutdown(const rclcpp_lifecycle::State & previous_state)
在下文中,我们假设我们在命名空间 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
内,以缩短返回类型的名称。所有这些回调都有一个正的默认返回值( return CallbackReturn::SUCCESS
)。这允许生命周期节点改变其状态,即使没有显式覆盖回调函数。还有一个用于错误处理的回调函数。每当状态转换抛出未捕获的异常时,我们调用 on_error
:
CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state)
这为执行自定义错误处理提供了空间。只有当此函数返回 CallbackReturn::SUCCESS
时,状态机才会转换到状态 unconfigured
。默认情况下, on_error
返回 CallbackReturn::FAILURE
,状态机转换到 finalized
。
同时,每个生命周期节点默认配备了 5 种不同的通信接口。
发布者
<node_name>__transition_event
: 在转换过程中进行发布。
这使用户能够接收到网络中转换事件的通知。
服务
<node_name>__get_state
:查询节点当前状态。
返回主要状态或过渡状态。
服务
<node_name>__change_state
:触发当前节点的状态转换。
此服务调用需要一个转换 ID。只有当该转换 ID 是从当前状态的有效转换时,才会执行转换。所有其他情况都将被忽略。
服务
<node_name>__get_available_states
: 这是一个用于自我反省的工具。
它返回该节点所有可能的状态列表。
服务
<node_name>__get_available_transitions
: 与上面相同,旨在用于自省工具。
它返回该节点可以执行的所有可能的转换列表。
ROS2 生命周期命令行工具
lifecycle_service_client 应用程序是一个固定顺序脚本,仅用于演示。它解释了生命周期实现的使用和 API 调用,但在其他情况可能不方便使用。因此,我们实现了一个命令行工具,可以让您动态更改状态或各种节点。
如果您想获取 lc_talker
节点的当前状态,可以调用以下命令:
$ ros2 lifecycle get /lc_talker
unconfigured [1]
下一步是执行状态更改:
$ ros2 lifecycle set /lc_talker configure
Transitioning successful
要查看当前可用的状态:
$ ros2 lifecycle list lc_talker
- configure [1]
Start: unconfigured
Goal: configuring
- shutdown [5]
Start: unconfigured
Goal: shuttingdown
在这种情况下,我们看到当前可用的转换是 configure
和 shutdown
。可以使用以下命令查看完整的状态机,这对于调试或可视化非常有帮助:
$ ros2 lifecycle list lc_talker -a
- configure [1]
Start: unconfigured
Goal: configuring
- transition_success [10]
Start: configuring
Goal: inactive
- transition_failure [11]
Start: configuring
Goal: unconfigured
- transition_error [12]
Start: configuring
Goal: errorprocessing
[...]
- transition_error [62]
Start: errorprocessing
Goal: finalized
上述所有命令只是调用生命周期节点的服务。也就是说,我们可以直接使用 ros2 命令行接口来调用这些服务:
$ ros2 service call /lc_talker/get_state lifecycle_msgs/GetState
requester: making request: lifecycle_msgs.srv.GetState_Request()
response:
lifecycle_msgs.srv.GetState_Response(current_state=lifecycle_msgs.msg.State(id=1, label='unconfigured'))
为了触发转换,我们调用了 change_state
服务
$ ros2 service call /lc_talker/change_state lifecycle_msgs/ChangeState "{transition: {id: 2}}"
requester: making request: lifecycle_msgs.srv.ChangeState_Request(transition=lifecycle_msgs.msg.Transition(id=2, label=''))
response:
lifecycle_msgs.srv.ChangeState_Response(success=True)
这有点不方便,因为你需要知道每个转换对应的 ID。不过,你可以在 lifecycle_msgs 包中找到它们。
cxy@cxy-Ubuntu2404:~/Desktop$ ros2 interface show lifecycle_msgs/msg/Transition
# Default values for transitions as described in:
# http://design.ros2.org/articles/node_lifecycle.html
# Reserved [0-9], publicly available transitions.
# When a node is in one of these primary states, these transitions can be
# invoked.
# This transition will instantiate the node, but will not run any code beyond
# the constructor.
uint8 TRANSITION_CREATE = 0
# The node's onConfigure callback will be called to allow the node to load its
# configuration and conduct any required setup.
uint8 TRANSITION_CONFIGURE = 1
# The node's callback onCleanup will be called in this transition to allow the
# node to load its configuration and conduct any required setup.
uint8 TRANSITION_CLEANUP = 2
# The node's callback onActivate will be executed to do any final preparations
# to start executing.
uint8 TRANSITION_ACTIVATE = 3
# The node's callback onDeactivate will be executed to do any cleanup to start
# executing, and reverse the onActivate changes.
uint8 TRANSITION_DEACTIVATE = 4
# This signals shutdown during an unconfigured state, the node's callback
# onShutdown will be executed to do any cleanup necessary before destruction.
uint8 TRANSITION_UNCONFIGURED_SHUTDOWN = 5
# This signals shutdown during an inactive state, the node's callback onShutdown
# will be executed to do any cleanup necessary before destruction.
uint8 TRANSITION_INACTIVE_SHUTDOWN = 6
# This signals shutdown during an active state, the node's callback onShutdown
# will be executed to do any cleanup necessary before destruction.
uint8 TRANSITION_ACTIVE_SHUTDOWN = 7
# This transition will simply cause the deallocation of the node.
uint8 TRANSITION_DESTROY = 8
# Reserved [10-69], private transitions
# These transitions are not publicly available and cannot be invoked by a user.
# The following transitions are implicitly invoked based on the callback
# feedback of the intermediate transition states.
uint8 TRANSITION_ON_CONFIGURE_SUCCESS = 10
uint8 TRANSITION_ON_CONFIGURE_FAILURE = 11
uint8 TRANSITION_ON_CONFIGURE_ERROR = 12
uint8 TRANSITION_ON_CLEANUP_SUCCESS = 20
uint8 TRANSITION_ON_CLEANUP_FAILURE = 21
uint8 TRANSITION_ON_CLEANUP_ERROR = 22
uint8 TRANSITION_ON_ACTIVATE_SUCCESS = 30
uint8 TRANSITION_ON_ACTIVATE_FAILURE = 31
uint8 TRANSITION_ON_ACTIVATE_ERROR = 32
uint8 TRANSITION_ON_DEACTIVATE_SUCCESS = 40
uint8 TRANSITION_ON_DEACTIVATE_FAILURE = 41
uint8 TRANSITION_ON_DEACTIVATE_ERROR = 42
uint8 TRANSITION_ON_SHUTDOWN_SUCCESS = 50
uint8 TRANSITION_ON_SHUTDOWN_FAILURE = 51
uint8 TRANSITION_ON_SHUTDOWN_ERROR = 52
uint8 TRANSITION_ON_ERROR_SUCCESS = 60
uint8 TRANSITION_ON_ERROR_FAILURE = 61
uint8 TRANSITION_ON_ERROR_ERROR = 62
# Reserved [90-99]. Transition callback success values.
# These return values ought to be set as a return value for each callback.
# Depending on which return value, the transition will be executed correctly or
# fallback/error callbacks will be triggered.
# The transition callback successfully performed its required functionality.
uint8 TRANSITION_CALLBACK_SUCCESS = 97
# The transition callback failed to perform its required functionality.
uint8 TRANSITION_CALLBACK_FAILURE = 98
# The transition callback encountered an error that requires special cleanup, if
# possible.
uint8 TRANSITION_CALLBACK_ERROR = 99
##
## Fields
##
# The transition id from above definitions.
uint8 id
# A text label of the transition.
string label
https://github.com/ros2/demos/tree/jazzy/lifecycle