navigation2基础-lifecycle_node 的使用

1 前言

由于ROS2没有提供杀死node方法，但是提供LifecycleNode，它允许对节点的生命周期进行详细管理。这种节点类似于状态机，可以在几种不同的状态之间切换。LifecycleNode的状态主要分为基本状态（Primary States）和切换状态（Transition States）。

1.1 基本状态

• 未配置（Unconfigured）：节点刚启动时的状态，未执行任何操作。
• 非活跃（Inactive）：节点持有资源但不执行任何操作，允许重新配置。
• 活跃（Active）：节点正常执行任务。
• 已完成（Finalized）：节点已被销毁，仅用于调试。

1.2 切换状态

• 配置（Configuring）：执行onConfigure()回调函数，加载配置和资源。

• 清理（CleaningUp）：执行onCleanup()回调函数，释放资源。

• 激活（Activating）：执行onActivate()回调函数，准备开始执行任务。

• 停用（Deactivating）：执行onDeactivate()回调函数，暂停执行任务。

• 关闭（ShuttingDown）：执行onShutdown()回调函数，进行销毁前的清理。

• 错误处理（ErrorProcessing）：执行onError()回调函数，处理错误。

2 写一个类继承nav2_util::LifecycleNode，实现状态的切换。

2.1 生成功能包：

ros2 pkg create ros_two_lifecycle_node --build-type ament_cmake --dependencies rclcpp rclcpp_lifecycle lifecycle_msgs nav2_util nav2_msgs std_msgs geometry_msgs  

生成配置文件：my_params.yaml

my_lifecycle_node:ros__parameters:publish_topic: "/print_log"subscribe_topic: "/cmd_vel"my_string_param: "world"

2.2 创建my_lifecycle_node.hpp文件

#ifndef MY_LIFECYCLE_PKG__MY_LIFECYCLE_NODE_HPP_
#define MY_LIFECYCLE_PKG__MY_LIFECYCLE_NODE_HPP_#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"namespace my_lifecycle_pkg
{
class MyLifecycleNode : public nav2_util::LifecycleNode
{
public:explicit MyLifecycleNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());~MyLifecycleNode();// 生命周期管理nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
private:// 定时器回调函数void timer_callback();  // 订阅者回调函数void subscription_callback(const geometry_msgs::msg::Twist::SharedPtr msg);// ROS2相关成员变量  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> publisher_;rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_;rclcpp::TimerBase::SharedPtr timer_;  // 计数器size_t count_;  // 话题名称std::string publish_topic_;std::string subscribe_topic_;  
};
}  // namespace my_lifecycle_pkg#endif  // MY_LIFECYCLE_PKG__MY_LIFECYCLE_NODE_HPP_

2.3 创建my_lifecycle_node.cpp文件

