ROS2从入门到精通4-4：局部控制插件开发案例(以PID算法为例)

本文主要是介绍ROS2从入门到精通4-4：局部控制插件开发案例(以PID算法为例)，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

0 专栏介绍

本专栏旨在通过对ROS2的系统学习，掌握ROS2底层基本分布式原理，并具有机器人建模和应用ROS2进行实际项目的开发和调试的工程能力。

🚀详情：《ROS2从入门到精通》

---

1 控制插件编写模板

1.1 构造控制插件类

所有全局规划插件的基类是nav2_core::Controller，该基类提供了7个纯虚方法来实现控制器插件，一个合法的控制插件必须覆盖这7个基本方法：

• configure()：在控制器服务器进入on_configure状态时会调用此方法，此方法执行ROS2参数声明和控制器成员变量的初始化；
• activate()：在控制器服务器进入on_activate状态时会调用此方法，此方法实现控制器进入活动状态前的必要操作；
• deactivate()：在控制器服务器进入on_deactivate状态时会调用此方法，此方法实现控制器进入非活动状态前的必要操作；
• cleanup()：在控制器服务器进入on_cleanup状态时会调用此方法，此方法清理为控制器创建的各种资源；
• setPlan()：当全局路径更新时调用该方法，此方法应执行转换、处理全局路径并存储的操作。
• computeVelocityCommands()：当控制器服务器需要一个新的速度命令以便机器人跟随全局路径时调用该方法。此方法返回一个geometry_msgs::msg::TwistStamped，表示机器人驱动的速度命令。此方法传递两个参数：当前机器人姿势的引用和当前速度。
• setSpeedLimit()：当需要限制机器人的最大线速度时调用该方法。速度限制可以用绝对值（m/s）或相对于最大机器人速度的百分比表示。请注意，通常，最大旋转速度会与最大线速度的变化成比例地限制，以保持当前机器人行为不受影响

此外还有一个可选的覆盖方法：

• cancel()：当控制器服务器接收到取消请求时调用该方法。如果未实现此方法，控制器将立即停止。如果实现了此方法，控制器可以执行更优雅的停止，并在完成时向控制器服务器发出信号。

按照上述标准，本文案例中PID控制器的基本成员函数和变量如下所示

namespace pid_controller
{/*** @class pid_controller::PIDController* @brief PIDController plugin*/
class PIDController : public nav2_core::Controller
{
public:/*** @brief Constructor for pid_controller::PIDController*/PIDController() = default;/*** @brief Destrructor for pid_controller::PIDController*/~PIDController() override = default;/*** @brief Configure controller state machine*/void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr& parent, std::string name,std::shared_ptr<tf2_ros::Buffer> tf,std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;/*** @brief Cleanup controller state machine*/void cleanup() override;/*** @brief Activate controller state machine*/void activate() override;/*** @brief Deactivate controller state machine*/void deactivate() override;/*** @brief Compute the best command given the current pose and velocity, with possible debug information*/geometry_msgs::msg::TwistStamped computeVelocityCommands(const geometry_msgs::msg::PoseStamped& pose,const geometry_msgs::msg::Twist& velocity,nav2_core::GoalChecker* /*goal_checker*/) override;/*** @brief Sets the global plan*/void setPlan(const nav_msgs::msg::Path& path) override;/*** @brief Limits the maximum linear speed of the robot.*/void setSpeedLimit(const double& speed_limit, const bool& percentage) override;protected:rclcpp_lifecycle::LifecycleNode::WeakPtr node_;std::shared_ptr<tf2_ros::Buffer> tf_;std::string plugin_name_;std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;nav2_costmap_2d::Costmap2D* costmap_;rclcpp::Logger logger_{ rclcpp::get_logger("PIDController") };rclcpp::Clock::SharedPtr clock_;std::mutex mutex_;
};}  // namespace pid_controller

1.2 注册并导出插件

