首页 > 其他分享 >nav2的behavior模块，controller模块，lifecycle_manager模块

nav2的behavior模块，controller模块，lifecycle_manager模块

时间：2024-09-26 13:20:34浏览次数：3  
标签：node 模块 cmd manager 节点 controller behavior vel lifecycle

Navigation2-main 项目中 nav2_behaviors、controller 和 lifecycle_manager 文件夹的源代码详解
在前述章节中，我们已经详细介绍了 constrained_smoother、collision_monitor、mppi_controller、dwb_controller、rotation_shim 和 core 模块。本节将继续介绍 navigation2-main 项目中的三个重要模块：nav2_behaviors、controller 和 lifecycle_manager。我们将深入解析这些模块的目录结构、核心功能、主要类和函数，帮助您全面理解和使用这些组件。

  1. nav2_behaviors
    nav2_behaviors 模块负责实现机器人在导航过程中可能执行的各种行为，如旋转、直行、避障等。这些行为是导航控制器执行复杂动作的基础，为机器人提供了灵活的运动能力。
    目录结构概述
    nav2_behaviors/
    ├── include/
    │ └── nav2_behaviors/
    │ ├── navigate_rotate.hpp
    │ ├── navigate_straight.hpp
    │ ├── behavior_base.hpp
    │ └── …
    ├── src/
    │ ├── navigate_rotate.cpp
    │ ├── navigate_straight.cpp
    │ ├── behavior_base.cpp
    │ └── …
    ├── CMakeLists.txt
    └── package.xml
    详细说明
    1.1 include/nav2_behaviors/
    此目录包含了 nav2_behaviors 模块的头文件，定义了各种导航行为的接口和基类。
    ● behavior_base.hpp定义了所有导航行为的基类 BehaviorBase，为具体行为类提供统一的接口和基础功能。
    ● navigate_rotate.hpp定义了 NavigateRotate 类，负责实现机器人的旋转行为。
    ● navigate_straight.hpp定义了 NavigateStraight 类，负责实现机器人的直行行为。
    1.2 src/
    此目录包含了 nav2_behaviors 模块的源代码，实现了各类导航行为的具体逻辑。
    1.2.1 behavior_base.cpp
    功能概述：
    BehaviorBase 类是所有导航行为的基类，提供了配置、执行和取消行为的基础接口，确保不同行为之间的一致性和可扩展性。
    关键方法：
    ● configure(): 配置行为，包括加载参数和初始化资源。
    ● execute(): 执行具体的导航行为。
    ● cancel(): 取消正在执行的行为。
    示例代码：
    #pragma once

#include “rclcpp/rclcpp.hpp”

namespace nav2_behaviors
{

class BehaviorBase
{
public:
BehaviorBase();
virtual ~BehaviorBase();

virtual void configure(const rclcpp::Node::SharedPtr & node, const std::string & name) = 0;
virtual void execute() = 0;
virtual void cancel() = 0;

protected:
rclcpp::Node::SharedPtr node_;
std::string name_;
};

} // namespace nav2_behaviors
1.2.2 navigate_rotate.cpp
功能概述：
NavigateRotate 类实现了机器人的旋转行为，通过控制机器人的角速度，使其旋转到指定的角度。
关键方法：
● configure(): 配置旋转行为，加载旋转速度和目标角度参数。
● execute(): 执行旋转动作，发布旋转速度指令直到达到目标角度。
● cancel(): 取消旋转动作，停止发布速度指令。
示例代码：
#include “nav2_behaviors/navigate_rotate.hpp”
#include “geometry_msgs/msg/twist.hpp”