#include "ros_two_lifecycle_node/my_lifecycle_node.hpp"#include <chrono>
#include <memory>
#include <string>#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include <lifecycle_msgs/msg/state.hpp> 
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"using namespace std::chrono_literals;namespace my_lifecycle_pkg
{MyLifecycleNode::MyLifecycleNode(const rclcpp::NodeOptions & options)
: nav2_util::LifecycleNode("my_lifecycle_node", "", options),count_(0)
{// 声明参数this->declare_parameter("publish_topic", "/motion_state");this->declare_parameter("subscribe_topic", "/cmd_vel");// 获取参数值publish_topic_ = this->get_parameter("publish_topic").as_string();subscribe_topic_ = this->get_parameter("subscribe_topic").as_string();RCLCPP_INFO(this->get_logger(), "MyLifecycleNode constructed");
}MyLifecycleNode::~MyLifecycleNode()
{RCLCPP_INFO(this->get_logger(), "MyLifecycleNode destroyed");
}nav2_util::CallbackReturn MyLifecycleNode::on_configure(const rclcpp_lifecycle::State & state)
{(void)state;RCLCPP_INFO(this->get_logger(), "on_configure() called");// 创建发布者（生命周期发布者）publisher_ = this->create_publisher<std_msgs::msg::String>(publish_topic_, 10);// 创建订阅者subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(subscribe_topic_, 10,std::bind(&MyLifecycleNode::subscription_callback, this, std::placeholders::_1));// 创建定时器（但不立即启动）timer_ = this->create_wall_timer(1000ms, std::bind(&MyLifecycleNode::timer_callback, this));// 初始状态下停止定时器timer_->cancel();RCLCPP_INFO(this->get_logger(), "Configuration completed successfully");return nav2_util::CallbackReturn::SUCCESS;
}nav2_util::CallbackReturn MyLifecycleNode::on_activate(const rclcpp_lifecycle::State & state)
{(void)state;RCLCPP_INFO(this->get_logger(), "on_activate() called");// 激活发布者publisher_->on_activate();// 启动定时器timer_->reset();RCLCPP_INFO(this->get_logger(), "Node activated");return nav2_util::CallbackReturn::SUCCESS;
}nav2_util::CallbackReturn MyLifecycleNode::on_deactivate(const rclcpp_lifecycle::State & state)
{(void)state;RCLCPP_INFO(this->get_logger(), "on_deactivate() called");// 停止定时器timer_->cancel();// 停用发布者publisher_->on_deactivate();RCLCPP_INFO(this->get_logger(), "Node deactivated");return nav2_util::CallbackReturn::SUCCESS;
}nav2_util::CallbackReturn MyLifecycleNode::on_cleanup(const rclcpp_lifecycle::State & state)
{(void)state;RCLCPP_INFO(this->get_logger(), "on_cleanup() called");// 清理资源timer_.reset();publisher_.reset();subscription_.reset();count_ = 0;RCLCPP_INFO(this->get_logger(), "Node cleaned up");return nav2_util::CallbackReturn::SUCCESS;
}nav2_util::CallbackReturn MyLifecycleNode::on_shutdown(const rclcpp_lifecycle::State & state)
{(void)state;RCLCPP_INFO(this->get_logger(), "on_shutdown() called");// 关闭时清理资源timer_.reset();publisher_.reset();subscription_.reset();RCLCPP_INFO(this->get_logger(), "Node shutdown completed");return nav2_util::CallbackReturn::SUCCESS;
}void MyLifecycleNode::timer_callback()
{auto message = std_msgs::msg::String();message.data = "Hello, ROS2! Count: " + std::to_string(count_++);// 只有在激活状态下才发布消息if (publisher_->is_activated()) {publisher_->publish(message);RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());}
}void MyLifecycleNode::subscription_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{RCLCPP_INFO(this->get_logger(), "Received Twist: linear.x=%.2f, angular.z=%.2f", msg->linear.x, msg->angular.z);
}}  // namespace my_lifecycle_pkgint main(int argc, char **argv) 
{rclcpp::init(argc, argv);auto node = std::make_shared<my_lifecycle_pkg::MyLifecycleNode>();// 加载参数auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node);synchronous_client->load_parameters("/home/yahboom/ros2_ws/src/ros_two_lifecycle_node/config/my_params.yaml");nav2_util::CallbackReturn ret_;// 获取当前状态auto current_state = node->get_current_state();// 如果当前状态是 UNCONFIGURED，则配置节点if (current_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED){node->configure(ret_);if (ret_ != nav2_util::CallbackReturn::SUCCESS) {RCLCPP_ERROR(rclcpp::get_logger("main"), "Configure failed");return 1;}RCLCPP_INFO(node->get_logger(), "Configured successfully.");// 更新状态current_state = node->get_current_state();}// 如果当前状态是 INACTIVE，则激活节点if (current_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE){node->activate(ret_);if (ret_ != nav2_util::CallbackReturn::SUCCESS) {RCLCPP_ERROR(rclcpp::get_logger("main"), "Activate failed");return 1;}RCLCPP_INFO(node->get_logger(), "Activated successfully.");// 更新状态current_state = node->get_current_state();}// 等待一段时间RCLCPP_INFO(node->get_logger(), "Node running for 10 seconds...");std::this_thread::sleep_for(std::chrono::seconds(10));// 正确的状态转换顺序：active -> inactive -> unconfigured -> finalizedif (current_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE){// 先停用node->deactivate(ret_);if (ret_ != nav2_util::CallbackReturn::SUCCESS) {RCLCPP_ERROR(rclcpp::get_logger("main"), "Deactivate failed");return 1;}RCLCPP_INFO(node->get_logger(), "Deactivated successfully.");// 更新状态current_state = node->get_current_state();}if (current_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE){// 再清理node->cleanup(ret_);if (ret_ != nav2_util::CallbackReturn::SUCCESS) {RCLCPP_ERROR(rclcpp::get_logger("main"), "Cleanup failed");return 1;}RCLCPP_INFO(node->get_logger(), "Cleaned up successfully.");// 更新状态current_state = node->get_current_state();}if (current_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED){// 最后关闭node->shutdown(ret_);if (ret_ != nav2_util::CallbackReturn::SUCCESS) {RCLCPP_ERROR(rclcpp::get_logger("main"), "Shutdown failed");return 1;}RCLCPP_INFO(node->get_logger(), "Shutdown completed successfully.");}rclcpp::shutdown();return 0;
}

2.4 配置 CMakeLists.txt
修改 CMakeLists.txt 文件：

include_directories(include ${rclcpp_INCLUDE_DIRS}
) set(dependenciesrclcpp std_msgs rclcpp_lifecycle geometry_msgs lifecycle_msgs nav2_utilnav2_msgs
)add_executable(lifecycle_node src/my_lifecycle_node.cpp)
ament_target_dependencies(lifecycle_node ${dependencies})
install(TARGETS lifecycle_nodeDESTINATION lib/${PROJECT_NAME})

3.编译并执行

编译

colcon build --packages-select ros_two_lifecycle_node

执行：

source install/local_setup.sh
ros2 run ros_two_lifecycle_node lifecycle_node

执行结果：