Path Planning

全局路径规划技能

实现 A*、RRT*、Dijkstra、Hybrid A* 等全局路径规划算法及 nav2 插件开发

常用算法对比

nav2 插件开发

# 创建 planner 插件包
ros2 pkg create my_planner --cmake-args -DBUILD_SHARED_LIBS=ON

plugin.xml 注册

<library path="my_planner_lib">
  <class name="my_planner/MyPlanner"
         type="my_planner::MyPlanner"
         base_class_type="nav2_core::GlobalPlanner"/>
</library>

C++ 插件实现

#include <nav2_core/global_planner.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

namespace my_planner {

class MyPlanner : public nav2_core::GlobalPlanner {
public:
  void configure(
    rclcpp::Node* node,
    const std::string& name,
    std::shared_ptr<tf2_ros::Buffer> tf,
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros
  ) override {
    node_ = node;
    costmap_ = costmap_ros->getCostmap();
    // 读取参数
    resolution_ = costmap_->getResolution();
    origin_ = costmap_->getOrigin();
  }

  nav_msgs::msg::Path createPlan(
    const geometry_msgs::msg::PoseStamped& start,
    const geometry_msgs::msg::PoseStamped& goal
  ) override {
    nav_msgs::msg::Path path;
    path.header.stamp = node_->now();
    path.header.frame_id = costmap_->getGlobalFrameID();
    
    // A* 实现
    // 1. 将 start/goal 从 map frame 转换到 grid 坐标
    // 2. A* 搜索
    // 3. 将结果转换为 PoseStamped 序列
    
    return path;
  }

private:
  rclcpp::Node* node_;
  nav2_costmap_2d::Costmap2D* costmap_;
  double resolution_;
  geometry_msgs::msg::Point origin_;
};

}  // namespace my_planner

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(my_planner::MyPlanner, nav2_core::GlobalPlanner)

CMakeLists.txt

ament_auto_add_library(my_planner_lib SHARED src/my_planner.cpp)
pluginlib_export_class_description_code(my_planner_lib my_planner)

A* 算法实现

struct Node {
  int x, y, g, h;
  Node* parent;
  bool operator<(const Node& other) const { return (g + h) < (other.g + other.h); }
};

std::vector<Node*> aStarSearch(
    nav2_costmap_2d::Costmap2D* costmap,
    int start_x, int start_y,
    int goal_x, int goal_y
) {
  std::priority_queue<Node*> open;
  std::unordered_set<int> closed;
  open.push(new Node{start_x, start_y, 0,
      std::abs(start_x-goal_x)+std::abs(start_y-goal_y), nullptr});
  
  const int dx[8] = {0,1,1,1,0,-1,-1,-1};
  const int dy[8] = {1,1,0,-1,-1,-1,0,1};
  const int cost[8] = {1,1,1,1,1,1,1,1};
  
  while (!open.empty()) {
    auto* cur = open.top(); open.pop();
    if (cur->x == goal_x && cur->y == goal_y) {
      std::vector<Node*> result;
      while (cur) { result.push_back(cur); cur = cur->parent; }
      return result;
    }
    int key = cur->x * 10000 + cur->y;
    if (closed.count(key)) continue;
    closed.insert(key);
    
    for (int i = 0; i < 8; ++i) {
      int nx = cur->x + dx[i], ny = cur->y + dy[i];
      if (nx < 0 || ny < 0) continue;
      unsigned char c = costmap->getCost(nx, ny);
      if (c == nav2_costmap_2d::LETHAL_OBSTACLE) continue;
      int ng = cur->g + cost[i] + (c > nav2_costmap_2d::FREE ? 50 : 0);
      open.push(new Node{nx, ny, ng,
          std::abs(nx-goal_x)+std::abs(ny-goy_y), cur});
    }
  }
  return {};
}

代价地图配置

global_costmap:
  global_costmap:
    ros__parameters:
      costmap:
        global_frame: map
        robot_base_frame: base_link
        update_frequency: 5.0
        publish_frequency: 1.0
        width: 20.0   # meters
        height: 20.0
        resolution: 0.05
        inflation:
          inflation_radius: 0.5
          cost_scaling_factor: 1.0

规范

• 路径分辨率：全局 0.05m/pixel，局部 0.025m/pixel
• 最大路径长度：100m（室内），500m（室外）
• 路径重规划触发：机器人偏离路径 > 0.5m 或障碍物阻塞
• Hybrid A* 最小转弯半径：车辆轴距 × 2

错误处理