namespace nav2_behaviors
{

NavigateRotate::NavigateRotate()
target_angle_(0.0), rotation_speed_(0.5), rotating_(false)
{
}

NavigateRotate::~NavigateRotate()
{
}

void NavigateRotate::configure(const rclcpp::Node::SharedPtr & node, const std::string & name)
{
node_ = node;
name_ = name;

node_->declare_parameter(“rotation_speed”, rotation_speed_);
node_->declare_parameter(“target_angle”, target_angle_);

node_->get_parameter(“rotation_speed”, rotation_speed_);
node_->get_parameter(“target_angle”, target_angle_);

cmd_vel_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>(“/cmd_vel”, 10);
}

void NavigateRotate::execute()
{
rotating_ = true;
double current_angle = getCurrentYaw();
double angle_diff = normalizeAngle(target_angle_ - current_angle);

geometry_msgs::msg::Twist cmd_vel;
cmd_vel.angular.z = (angle_diff > 0) ? rotation_speed_ : -rotation_speed_;

RCLCPP_INFO(node_->get_logger(), “开始旋转到目标角度: %.2f rad”, target_angle_);

while (rotating_ && std::abs(angle_diff) > 0.01) { // 0.01弧度容差
cmd_vel_pub_->publish(cmd_vel);
rclcpp::sleep_for(std::chrono::milliseconds(100));

current_angle = getCurrentYaw();
angle_diff = normalizeAngle(target_angle_ - current_angle);
cmd_vel.angular.z = (angle_diff > 0) ? rotation_speed_ : -rotation_speed_;

}

// 停止旋转
cmd_vel.angular.z = 0.0;
cmd_vel_pub_->publish(cmd_vel);

rotating_ = false;
RCLCPP_INFO(node_->get_logger(), “已到达目标角度: %.2f rad”, target_angle_);
}

void NavigateRotate::cancel()
{
rotating_ = false;
geometry_msgs::msg::Twist cmd_vel;
cmd_vel.angular.z = 0.0;
cmd_vel_pub_->publish(cmd_vel);
RCLCPP_WARN(node_->get_logger(), “旋转行为已取消”);
}

double NavigateRotate::getCurrentYaw()
{
// 获取当前机器人的YAW角度，实际实现需订阅/odom或其他位姿主题
// 这里使用示例值
return 0.0;
}

double NavigateRotate::normalizeAngle(double angle)
{
while (angle > M_PI)
angle -= 2.0 * M_PI;
while (angle < -M_PI)
angle += 2.0 * M_PI;
return angle;
}

} // namespace nav2_behaviors
1.2.3 navigate_straight.cpp
功能概述：
NavigateStraight 类实现了机器人的直行行为，通过控制机器人的线速度，使其沿直线前进指定的距离或时间。
关键方法：
● configure(): 配置直行行为，加载线速度和前进时间/距离参数。
● execute(): 执行直行动作，发布线速度指令直到达到目标。
● cancel(): 取消直行动作，停止发布速度指令。
示例代码：
#include “nav2_behaviors/navigate_straight.hpp”
#include “geometry_msgs/msg/twist.hpp”

namespace nav2_behaviors
{

NavigateStraight::NavigateStraight()
target_distance_(1.0), linear_speed_(0.5), moving_(false), start_time_(0.0)
{
}

NavigateStraight::~NavigateStraight()
{
}

void NavigateStraight::configure(const rclcpp::Node::SharedPtr & node, const std::string & name)
{
node_ = node;
name_ = name;

node_->declare_parameter(“linear_speed”, linear_speed_);
node_->declare_parameter(“target_distance”, target_distance_);

node_->get_parameter(“linear_speed”, linear_speed_);
node_->get_parameter(“target_distance”, target_distance_);

cmd_vel_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>(“/cmd_vel”, 10);
}

void NavigateStraight::execute()
{
moving_ = true;
double current_distance = 0.0;
start_time_ = node_->now().seconds();

geometry_msgs::msg::Twist cmd_vel;
cmd_vel.linear.x = linear_speed_;

RCLCPP_INFO(node_->get_logger(), “开始直行，目标距离: %.2f 米”, target_distance_);

while (moving_ && current_distance < target_distance_) {
cmd_vel_pub_->publish(cmd_vel);
rclcpp::sleep_for(std::chrono::milliseconds(100));

double current_time = node_->now().seconds();
current_distance = linear_speed_ * (current_time - start_time_);

}

// 停止移动
cmd_vel.linear.x = 0.0;
cmd_vel_pub_->publish(cmd_vel);

moving_ = false;
RCLCPP_INFO(node_->get_logger(), “已到达目标距离: %.2f 米”, target_distance_);
}

void NavigateStraight::cancel()
{
moving_ = false;
geometry_msgs::msg::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel_pub_->publish(cmd_vel);
RCLCPP_WARN(node_->get_logger(), “直行行为已取消”);
}

} // namespace nav2_behaviors
1.3 关键类和接口
1.3.1 BehaviorBase
BehaviorBase 类作为所有导航行为的基类，提供了统一的接口和基础功能，确保不同行为类之间的一致性和可扩展性。
主要成员变量：
● node_: 指向ROS节点的共享指针，用于参数获取和日志记录。
● name_: 行为的名称，用于配置和管理。
主要功能：

  1. 配置：
    ○ 加载并初始化行为所需的参数。
    ○ 设置ROS接口，如发布器和订阅器。
  2. 行为执行：
    ○ 处理具体行为的执行逻辑，通过继承实现不同的行为。
  3. 行为取消：
    ○ 提供取消行为的接口，以便在需要时中断当前执行的行为。
    1.3.2 NavigateRotate
    NavigateRotate 类实现了机器人旋转的行为，通过控制机器人发布旋转速度指令，使其旋转到指定的目标角度。
    主要成员变量：
    ● target_angle_: 目标旋转角度（弧度）。
    ● rotation_speed_: 旋转速度（弧度/秒）。
    ● rotating_: 标志变量，指示是否正在旋转。
    ● cmd_vel_pub_: ROS发布器，用于发布速度指令。
    主要功能：
  4. 初始化与配置：
    ○ 加载旋转相关参数，如目标角度和旋转速度。
    ○ 设置ROS发布器，用于发布旋转控制指令。
  5. 旋转执行：
    ○ 计算当前角度与目标角度的差值。
    ○ 发布相应的旋转速度指令，直到达到目标角度。
    ○ 停止旋转，确保机器人不再继续旋转。
  6. 行为取消：
    ○ 停止发布旋转速度指令，停止旋转动作。
    1.3.3 NavigateStraight
    NavigateStraight 类实现了机器人直行的行为，通过控制机器人发布线速度指令，使其沿直线前进指定的距离或时间。
    主要成员变量：
    ● target_distance_: 目标前进距离（米）。
    ● linear_speed_: 线速度（米/秒）。
    ● moving_: 标志变量，指示是否正在直行。
    ● start_time_: 行为开始的时间点。
    ● cmd_vel_pub_: ROS发布器，用于发布速度指令。
    主要功能：
  7. 初始化与配置：
    ○ 加载直行相关参数，如目标距离和线速度。
    ○ 设置ROS发布器，用于发布直行动作的速度指令。
  8. 直行动作执行：
    ○ 计算已移动的距离。
    ○ 发布相应的线速度指令，直到达到目标距离。
    ○ 停止移动，确保机器人不再继续前进。
  9. 行为取消：
    ○ 停止发布线速度指令，停止直行动作。
    1.4 使用示例
    以下是如何集成和使用 nav2_behaviors 模块的示例，展示如何配置和执行旋转与直行行为：
    #include “nav2_behaviors/navigate_rotate.hpp”
    #include “nav2_behaviors/navigate_straight.hpp”
    #include “rclcpp/rclcpp.hpp”

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_sharedrclcpp::Node(“behaviors_example_node”);

