ROS2系列教程：Nav2导航仿真实战（全局+局部规划）

1
用户目标点 (goal_pose)
2
  → BT Navigator（行为树编排）
3
     → Planner Server：A* 全局路径（map 上找路）
4
     → Costmap：局部代价图（实时障碍）
5
     → Controller Server：DWA 局部规划（避开动态障碍）
6
  → /cmd_vel（速度指令）
7
  → 机器人移动 → odom/scan 反馈 → 循环

1
# 全局代价地图
2
global_costmap:
3
  global_costmap:
4
    ros__parameters:
5
      use_sim_time: True
6
      robot_radius: 0.20
7
      obstacle_layer:
8
        plugin: "nav2_costmap_2d::ObstacleLayer"
9
        enabled: True
10
        observation_sources: scan
11
        scan:
12
          topic: /scan
13
          max_obstacle_height: 2.0
14
          clearing: True
15
          marking: True
16
          data_type: "LaserScan"
17
      inflation_layer:
18
        plugin: "nav2_costmap_2d::InflationLayer"
19
        cost_scaling_factor: 3.0
20
        inflation_radius: 0.55
21
      static_layer:
22
        plugin: "nav2_costmap_2d::StaticLayer"
23
        map_subscribe_transient_local: True
24
      always_send_full_costmap: True
25
      robot_radius: 0.20
26
      update_frequency: 1.0
27
      publish_frequency: 1.0
28

29
# 局部代价地图
30
local_costmap:
31
  local_costmap:
32
    ros__parameters:
33
      use_sim_time: True
34
      robot_radius: 0.20
35
      obstacle_layer:
36
        plugin: "nav2_costmap_2d::ObstacleLayer"
37
        enabled: True
38
        observation_sources: scan
39
        scan:
40
          topic: /scan
41
          max_obstacle_height: 2.0
42
          clearing: True
43
          marking: True
44
          data_type: "LaserScan"
45
      inflation_layer:
46
        plugin: "nav2_costmap_2d::InflationLayer"
47
        cost_scaling_factor: 3.0
48
        inflation_radius: 0.55
49
      static_layer:
50
        plugin: "nav2_costmap_2d::StaticLayer"
51
      always_send_full_costmap: True
52
      width: 3
53
      height: 3
54
      resolution: 0.05
55
      update_frequency: 5.0
56
      publish_frequency: 2.0
57
      rolling_window: true

1
from launch import LaunchDescription
2
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
3
from launch.launch_description_sources import PythonLaunchDescriptionSource
4
from launch_ros.actions import Node
5
from launch.substitutions import LaunchConfiguration
6
import os
7
from ament_index_python.packages import get_package_share_directory
8

9
def generate_launch_description():
10
    pkg_bringup = get_package_share_directory('my_robot_bringup')
11

12
    map_yaml = os.path.join(pkg_bringup, 'maps', 'my_map.yaml')
13
    nav2_params = os.path.join(pkg_bringup, 'config', 'nav2_params.yaml')
14

15
    nav2_bringup = IncludeLaunchDescription(
16
        PythonLaunchDescriptionSource(
17
            os.path.join(get_package_share_directory('nav2_bringup'),
18
                         'launch', 'bringup_launch.py')),
19
        launch_arguments={
20
            'map': map_yaml,
21
            'params_file': nav2_params,
22
            'use_sim_time': 'true'}.items())
23

24
    rviz = Node(
25
        package='rviz2', executable='rviz2',
26
        arguments=['-d', os.path.join(pkg_bringup, 'config', 'nav2.rviz')],
27
        output='screen')
28

29
    return LaunchDescription([nav2_bringup, rviz])

1
// send_goal.cpp：向 /navigate_to_pose 发送目标
2
#include <rclcpp/rclcpp.hpp>
3
#include <nav2_msgs/action/navigate_to_pose.hpp>
4
#include <rclcpp_action/rclcpp_action.hpp>
5

6
class NavClient : public rclcpp::Node
7
{
8
public:
9
  using NavigateToPose = nav2_msgs::action::NavigateToPose;
10
  using GoalHandle = rclcpp_action::ClientGoalHandle<NavigateToPose>;
11

12
  NavClient() : Node("nav_client")
13
  {
14
    client_ = rclcpp_action::create_client<NavigateToPose>(this, "/navigate_to_pose");
15
  }
16

17
  void send_goal(double x, double y, double yaw)
18
  {
19
    if (!client_->wait_for_action_server(std::chrono::seconds(5))) {
20
      RCLCPP_ERROR(this->get_logger(), "Nav2 action server 不可用");
21
      return;
22
    }
23
    auto goal = NavigateToPose::Goal();
24
    goal.pose.header.frame_id = "map";
25
    goal.pose.header.stamp = now();
26
    goal.pose.pose.position.x = x;
27
    goal.pose.pose.position.y = y;
28
    // yaw -> 四元数（绕 z 轴旋转）
29
    goal.pose.pose.orientation.z = std::sin(yaw / 2.0);
30
    goal.pose.pose.orientation.w = std::cos(yaw / 2.0);
31

32
    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
33
    send_goal_options.goal_response_callback =
34
      [this](GoalHandle::SharedPtr gh) {
35
        if (gh) RCLCPP_INFO(this->get_logger(), "目标被接受");
36
        else    RCLCPP_ERROR(this->get_logger(), "目标被拒绝");
37
      };
38
    send_goal_options.result_callback =
39
      [this](const GoalHandle::WrappedResult& result) {
40
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
41
          RCLCPP_INFO(this->get_logger(), "导航成功！");
42
        else
43
          RCLCPP_ERROR(this->get_logger(), "导航失败，状态码=%d",
44
                       static_cast<int>(result.code));
45
      };
46
    client_->async_send_goal(goal, send_goal_options);
47
  }
48

49
private:
50
  rclcpp_action::Client<NavigateToPose>::SharedPtr client_;
51
};
52

53
int main(int argc, char** argv)
54
{
55
  rclcpp::init(argc, argv);
56
  auto node = std::make_shared<NavClient>();
57
  double x = argc > 1 ? std::atof(argv[1]) : 1.5;
58
  double y = argc > 2 ? std::atof(argv[2]) : 0.0;
59
  node->send_goal(x, y, 0.0);
60
  rclcpp::spin(node);
61
  rclcpp::shutdown();
62
  return 0;
63
}