在创建了自定义规划器的前提下，需要导出该控制器插件以便控制器服务器可以在运行时正确地加载。在ROS2中，插件的导出和加载由pluginlib处理。

• 源文件配置导出宏

  #include "pluginlib/class_list_macros.hpp"
  PLUGINLIB_EXPORT_CLASS(pid_controller::PIDController, nav2_core::Controller)
  

• 配置插件描述文件xxx_controller_plugin.xml，例如本案例为pid_controller_plugin.xml文件。此XML文件包含以下信息：

  • library path：插件库名称及其位置；
  • class name：控制算法类的名称；
  • class type：控制算法类的类型；
  • base class：控制基类的名称，统一为nav2_core::Controller
  • description：插件的描述。

  实例如下

  <library path="pid_controller_plugin"><class name="pid_controller/PIDController" type="pid_controller::PIDController" base_class_type="nav2_core::Controller"><description>This is an example of pid controller.</description></class>
  </library>
  

• 配置CMakeLists.txt文件
  使用cmake函数pluginlib_export_plugin_description_file()来导出插件。这个函数会将插件描述文件安装到install/share目录中，并设置ament索引以使其可被发现，实例如下

  pluginlib_export_plugin_description_file(nav2_core pid_controller_plugin.xml)
  

• 配置package.xml描述文件，实例如下：

  <export><build_type>ament_cmake</build_type><nav2_core plugin="${prefix}/pid_controller_plugin.xml" />
  </export>
  

1.3 编译与使用插件

编译该插件软件包，接着通过配置文件使用插件。

参数的传递链如下：首先在simulation.launch.py中引用配置文件navigation.yaml

declare_params_file_cmd = DeclareLaunchArgument('params_file',default_value=os.path.join(simulation_dir, 'config', 'navigation.yaml'),description='Full path to the ROS2 parameters file to use for all launched nodes')

接着在navigation.yaml中修改插件配置，默认如下，是用的是DWBLocalPlanner插件：

controller_server:ros__parameters:controller_plugins: ["FollowPath"]# DWB parametersFollowPath:plugin: "dwb_core::DWBLocalPlanner"min_vel_x: 0.0min_vel_y: 0.0max_vel_x: 0.26max_vel_y: 0.0max_vel_theta: 1.0min_speed_xy: 0.0max_speed_xy: 0.26min_speed_theta: 0.0...

将上述替换为自己的插件，本案例为：

controller_server:ros__parameters:controller_plugins: ["FollowPath"]FollowPath: plugin: "pid_controller/PIDController"

接着运行路径规划即可看到控制算法被替换

2 基于PID的路径跟踪原理

PID控制是一种常用的经典控制算法，其应用背景广泛，例如

• 工业自动化控制：温度控制、压力控制、流量控制、液位控制等过程控制系统多采用PID闭环，可以帮助维持系统参数在设定值附近，以提高生产过程的稳定性和效率；
• 机械工程：PID算法可用于实现精确的运动控制，包括控制位置、速度和力。这包括机器人控制、电机控制、汽车巡航控制等；
• 农业自动化：PID算法可用于控制温室环境，包括温度、湿度和光照，以促进植物的生长和提高农业生产；
• …

PID代表比例(Proportional)、积分(Integral)和微分(Derivative)，它通过根据误差信号的大小和变化率来调整控制器的输出，以使系统的输出尽可能接近期望值，其控制框图如下所示

在基于PID的局部路径规划中，希望机器人能快速跟踪上预设的轨迹，设误差量为$e_k$。$e_k$可以根据实际的控制目标进行选择，例如线速度误差、角速度误差、轨迹跟踪误差等

以轨迹跟踪误差为例，如图所示，根据几何关系可得

$$e_k=\sin \left({\theta }_{k,d}-{\theta }_k\right)\cdot d_k$$

其中