// 初始化旋转行为
nav2_behaviors::NavigateRotate rotate_behavior;
rotate_behavior.configure(node, “rotate_behavior”);

// 初始化直行行为
nav2_behaviors::NavigateStraight straight_behavior;
straight_behavior.configure(node, “straight_behavior”);

// 执行旋转行为
double target_angle = M_PI / 2; // 90度
rotate_behavior.execute();

// 执行直行行为
double target_distance = 2.0; // 2米
straight_behavior.execute();

rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
2. controller
controller 模块负责实现机器人导航过程中的控制逻辑，包括速度指令的生成与执行。该模块通常与局部规划器紧密结合，通过处理来自局部规划器的路径信息，生成实际的运动控制指令，指导机器人沿路径移动。
目录结构概述
controller/
├── include/
│ └── nav2_controller/
│ ├── controller_base.hpp
│ ├── velocity_smoother.hpp
│ ├── controller_server.hpp
│ └── …
├── src/
│ ├── controller_base.cpp
│ ├── velocity_smoother.cpp
│ ├── controller_server.cpp
│ └── …
├── CMakeLists.txt
└── package.xml
详细说明
2.1 include/nav2_controller/
此目录包含了 controller 模块的头文件，定义了控制器的基类、速度平滑器和控制服务器等组件的接口。
● controller_base.hpp定义了 ControllerBase 类，作为所有控制器的基类，提供统一的接口和基础功能。
● velocity_smoother.hpp定义了 VelocitySmoother 类，用于平滑控制指令，确保速度指令的连续性和稳定性。
● controller_server.hpp定义了 ControllerServer 类，负责与ROS系统交互，接收规划器的路径指令并生成控制指令。
2.2 src/
此目录包含了 controller 模块的源代码，实现了各类控制器组件的具体逻辑。
2.2.1 controller_base.cpp
功能概述：
ControllerBase 类作为所有控制器的基类，提供了统一的接口和基础功能，确保不同控制器之间的一致性和可扩展性。
关键方法：
● configure(): 配置控制器，包括加载参数和初始化资源。
● computeControl(): 计算并返回控制指令。
● cancel(): 取消当前控制指令的执行。
示例代码：
#pragma once

#include “rclcpp/rclcpp.hpp”
#include “geometry_msgs/msg/twist.hpp”

