ROS2 Nav2详解与C++ API介绍
Nav2简介
一、 什么是Nav2？
Nav2是ROS2中的导航系统，它提供了一系列算法和工具，使移动机器人能够从一点自主移动到另一点。Nav2采用行为树结构来组织导航任务，并大量使用动作通信来实现模块间的交互。
核心组成部分：
全局规划器 - 计算从起点到目标点的全局路径
局部规划器 - 根据全局路径和传感器数据计算局部控制命令
恢复行为 - 在机器人被困时执行恢复操作
行为树 - 协调导航任务中的各个模块
形象比喻：
Nav2就像一个自动驾驶系统
全局规划器 = 导航系统（计算整个路线）
局部规划器 = 方向盘和油门（避免障碍，沿路线行驶）
恢复行为 = 遇到死胡同时的倒车和重新规划
🔧 Nav2 核心组件详解
1. 行为树导航器 (Behavior Tree Navigator)
行为树是Nav2的"大脑"，负责协调整个导航流程。与ROS1的有限状态机相比，行为树提供了更具可扩展性和人类可理解性的框架。
主要功能：
- 接收导航目标（单点或多点）
- 协调规划、控制、恢复等组件的执行顺序
- 处理导航过程中的异常情况和恢复行为
2.规划器系统 (Planner System)
规划器负责计算从起点到目标点的全局路径。
常用规划器插件：
NavFn规划器：使用Dijkstra或A算法，稳定性经过时间验证
Smac2D规划器：使用A算法在2D网格中进行路径搜素
ThetaStar规划器：使用Theta*算法，规划更自然的路径
3. 控制器系统 (Controller System)
控制器（局部规划器）负责生成速度命令来跟踪全局路径。
常用控制器插件：
RPP控制器：纯追踪算法，环境单纯时使用
DWB控制器：动态窗口法，具备动态避障能力，是系统默认控制器
MPPI控制器：模型预测路径积分，运算需求高，适合复杂环境
4. 恢复系统 (Recovery System)
恢复行为是容错系统的支柱，用于处理系统的未知或故障情况。
典型恢复行为：
- 清除代价地图
- 原地旋转
- 后退操作
- 随机游走
代价地图系统 (Costmap System)
代价地图表示环境信息，包括障碍物、膨胀区域等。
组成结构：
**全局代价地图：**基于静态地图生成，用于全局路径规划
**局部代价地图：**以机器人为中心，包含实时传感器数据，用于局部避障
6. 定位系统 (Localization System)
AMCL（自适应蒙特卡洛定位）：
基于粒子滤波的定位算法
使用激光雷达数据和地图进行位姿估计
发布map到odom坐标系的变换
7. 地图服务器 (Map Server)
负责加载、提供和存储地图，提供：
静态地图数据（map.pgm + map.yaml）
地图服务接口
8. 生命周期管理器 (Lifecycle Manager)
管理所有Nav2节点的生命周期状态，确保系统安全启动和关闭。
工作流程：
当机器人执行导航任务时，完整的流程如下：
- 系统启动：生命周期管理器按顺序启动并激活所有核心节点
- 定位初始化：AMCL根据初始位姿进行定位，将机器人"锚定"在地图上
- 路径规划：接收到目标后，规划器基于全局代价地图计算最优路径
- 路径跟踪：控制器根据全局路径和局部代价地图生成速度命令
- 异常处理：如果机器人被卡住，恢复行为会被触发以解决问题
- 任务完成：到达目标后，系统向请求者返回成功结果
Nav2 C++核心管理器API
1. Nav2生命周期管理器
#include "nav2_lifecycle_manager/lifecycle_manager.hpp"
#include "nav2_util/lifecycle_service_client.hpp"
class Nav2LifecycleManager {
public:
// 创建生命周期管理器
Nav2LifecycleManager();
// 启动所有Nav2节点
bool startup();
// 关闭所有Nav2节点
bool shutdown();
// 暂停导航系统
bool pause();
// 恢复导航系统
bool resume();
// 检查系统状态
bool isActive();
};
示例：
auto lifecycle_manager = std::make_shared<nav2_lifecycle_manager::LifecycleManager>();
auto client = std::make_shared<nav2_util::LifecycleServiceClient>("lifecycle_manager");
// 启动Nav2系统
if (client->startup()) {
RCLCPP_INFO(logger, "Nav2 system started successfully");
}
// 暂停导航
if (client->pause()) {
RCLCPP_INFO(logger, "Nav2 system paused");
}
2. 行为树导航器
#include "nav2_bt_navigator/navigator.hpp"
#include "nav2_behavior_tree/behavior_tree_engine.hpp"
class BehaviorTreeNavigator : public rclcpp::Node {
public:
// 加载行为树
bool loadBehaviorTree(const std::string& bt_xml_filename);
// 导航到目标位姿
nav2_behavior_tree::BtStatus navigateToPose(
const geometry_msgs::msg::PoseStamped& goal);
// 跟随航点
nav2_behavior_tree::BtStatus followWaypoints(
const std::vector<geometry_msgs::msg::PoseStamped>& waypoints);
// 取消当前导航任务
bool cancelNavigation();
// 获取当前任务状态
nav2_behavior_tree::BtStatus getNavigationStatus();
};
规划器API
1. 全局规划器接口
#include "nav2_core/global_planner.hpp"
#include "nav_msgs/msg/path.hpp"
class GlobalPlannerClient : public rclcpp::Node {
public:
GlobalPlannerClient();
// 计算从起点到目标的路径
nav_msgs::msg::Path computePath(
const geometry_msgs::msg::PoseStamped& start,
const geometry_msgs::msg::PoseStamped& goal);
// 检查规划器是否准备好
bool isPlannerReady();
// 获取规划器名称
std::string getPlannerName();
// 设置规划器参数
void setPlannerParameters(const rclcpp::ParameterMap& parameters);
};
使用示例：
auto planner_client = std::make_shared<GlobalPlannerClient>();
geometry_msgs::msg::PoseStamped start, goal;
// 设置起点和目标位姿
start.pose.position.x = 0.0;
start.pose.position.y = 0.0;
goal.pose.position.x = 5.0;
goal.pose.position.y = 3.0;
// 计算路径
auto path = planner_client->computePath(start, goal);
if (!path.poses.empty()) {
RCLCPP_INFO(logger, "Path found with %zu points", path.poses.size());
}
2. 局部规划器接口
#include "nav2_core/controller.hpp"
#include "geometry_msgs/msg/twist.hpp"
class ControllerClient : public rclcpp::Node {
public:
ControllerClient();
// 计算速度命令以跟踪路径
geometry_msgs::msg::Twist computeVelocityCommands(
const geometry_msgs::msg::PoseStamped& pose,
const geometry_msgs::msg::Twist& velocity);
// 设置要跟踪的路径
bool setPlan(const nav_msgs::msg::Path& path);
// 检查是否到达目标
bool isGoalReached();
// 获取控制器名称
std::string getControllerName();
};
代价地图API
1. 代价地图管理器
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
class CostmapManager : public rclcpp::Node {
public:
CostmapManager();
// 获取全局代价地图
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getGlobalCostmap();
// 获取局部代价地图
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getLocalCostmap();
// 清除代价地图中的障碍物
void clearCostmap();
// 在指定区域清除障碍物
void clearArea(
double origin_x, double origin_y,
double width, double height);
// 获取代价地图的层信息
std::vector<std::string> getLayers();
// 启用/禁用特定层
void enableLayer(const std::string& layer_name, bool enable);
};
使用示例：
auto costmap_manager = std::make_shared<CostmapManager>();
auto global_costmap = costmap_manager->getGlobalCostmap();
// 清除机器人周围的障碍物
costmap_manager->clearArea(0.0, 0.0, 2.0, 2.0);
// 获取代价地图层
auto layers = costmap_manager->getLayers();
for (const auto& layer : layers) {
RCLCPP_INFO(logger, "Costmap layer: %s", layer.c_str());
}
恢复行为API
1. 恢复器管理器
#include "nav2_core/recovery.hpp"
class RecoveryManager : public rclcpp::Node {
public:
RecoveryManager();
// 执行恢复行为
bool executeRecovery();
// 执行自旋恢复
bool spinRecovery();
// 执行后退恢复
bool backupRecovery();
// 获取可用的恢复行为列表
std::vector<std::string> getAvailableRecoveries();
// 设置恢复行为参数
void setRecoveryParameters(const rclcpp::ParameterMap& parameters);
};
工具类API
1. 机器人工具类
#include "nav2_util/robot_utils.hpp"
class RobotUtils : public rclcpp::Node {
public:
RobotUtils();
// 获取机器人当前位置
geometry_msgs::msg::PoseStamped getCurrentPose(
const std::string& frame_id = "map");
// 获取机器人当前速度
geometry_msgs::msg::Twist getCurrentVelocity();
// 停止机器人
bool stopRobot();
// 检查机器人是否静止
bool isRobotStopped();
// 获取机器人足迹
geometry_msgs::msg::Polygon getRobotFootprint();
};
2. 坐标变换工具
#include "nav2_util/geometry_utils.hpp"
class GeometryUtils : public rclcpp::Node {
public:
GeometryUtils();
// 计算两点之间的距离
static double calculateDistance(
const geometry_msgs::msg::Point& p1,
const geometry_msgs::msg::Point& p2);
// 计算位姿之间的角度差
static double calculateYawDifference(
const geometry_msgs::msg::Pose& p1,
const geometry_msgs::msg::Pose& p2);
// 检查位姿是否在容差范围内
static bool isPoseWithinTolerance(
const geometry_msgs::msg::Pose& current,
const geometry_msgs::msg::Pose& target,
double linear_tolerance,
double angular_tolerance);
// 将偏航角转换为四元数
static geometry_msgs::msg::Quaternion createQuaternionFromYaw(double yaw);
};
完整的Nav2应用示例
#include "rclcpp/rclcpp.hpp"
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
#include "nav2_bt_navigator/navigator.hpp"
#include "nav2_util/robot_utils.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
class AdvancedNav2Controller : public rclcpp::Node {
public:
AdvancedNav2Controller() : Node("advanced_nav2_controller") {
// 初始化各个组件
lifecycle_client_ = std::make_shared<nav2_lifecycle_manager::LifecycleManagerClient>();
robot_utils_ = std::make_shared<RobotUtils>();
// 启动Nav2系统
if (!lifecycle_client_->startup()) {
RCLCPP_ERROR(get_logger(), "Failed to start Nav2 system");
return;
}
RCLCPP_INFO(get_logger(), "Nav2 system started successfully");
}
// 高级导航函数
bool navigateToGoal(double x, double y, double yaw) {
auto goal = createPoseStamped(x, y, yaw);
return executeNavigation(goal);
}
bool navigateThroughWaypoints(const std::vector<std::tuple<double, double, double>>& waypoints) {
std::vector<geometry_msgs::msg::PoseStamped> poses;
for (const auto& wp : waypoints) {
poses.push_back(createPoseStamped(std::get<0>(wp), std::get<1>(wp), std::get<2>(wp)));
}
return executeWaypointNavigation(poses);
}
// 安全停止函数
void emergencyStop() {
robot_utils_->stopRobot();
RCLCPP_WARN(get_logger(), "Emergency stop activated");
}
// 获取导航状态
void printNavigationStatus() {
auto current_pose = robot_utils_->getCurrentPose();
auto velocity = robot_utils_->getCurrentVelocity();
RCLCPP_INFO(get_logger(),
"Current Pose: (%.2f, %.2f, %.2f), Velocity: (%.2f, %.2f)",
current_pose.pose.position.x,
current_pose.pose.position.y,
tf2::getYaw(current_pose.pose.orientation),
velocity.linear.x,
velocity.angular.z);
}
private:
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> lifecycle_client_;
std::shared_ptr<RobotUtils> robot_utils_;
geometry_msgs::msg::PoseStamped createPoseStamped(double x, double y, double yaw) {
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "map";
pose.header.stamp = this->now();
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.orientation = GeometryUtils::createQuaternionFromYaw(yaw);
return pose;
}
bool executeNavigation(const geometry_msgs::msg::PoseStamped& goal) {
// 这里应该调用行为树导航器的API
// 简化示例
RCLCPP_INFO(get_logger(), "Navigating to: (%.2f, %.2f)",
goal.pose.position.x, goal.pose.position.y);
return true;
}
bool executeWaypointNavigation(const std::vector<geometry_msgs::msg::PoseStamped>& waypoints) {
RCLCPP_INFO(get_logger(), "Navigating through %zu waypoints", waypoints.size());
return true;
}
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto controller = std::make_shared<AdvancedNav2Controller>();
// 示例使用
std::vector<std::tuple<double, double, double>> waypoints = {
{1.0, 1.0, 0.0},
{2.0, 2.0, 1.57},
{3.0, 1.0, 3.14}
};
controller->navigateThroughWaypoints(waypoints);
// 定期打印状态
auto timer = controller->create_wall_timer(
std::chrono::seconds(2),
[controller]() { controller->printNavigationStatus(); });
rclcpp::spin(controller);
rclcpp::shutdown();
return 0;
}
CMakeLists.txt配置
cmake_minimum_required(VERSION 3.8)
project(nav2_advanced_controller)
# 设置C++标准
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
# 查找依赖包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(nav2_common REQUIRED)
find_package(nav2_lifecycle_manager REQUIRED)
find_package(nav2_bt_navigator REQUIRED)
find_package(nav2_util REQUIRED)
find_package(nav2_core REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
# 创建可执行文件
add_executable(nav2_advanced_controller
src/nav2_advanced_controller.cpp
)
# 添加依赖
ament_target_dependencies(nav2_advanced_controller
rclcpp
nav2_common
nav2_lifecycle_manager
nav2_bt_navigator
nav2_util
nav2_core
nav2_costmap_2d
geometry_msgs
tf2_geometry_msgs
)
# 安装
install(TARGETS
nav2_advanced_controller
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
总结
Nav2是ROS2中强大的导航系统，它通过动作通信提供导航功能。最常用的动作是NavigateToPose，它允许机器人导航到指定的位姿。通过C++客户端，我们可以轻松地发送目标位姿并接收反馈和结果。
对于更复杂的导航任务，还可以使用FollowWaypoints动作来导航到多个航点，或使用ComputePathToPose和FollowPath动作分别进行路径规划和路径跟踪。
- Edited By Kaiserzz
- 2025/10/31
扫一扫
4469
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