$$\begin{gathered} {\theta }_{k,d}=\mathrm{a}\tan \left(y_{k,d}-y_k,x_{k,d}-x_k\right) \\ {d}_k=\sqrt{{\left(x_{k,d}-x_k\right)}^2+{\left(y_{k,d}-y_k\right)}^2} \end{gathered}$$

接着以该误差作为反馈测量值通过PID控制器生成控制量，机器人基于控制量和运动学模型运动，循环往复直到机器人完成控制目标

3 控制插件开发案例(PID算法)

不同控制器其本质区别在于 computeVelocityCommands()方法的不同，对于本文案例的PID控制器，我们的 computeVelocityCommands()接口如下：

geometry_msgs::msg::TwistStamped PIDController::computeVelocityCommands(const geometry_msgs::msg::PoseStamped& pose, const geometry_msgs::msg::Twist& speed, nav2_core::GoalChecker* goal_checker)
{...// position reachedgeometry_msgs::msg::TwistStamped cmd_vel;if (shouldRotateToGoal(current_ps_map, global_plan_.poses.back())){double e_theta = regularizeAngle(goal_rpy_.z() - theta);// orientation reachedif (!shouldRotateToPath(std::fabs(e_theta))){cmd_vel.twist.linear.x = 0.0;cmd_vel.twist.angular.z = 0.0;}// orientation not reachedelse{cmd_vel.twist.linear.x = 0.0;cmd_vel.twist.angular.z = angularRegularization(speed.angular.z, e_theta / d_t_);}}// posistion not reachedelse{Eigen::Vector3d s(current_ps_map.pose.position.x, current_ps_map.pose.position.y, theta);    // current stateEigen::Vector3d s_d(target_ps_map.pose.position.x, target_ps_map.pose.position.y, theta_d);  // desired stateEigen::Vector2d u_r(vt, wt);                                                                 // refered inputEigen::Vector2d u = _pidControl(s, s_d, u_r);cmd_vel.twist.linear.x = linearRegularization(std::hypot(speed.linear.x, speed.linear.y), u[0]);cmd_vel.twist.angular.z = angularRegularization(speed.angular.z, u[1]);}// publish next target_ps_map posetarget_ps_map.header.frame_id = "map";target_ps_map.header.stamp = current_ps_map.header.stamp;target_pose_pub_->publish(target_ps_map);// publish robot posecurrent_ps_map.header.frame_id = "map";current_ps_map.header.stamp = current_ps_map.header.stamp;current_pose_pub_->publish(current_ps_map);// populate and return messagecmd_vel.header = pose.header;return cmd_vel;
}

这里发布了两个话题target_pose和current_pose，分别代表PID算法的目标位姿和实际位姿，二者的差值将作为误差量驱动PID控制器执行，其效果如下所示(蓝色箭头是target_pose以及绿色箭头是current_pose)

常见问题

1. /opt/ros/noetic/lib/move_base/move_base: symbol lookup error: /home/winter/ROS/ros_learning_tutorials/Lecture19/devel/lib//libmy_planner.so: undefined symbol: _ZN18base_local_planner12CostmapModelC1ERKN10costmap_2d9Costmap2DE

   解决方案：未定义符号错误undefined symbol一般是依赖配置错误导致，采用c++filt工具解析符号

   c++filt _ZN18base_local_planner12CostmapModelC1ERKN10costmap_2d9Costmap2DE
   base_local_planner::CostmapModel::CostmapModel(costmap_2d::Costmap2D const&)
   

   可以看出是base_local_planner的问题，需要在功能包CMakeLists.txt中配置base_local_planner的相关依赖。

   c++filt是什么？g++编译器有名字修饰机制,其目的是给同名的重载函数不同的、唯一的签名识别，所有函数在编译后的文件中都会生成唯一的符号，c++filt可以逆向解析符号，还原函数，定位代码。

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇

这篇关于ROS2从入门到精通4-4：局部控制插件开发案例(以PID算法为例)的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---