namespace nav2_controller
{

class ControllerBase
{
public:
ControllerBase();
virtual ~ControllerBase();

virtual void configure(const rclcpp::Node::SharedPtr & node, const std::string & name) = 0;
virtual geometry_msgs::msg::Twist computeControl() = 0;
virtual void cancel() = 0;

protected:
rclcpp::Node::SharedPtr node_;
std::string name_;
};

} // namespace nav2_controller
2.2.2 velocity_smoother.cpp
功能概述：
VelocitySmoother 类实现了控制指令的平滑处理，通过限制速度变化率和加速度，确保机器人的运动指令平稳且符合物理限制，避免运动过程中的震动和不稳定。
关键方法：
● smooth(): 对输入的速度指令进行平滑处理，返回平滑后的速度指令。
● setParameters(): 设置平滑参数，如最大加速度和速度变化率。
示例代码：
#include “nav2_controller/velocity_smoother.hpp”

namespace nav2_controller
{

VelocitySmoother::VelocitySmoother()
max_acceleration_(0.5), max_jerk_(1.0), prev_cmd_vel_({0.0, 0.0, 0.0})
{
}

VelocitySmoother::~VelocitySmoother()
{
}

void VelocitySmoother::configure(const rclcpp::Node::SharedPtr & node, const std::string & name)
{
node_ = node;
name_ = name;

node_->declare_parameter(“max_acceleration”, max_acceleration_);
node_->declare_parameter(“max_jerk”, max_jerk_);

node_->get_parameter(“max_acceleration”, max_acceleration_);
node_->get_parameter(“max_jerk”, max_jerk_);
}

geometry_msgs::msg::Twist VelocitySmoother::smooth(const geometry_msgs::msg::Twist & cmd_vel)
{
geometry_msgs::msg::Twist smooth_vel = cmd_vel;

// 计算线速度变化
double delta_vx = cmd_vel.linear.x - prev_cmd_vel_.linear.x;
double delta_vy = cmd_vel.linear.y - prev_cmd_vel_.linear.y;
double delta_wz = cmd_vel.angular.z - prev_cmd_vel_.angular.z;

// 限制加速度
if (std::abs(delta_vx) > max_acceleration_) {
smooth_vel.linear.x = prev_cmd_vel_.linear.x + ((delta_vx > 0) ? max_acceleration_ : -max_acceleration_);
}
if (std::abs(delta_vy) > max_acceleration_) {
smooth_vel.linear.y = prev_cmd_vel_.linear.y + ((delta_vy > 0) ? max_acceleration_ : -max_acceleration_);
}
if (std::abs(delta_wz) > max_acceleration_) {
smooth_vel.angular.z = prev_cmd_vel_.angular.z + ((delta_wz > 0) ? max_acceleration_ : -max_acceleration_);
}

// 更新前一次速度指令
prev_cmd_vel_ = smooth_vel;

return smooth_vel;
}

} // namespace nav2_controller
2.2.3 controller_server.cpp
功能概述：
ControllerServer 类是控制器模块的核心，负责与ROS系统交互，接收来自局部规划器的路径指令，调用控制器计算控制指令，并将其发布到ROS主题以驱动机器人。
关键方法：
● configure(): 配置控制器服务器，初始化发布器和订阅器。
● updateControl(): 处理路径指令，计算控制指令并发布。
● shutdown(): 关闭控制器服务器，清理资源。
示例代码：
#include “nav2_controller/controller_server.hpp”
#include “nav2_controller/velocity_smoother.hpp”

