(1/100) ros的plugin插件和动态参数配置dynamic_reconfigure

写在前面：实际机器人开发中，往往会存在很多参数，这些参数需要在实际的调试中得出，为了更高效的开发与测试，则选择使用动态的参数配置。而在机器人功能模块层面，我们可能也需要在不同的场景动态加载不同的功能算法。比如，机器人的规划算法，不同的场景下可能会选择不同的规划算法，为了更方便的加载，将不同的规划算法以统一的接口进行封装，然后利用plugin机制进行动态的加载。
参考博客：

1. ROS学习之路07：编写动态重配置(dynamic_reconfigure)参数的节点
2. ROS设置plugin插件

1.动态参数配置

主要步骤：

1.1创建功能包

cd ~/catkin_ws/src
catkin_create_pkg goal_adjust roscpp dynamic_reconfigure pluginlib
cd ..
catkin_make
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

1.2.添加动态重配置文件（.cfg）

一般会像msg一样有一个头文件，但是这个文件是.cfg格式，里面一些参数的说明

#gen.add(参数名称，   参数类型，位掩码，参数描述，                        默认值，最小值，最大值)
gen.add("int_param", int_t,        0,           "An Integer parameter",  50 ,          0,             100)

1.3.修改CmakeLists.txt

#dynamic reconfigure
generate_dynamic_reconfigure_options(
    cfg/GoalAdjustInCircle.cfg
)

1.4.添加动态配置节点

主要头文件

#include <dynamic_reconfigure/server.h>
#include "goal_adjust/GoalAdjustConfig.h"

//声明
    dynamic_reconfigure::Server<::goal_adjust::GoalAdjustInCircleConfig> *server_;    dynamic_reconfigure::Server<::goal_adjust::GoalAdjustInCircleConfig>::CallbackType callbackFun;
    void reconfigureCB(::goal_adjust::GoalAdjustInCircleConfig &config, uint32_t level);
    ::goal_adjust::GoalAdjustInCircleConfig dynamic_config_;
    
//定义
      server_ = new dynamic_reconfigure::Server<::goal_adjust::GoalAdjustInCircleConfig>(ros::NodeHandle("~/" + name));
      callbackFun = boost::bind(&GoalAdjustInCircle::reconfigureCB, this, _1, _2);
      server_->setCallback(callbackFun);
 
dynamic_reconfigure::Server<dynamic_reconfigure::GoalAdjustConfig> server;
dynamic_reconfigure::Server<dynamic_reconfigure::GoalAdjustConfig>::CallbackType callbackFun;
callbackFun = boost::bind(&callback, -1, -2);//绑定实际的回调函数
Server.setCallback(callbackFun);//为服务器设置回调函数

2.plugin插件

实现一个插件主要需要以下几个步骤：
1） 创建基类，定义统一的接口
2） 创建plugin类，继承基类
3） 注册插件
4） 编译生成插件的动态链接库
5） 将插件加入ros系统

2.1 创建基类，定义统一的接口

这里利用navigation下的nav_core写了一个目标点调节的基类；

#ifndef NAV_CORE_BASE_GOAL_ADJUSTER_H
#define NAV_CORE_BASE_GOAL_ADJUSTER_H
#include <geometry_msgs/PoseStamped.h>
#include <costmap_2d/costmap_2d_ros.h>
namespace nav_core {
class BaseGoalAdjuster{
 	protected:
  		BaseGoalAdjuster(){}
 	public:
  		virtual ~BaseGoalAdjuster(){}
  		virtual void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) = 0;
  		virtual bool adjustGoal(const geometry_msgs::PoseStamped& goal,
                         geometry_msgs::PoseStamped& new_goal, double adjust_angle, std::string adjust_mode) = 0;       
};
}  // namespace nav_core
#endif  // GOAL_ADJUSTER_BASE_GOAL_ADJUSTER_H

2.2 创建plugin类，继承基类

class GoalAdjustInCircle : public nav_core::BaseGoalAdjuster
{
public:
	  GoalAdjustInCircle();
	  virtual ~GoalAdjustInCircle();
	  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) override;
	  bool adjustGoal(const geometry_msgs::PoseStamped& goal,
		                 geometry_msgs::PoseStamped& new_goal, double adjust_angle, std::string adjust_mode) override;
};

2.3 注册插件

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(cadmus::goal_adjust::GoalAdjustInCircle, nav_core::BaseGoalAdjuster)

2.4 编译生成插件的动态链接库

在CmakeLists.txt中写入以下两行代码

 install(FILES goal_adjust.xml
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

2.5 将插件加入ros系统

a.创建插件描述文件xml

<library path="lib/libgoal_adjust">
  <class name="goal_adjust/GoalAdjustInCircle" type="cadmus::goal_adjust::GoalAdjustInCircle" base_class_type="nav_core::BaseGoalAdjuster">
    <description>
      goal pose adjust in circle.
    </description>
  </class>
</library>

b.导出插件

在package.xml文件中添加以下代码，将创建的插件导出：

<export>
    <nav_core plugin="${prefix}/goal_adjust.xml" />
</export>

c.验证

编译完成后，输入 rospack plugins --attrib=plugin nav_core，输出结果为创建的插件的绝对路径。