ROS2系列教程：Nav2进阶实践（自定义算法、插件与行为树）

1
#ifndef NAV2_MY_CONTROLLER__CONTROLLER_HPP_
2
#define NAV2_MY_CONTROLLER__CONTROLLER_HPP_
3

4
#include <string>
5
#include "nav2_core/controller.hpp"
6
#include "rclcpp/rclcpp.hpp"
7
#include "geometry_msgs/msg/twist.hpp"
8

9
namespace nav2_my_controller
10
{
11

12
// 继承 nav2_core::Controller，实现 5 个纯虚接口
13
class MyController : public nav2_core::Controller
14
{
15
public:
16
  MyController() = default;
17
  ~MyController() override = default;
18

19
  // 1. 配置：从参数获取配置
20
  void configure(
21
    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
22
    std::string name,
23
    const std::shared_ptr<tf2_ros::Buffer> & tf,
24
    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
25

26
  // 2. 清理与激活
27
  void cleanup() override;
28
  void activate() override;
29
  void deactivate() override;
30

31
  // 3. 核心：计算速度指令（每 control cycle 调用）
32
  geometry_msgs::msg::Twist computeVelocityCommands(
33
    const geometry_msgs::msg::PoseStamped & pose,
34
    const geometry_msgs::msg::Twist & velocity,
35
    nav2_core::GoalChecker * goal_checker) override;
36

37
  // 4. 设置/获取规划路径
38
  void setPlan(const nav_msgs::msg::Path & path) override;
39
  nav_msgs::msg::Path getPlan() override { return plan_; }
40

41
private:
42
  double max_vel_x_;
43
  double max_vel_theta_;
44
  double k_linear_;    // 比例系数
45
  double k_angular_;
46
  nav_msgs::msg::Path plan_;   // 保存全局路径
47
  rclcpp::Logger logger_{rclcpp::get_logger("my_controller")};
48
};
49

50
}  // namespace nav2_my_controller
51

52
#endif  // NAV2_MY_CONTROLLER__CONTROLLER_HPP_

1
#include "nav2_my_controller/controller.hpp"
2
#include "pluginlib/class_list_macros.hpp"
3
#include <cmath>
4
#include <algorithm>
5

6
namespace nav2_my_controller
7
{
8

9
void MyController::configure(
10
  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
11
  std::string name,
12
  const std::shared_ptr<tf2_ros::Buffer> & tf,
13
  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros)
14
{
15
  auto node = parent.lock();
16
  nav2_util::declare_parameter_if_not_declared(
17
    node, name + ".max_vel_x", rclcpp::ParameterValue(0.5));
18
  nav2_util::declare_parameter_if_not_declared(
19
    node, name + ".max_vel_theta", rclcpp::ParameterValue(1.0));
20
  nav2_util::declare_parameter_if_not_declared(
21
    node, name + ".k_linear", rclcpp::ParameterValue(0.6));
22
  nav2_util::declare_parameter_if_not_declared(
23
    node, name + ".k_angular", rclcpp::ParameterValue(1.5));
24

25
  max_vel_x_ = node->get_parameter(name + ".max_vel_x").as_double();
26
  max_vel_theta_ = node->get_parameter(name + ".max_vel_theta").as_double();
27
  k_linear_ = node->get_parameter(name + ".k_linear").as_double();
28
  k_angular_ = node->get_parameter(name + ".k_angular").as_double();
29

30
  logger_ = node->get_logger();
31
  RCLCPP_INFO(logger_, "MyController 已配置");
32
}
33

34
void MyController::cleanup() {}
35
void MyController::activate() {}
36
void MyController::deactivate() {}
37

38
void MyController::setPlan(const nav_msgs::msg::Path & path)
39
{
40
  plan_ = path;
41
}
42

43
geometry_msgs::msg::Twist MyController::computeVelocityCommands(
44
  const geometry_msgs::msg::PoseStamped & pose,
45
  const geometry_msgs::msg::Twist & velocity,
46
  nav2_core::GoalChecker * goal_checker)
47
{
48
  geometry_msgs::msg::Twist cmd;
49
  if (plan_.poses.empty()) {
50
    cmd.linear.x = 0.0; cmd.angular.z = 0.0;
51
    return cmd;
52
  }
53

54
  // 找到路径上离当前位置最近的点作为"前视目标"（simple pursuit）
55
  double min_dist = 1e9;
56
  size_t idx = 0;
57
  for (size_t i = 0; i < plan_.poses.size(); ++i) {
58
    double dx = plan_.poses[i].pose.position.x - pose.pose.position.x;
59
    double dy = plan_.poses[i].pose.position.y - pose.pose.position.y;
60
    double d = std::hypot(dx, dy);
61
    if (d < min_dist) { min_dist = d; idx = i; }
62
  }
63
  // 前视 3 个点
64
  size_t lookahead = std::min(idx + 3, plan_.poses.size() - 1);
65
  double gx = plan_.poses[lookahead].pose.position.x - pose.pose.position.x;
66
  double gy = plan_.poses[lookahead].pose.position.y - pose.pose.position.y;
67

68
  // 目标在机器人坐标系下的角度
69
  double goal_angle = std::atan2(gy, gx);
70
  // 机器人当前朝向（四元数转 yaw）
71
  double qz = pose.pose.orientation.z, qw = pose.pose.orientation.w;
72
  double yaw = 2.0 * std::atan2(qz, qw);
73

74
  double ang_diff = goal_angle - yaw;
75
  // 归一化到 [-π, π]
76
  while (ang_diff > M_PI) ang_diff -= 2 * M_PI;
77
  while (ang_diff < -M_PI) ang_diff += 2 * M_PI;
78

79
  // P 控制：直行速度按距离、转角速度按角度差
80
  cmd.linear.x = std::clamp(k_linear_ * std::hypot(gx, gy), 0.0, max_vel_x_);
81
  cmd.angular.z = std::clamp(k_angular_ * ang_diff, -max_vel_theta_, max_vel_theta_);
82

83
  return cmd;
84
}
85

86
}  // namespace nav2_my_controller
87

88
PLUGINLIB_EXPORT_CLASS(
89
  nav2_my_controller::MyController, nav2_core::Controller)

1
<library path="nav2_my_controller">
2
  <class name="nav2_my_controller/MyController"
3
         type="nav2_my_controller::MyController"
4
         base_class_type="nav2_core::Controller">
5
    <description>一个简单的 Pure Pursuit 局部控制器</description>
6
  </class>
7
</library>