namespace nav2_controller
{

ControllerServer::ControllerServer()
smoother_(std::make_shared()), active_(false)
{
}

ControllerServer::~ControllerServer()
{
shutdown();
}

void ControllerServer::configure(const rclcpp::Node::SharedPtr & node, const std::string & name)
{
node_ = node;
name_ = name;

// 初始化速度平滑器
smoother_->configure(node_, name_ + “.velocity_smoother”);

// 初始化控制指令发布器
cmd_vel_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>(“/cmd_vel”, 10);

// 订阅局部规划器的路径指令
path_sub_ = node_->create_subscription<nav_msgs::msg::Path>(
“/local_planner/path”, 10,
std::bind(&ControllerServer::pathCallback, this, std::placeholders::_1));

active_ = true;
}

void ControllerServer::pathCallback(const nav_msgs::msg::Path::SharedPtr msg)
{
if (!active_) {
return;
}

// 从路径中提取目标速度，实际实现可能更复杂
geometry_msgs::msg::Twist goal_vel;
if (!msg->poses.empty()) {
// 假设最后一个路径点的速度作为目标速度
// 实际情况需根据全局规划器和局部规划器的实现调整
// 这里使用示例值
goal_vel.linear.x = 0.5;
goal_vel.angular.z = 0.1;
}

// 计算控制指令
geometry_msgs::msg::Twist cmd_vel = smoother_->smooth(goal_vel);

// 发布控制指令
cmd_vel_pub_->publish(cmd_vel);
}

geometry_msgs::msg::Twist ControllerServer::computeControl()
{
// 这里可以实现更复杂的控制逻辑
// 目前通过回调直接发布控制指令
geometry_msgs::msg::Twist cmd_vel;
return cmd_vel;
}

void ControllerServer::shutdown()
{
if (active_) {
path_sub_.reset();
cmd_vel_pub_.reset();
active_ = false;
}
}

} // namespace nav2_controller
2.3 关键类和接口
2.3.1 ControllerBase
ControllerBase 类作为所有控制器的基类，提供了统一的接口和基础功能，确保不同控制器之间的一致性和可扩展性。
主要成员变量：
● node_: 指向ROS节点的共享指针，用于参数获取和日志记录。
● name_: 控制器的名称，用于配置和管理。
主要功能：

  1. 配置：
    ○ 加载并初始化控制器所需的参数。
    ○ 设置ROS接口，如发布器和订阅器。
  2. 控制指令计算：
    ○ 根据当前机器人状态和目标状态，计算并返回控制指令。
    ○ 提供接口以实现不同的控制策略。
  3. 行为取消：
    ○ 提供取消控制指令的接口，以便在需要时中断当前执行的控制动作。
    2.3.2 VelocitySmoother
    VelocitySmoother 类实现了控制指令的平滑处理，通过限制速度变化率和加速度，确保速度指令的连续性和稳定性，避免运动过程中的震动和不稳定。
    主要成员变量：
    ● max_acceleration_: 最大允许的加速度（米/秒²）。
    ● max_jerk_: 最大允许的加速度变化率（米/秒³）。
    ● prev_cmd_vel_: 前一次发布的速度指令，用于计算速度变化。
    ● node_: 指向ROS节点的共享指针，用于参数获取和日志记录。
    主要功能：
  4. 初始化与配置：
    ○ 加载速度平滑相关参数，如最大加速度和速度变化率。
    ○ 设置ROS接口，如发布器和订阅器。
  5. 速度指令平滑：
    ○ 接收目标速度指令，计算与前一次速度指令的差值。
    ○ 限制速度变化率和加速度，生成平滑后的速度指令。
    ○ 更新前一次速度指令，以便下一次平滑处理。
    2.3.3 ControllerServer
    ControllerServer 类是控制器模块的核心，负责与ROS系统交互，接收来自局部规划器的路径指令，调用控制器计算控制指令，并将其发布到ROS主题以驱动机器人。
    主要成员变量：
    ● node_: 指向ROS节点的共享指针，用于参数获取和日志记录。
    ● name_: 控制器服务器的名称，用于配置和管理。
    ● smoother_: 指向 VelocitySmoother 类实例的共享指针，负责速度平滑处理。
    ● cmd_vel_pub_: ROS发布器，用于发布控制指令。
    ● path_sub_: ROS订阅器，用于接收局部规划器的路径指令。
    ● active_: 标志变量，指示控制器服务器是否处于活动状态。
    主要功能：
  6. 初始化与配置：
    ○ 加载并初始化控制器参数。
    ○ 初始化速度平滑器。
    ○ 设置ROS发布器和订阅器，用于发布控制指令和接收路径指令。
  7. 控制指令处理：
    ○ 接收来自局部规划器的路径指令。
    ○ 调用速度平滑器处理目标速度指令，生成平滑后的控制指令。
    ○ 发布控制指令至ROS主题，以驱动机器人运动。
  8. 控制器服务器管理：
    ○ 管理控制器服务器的生命周期，包括启动、运行和关闭。
    ○ 提供接口进行资源清理和行为取消。
    2.4 使用示例
    以下是如何集成和使用 controller 模块的示例，展示如何配置和执行控制指令：
    #include “nav2_controller/controller_server.hpp”
    #include “nav2_controller/velocity_smoother.hpp”
    #include “rclcpp/rclcpp.hpp”

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_sharedrclcpp::Node(“controller_example_node”);

// 初始化控制器服务器
nav2_controller::ControllerServer controller_server;
controller_server.configure(node, “controller_server”);

rclcpp::spin(node);

// 关闭控制器服务器
controller_server.shutdown();
rclcpp::shutdown();
return 0;
}
3. lifecycle_manager
lifecycle_manager 模块负责管理ROS节点的生命周期，协调多个ROS节点的启动、运行和关闭，确保整个导航系统的稳定性和一致性。通过管理节点的生命周期，lifecycle_manager 提供了机制来控制节点的初始化、激活、停用和清理过程。
目录结构概述
lifecycle_manager/
├── include/
│ └── lifecycle_manager/
│ ├── lifecycle_manager_client.hpp
│ ├── lifecycle_manager.hpp
│ └── …
├── src/
│ ├── lifecycle_manager_client.cpp
│ ├── lifecycle_manager.cpp
│ └── …
├── CMakeLists.txt
└── package.xml
详细说明
3.1 include/lifecycle_manager/
此目录包含了 lifecycle_manager 模块的头文件，定义了生命周期管理器和客户端的接口。
● lifecycle_manager.hpp定义了 LifecycleManager 类，负责管理多个ROS节点的生命周期状态。
● lifecycle_manager_client.hpp定义了 LifecycleManagerClient 类，作为其他节点与生命周期管理器交互的客户端接口。
3.2 src/
此目录包含了 lifecycle_manager 模块的源代码，实现了生命周期管理器和客户端的具体逻辑。
3.2.1 lifecycle_manager.cpp
功能概述：
LifecycleManager 类负责管理多个ROS节点的生命周期状态，通过协调节点的初始化、激活、停用和清理过程，确保整个系统的稳定运行。
关键方法：
● addNode(): 添加需要管理生命周期的ROS节点。
● activateNodes(): 激活所有管理的节点。
● deactivateNodes(): 停用所有管理的节点。
● shutdownNodes(): 清理并关闭所有管理的节点。
示例代码：
#pragma once

#include “rclcpp/rclcpp.hpp”
#include “nav2_util/lifecycle_service_client.hpp”
#include
#include

namespace lifecycle_manager
{

class LifecycleManager : public rclcpp::Node
{
public:
LifecycleManager();
~LifecycleManager();

void addNode(const std::string & node_name);
void activateNodes();
void deactivateNodes();
void shutdownNodes();

private:
std::vector<nav2_util::LifecycleServiceClient::SharedPtr> managed_nodes_;
};

} // namespace lifecycle_manager
#include “lifecycle_manager/lifecycle_manager.hpp”
#include “rclcpp/rclcpp.hpp”
#include “lifecycle_msgs/msg/state.hpp”

namespace lifecycle_manager
{

LifecycleManager::LifecycleManager()
Node(“lifecycle_manager”)
{
// 在此可以添加参数加载和其他初始化操作
}

LifecycleManager::~LifecycleManager()
{
shutdownNodes();
}

void LifecycleManager::addNode(const std::string & node_name)
{
auto client = std::make_shared<nav2_util::LifecycleServiceClient>(node_, node_name);
managed_nodes_.push_back(client);
}

void LifecycleManager::activateNodes()
{
for (auto & client : managed_nodes_) {
if (client->getState().id != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
client->activate();
}
}
}

void LifecycleManager::deactivateNodes()
{
for (auto & client : managed_nodes_) {
if (client->getState().id == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
client->deactivate();
}
}
}

void LifecycleManager::shutdownNodes()
{
for (auto & client : managed_nodes_) {
client->shutdown();
}
}

} // namespace lifecycle_manager
3.2.2 lifecycle_manager_client.cpp
功能概述：
LifecycleManagerClient 类作为其他节点与 LifecycleManager 交互的客户端接口，提供了添加节点、激活节点、停用节点和关闭节点的功能。
关键方法：
● initialize(): 初始化生命周期管理客户端，与 LifecycleManager 建立连接。
● addManagedNode(): 向 LifecycleManager 添加需要管理生命周期的节点。
示例代码：
#pragma once

#include “rclcpp/rclcpp.hpp”

namespace lifecycle_manager
{

class LifecycleManagerClient
{
public:
LifecycleManagerClient(const rclcpp::Node::SharedPtr & node);
~LifecycleManagerClient();

void addManagedNode(const std::string & node_name);
void activateAll();
void deactivateAll();
void shutdownAll();

private:
rclcpp::Node::SharedPtr node_;
std::vectorstd::string nodes_to_manage_;
};

} // namespace lifecycle_manager
#include “lifecycle_manager/lifecycle_manager_client.hpp”
#include “lifecycle_manager/lifecycle_manager.hpp”

namespace lifecycle_manager
{

LifecycleManagerClient::LifecycleManagerClient(const rclcpp::Node::SharedPtr & node)
node_(node)
{
}

LifecycleManagerClient::~LifecycleManagerClient()
{
}

void LifecycleManagerClient::addManagedNode(const std::string & node_name)
{
// 通过服务调用或其他机制，将节点名称添加到LifecycleManager中
// 这里假设LifecycleManager已经在同一进程中运行
// 实际实现可能需要IPC或服务调用
}

void LifecycleManagerClient::activateAll()
{
// 调用LifecycleManager的激活方法
}

void LifecycleManagerClient::deactivateAll()
{
// 调用LifecycleManager的停用方法
}

void LifecycleManagerClient::shutdownAll()
{
// 调用LifecycleManager的关闭方法
}

} // namespace lifecycle_manager
3.3 关键类和接口
3.3.1 LifecycleManager
LifecycleManager 类负责管理多个ROS节点的生命周期状态，通过协调节点的初始化、激活、停用和清理过程，确保整个系统的稳定运行。
主要成员变量：
● managed_nodes_: 存储所有被管理的节点的生命周期服务客户端。
主要功能：

  1. 节点管理：
    ○ 添加需要管理生命周期的节点。
    ○ 启动和关闭节点的生命周期状态。
  2. 生命周期控制：
    ○ 激活所有管理的节点，使其进入活动状态。
    ○ 停用所有管理的节点，使其进入不活动状态。
    ○ 关闭所有管理的节点，进行清理和资源释放。
    3.3.2 LifecycleManagerClient
    LifecycleManagerClient 类作为其他节点与 LifecycleManager 交互的客户端接口，提供了添加节点、激活节点、停用节点和关闭节点的功能。
    主要成员变量：
    ● node_: 指向ROS节点的共享指针，用于与 LifecycleManager 交互。
    ● nodes_to_manage_: 存储需要管理生命周期的节点名称列表。
    主要功能：
  3. 初始化与配置：
    ○ 初始化生命周期管理客户端，与 LifecycleManager 建立连接。
  4. 节点管理接口：
    ○ 向 LifecycleManager 添加需要管理生命周期的节点。
    ○ 提供接口以激活、停用和关闭所有管理的节点。
    3.3.3 LifecycleManager
    LifecycleManager 类是生命周期管理的核心，负责统一管理多个ROS节点的生命周期状态，通过协调节点的初始化、激活、停用和清理过程，确保整个导航系统的稳定性和一致性。
    主要成员变量：
    ● managed_nodes_: 存储所有被管理的节点的生命周期服务客户端。
    主要功能：
  5. 节点添加与管理：
    ○ 允许动态添加需要管理生命周期的节点。
  6. 生命周期状态控制：
    ○ 激活所有管理的节点，使其进入活动状态。
    ○ 停用所有管理的节点，使其进入不活动状态。
    ○ 关闭所有管理的节点，进行资源清理和释放。
  7. 资源管理：
    ○ 保证在系统关闭或重新配置时，所有管理的节点能够正确地进行生命周期转换，避免资源泄漏和不一致状态。
    3.4 使用示例
    以下是如何集成和使用 lifecycle_manager 模块的示例，展示如何配置和管理多个ROS节点的生命周期：
    #include “lifecycle_manager/lifecycle_manager.hpp”
    #include “nav2_core/planner_base.hpp”
    #include “nav2_core/controller.hpp”
    #include “rclcpp/rclcpp.hpp”

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_sharedrclcpp::Node(“lifecycle_manager_example_node”);

// 初始化生命周期管理器
lifecycle_manager::LifecycleManager lifecycle_manager;

// 添加需要管理生命周期的节点
lifecycle_manager.addNode(“planner_server”);
lifecycle_manager.addNode(“controller_server”);
lifecycle_manager.addNode(“costmap_2d”);

// 激活所有节点
lifecycle_manager.activateNodes();

rclcpp::spin(node);

// 关闭所有节点
lifecycle_manager.shutdownNodes();
rclcpp::shutdown();
return 0;
}
总结
nav2_behaviors、controller 和 lifecycle_manager 是 navigation2-main 项目中至关重要的三个模块，分别负责实现各类导航行为、控制逻辑和生命周期管理。通过详细解析这三个模块的文件夹结构和源代码实现，我们可以更深入地理解Navigation2的工作原理和设计理念。
关键点回顾

  1. nav2_behaviors 模块：
    ○ 导航行为实现：提供了多种导航行为，如旋转和直行，帮助机器人执行复杂的运动任务。
    ○ 基类与继承：通过基类 BehaviorBase 提供统一的接口，确保不同导航行为的一致性和可扩展性。
    ○ ROS接口集成：与ROS系统紧密结合，通过发布速度指令控制机器人运动。
  2. controller 模块：
    ○ 控制逻辑实现：负责生成和执行控制指令，确保机器人沿预定路径平稳移动。
    ○ 速度平滑处理：通过 VelocitySmoother 类实现控制指令的平滑处理，提升运动的稳定性。
    ○ ROS接口整合：通过订阅路径指令和发布控制指令，协调局部规划器与机器人运动。
  3. lifecycle_manager 模块：
    ○ 生命周期管理：统一管理多个ROS节点的生命周期状态，确保系统的稳定性和一致性。
    ○ 节点协调：通过 LifecycleManager 类协调节点的初始化、激活、停用和关闭过程。
    ○ 资源管理：避免资源泄漏和不一致状态，通过生命周期管理确保系统高效运行。
    通过对这三个模块的深入理解，开发者可以更有效地定制和优化导航系统，满足不同应用场景下的需求。同时，这种模块化和分层设计也为系统的扩展和维护提供了坚实的基础。
    如果需要进一步了解具体的实现细节和高级功能，建议参考各模块的源代码和官方文档，结合实际项目需求进行深入学习和实践。

标签：node,模块,cmd,manager,节点,controller,behavior,vel,lifecycle
From： https://blog.csdn.net/u011027104/article/details/142521079

相关文章

  • Android平台RTMP推送模块的设计意义
    为什么要做RTMP推送RTMP是一种广泛使用的流媒体传输协议，它允许视频和音频数据在互联网上实时、高效地传输。实现RTMP推送功能，主要是为了满足以下需求：实时性要求：RTMP协议具有低延迟的特点，适合用于需要实时交互的场景，如直播、视频会议等。通过RTMP推送，观众可以几乎实时地观看到主播的......
  • 致远OA公文管理模块相关操作
    目录1、系统模块管理-开启公文2、角色权限分配-分配管理员3、公文插件参数修改4、公文文单制作5、配置发文单6、配置收文单7、配置签报单8、配置签收单9、公文管理-文单设置功能按钮介绍10、模板管理11、节点权限12、节点权限——续办13、印章管理14、文号管理—......
  • 【Java】@RestController 和@Controller的区别
    二者都是定义控制器的注解，它们的区别如下：（1）返回值不同。@RestController注解相当于@Controller和@ResponseBody的结合。其中，@ResponseBody是一个针对方法返回值进行处理的注解，处理后的RestController注解会将所有处理请求的方法默认解析为将方法返回值直接作为响应体内容返回，......
  • 搭建jforum报错zeroDateTimeBehavior CONVERT_TO_NULL convertToNull
    今天搭建项目的时候，本来马上就搭好了，结果在安装的时候报错："Theconnectionproperty‘zeroDateTimeBehavior’acceptablevaluesare:‘CONVERT_TO_NULL’,‘EXCEPTION’or‘ROUND’.Thevalue‘convertToNull’isnotacceptable."照着网上的教程找了好久都不行，最后自......
  • 06 常用内置模块总结
    -其他需背会len获取长度openrange随机生成数id是比较内存地址is/==是进行比较type获取数据类型输入输出printinput强制转换dict()list()tuple()int()str()bool()set()数学相关abs，绝对值v=abs(-1)print(v)float，转换成浮点型（小数）v=55v1=......
  • Exadata中的Infiniband交换机，更换完SSD模块后，交换机反复重启
    1.故障描述客户的ExadataX5-2, 某天收到EM13c告警，提示Exadata中的Infiniband交换机出现故障。 2.故障处理2.1 执行showunhealthy命令后，显示告警信息。#showunhealthyWARNINGFlashdiskhasbadblocks.FALURE-1sensorsNOTOK从命令输出可以看了，该Infiniband......
  • OMRON欧姆龙通讯模块CI541V1
    OMRON欧姆龙通讯模块CI541V1是一款在工业自动化和控制系统中广泛应用的设备，其主要功能和特点如下：一、产品概述型号：CI541V1品牌：OMRON欧姆龙类别：通讯模块功能：用于工业自动化和控制系统中，实现不同设备和系统之间的通信。二、主要特性多协议支持：CI541V1模块支持多种通信协议和......
  • 2、A+B（for循环，数据类型，sys模块）
    总结for循环foriteminlist:#循环体这里的list就是一个列表，而item是一个循环变量，表示每次循环时的值。persons=["Tom","Jerry","Vicky"]forpersoninpersons:print(person)除此之外，for循环遍历字符串。word='hello'forletterinword:print(letter)此外......
  • 嵌入式C语言自我修养：C语言的模块化的编程思想
    不同模块如何集成到系统中去？模块的编译和链接一个C语言项目划分成不同的模块，通常由多个文件来实现。在项目编译过程中，编译器是以C源文件为单位进行编译的，每一个C源文件都会被编译器翻译成对应的一个目标文件。链接器对每一个目标文件进行解析，将文件中的代码段、数据段分别组装，生成......
  • Python 路径管理新纪元：pathlib 模块带你玩转文件系统
    引言在早期的Python版本中，处理文件路径往往需要依赖于os和os.path模块，虽然它们功能强大，但复杂的API设计使得代码可读性较差。随着Python3.4的发布，pathlib模块正式登场，它以对象导向的方式简化了路径操作，使得代码更加直观易懂。本文旨在通过一系列实例帮助你快速上手pa......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99