ROS动态调整PID

@动态调整PID
ROS提供了一个专门用于动态调整参数的功能包dynamic_reconfigure,它实现了动态配置参数的机制

我们先来创建一个功能包,添加相应的一些依赖

cd catkin_ws/src
catkin_create_pkg zxcar_pid roscpp rospy rosmsg dynamic_reconfigure std_msgs

1.创建cfg文件¶
在包的路径下新建一个cfg文件夹,再新建一个zxcar_PID.cfg文件,内容如下

#! /usr/bin/env python

PACKAGE='zxcar_pid'
import roslib
roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
#       名称       类型      等级          参数描述     默认  最小    最大
gen.add("p",       double_t,    0,    "KP param.", 0.2,     -500.00, 500.00)
gen.add("i",       double_t,    0,    "KI param.", 0.2,     -500.00, 500.00)
gen.add("d",       double_t,    0,    "KD param.", 0.2,     -500.00, 500.00)

exit(gen.generate(PACKAGE, "zxcar_pid", "zxcar_PID"))

这里我们如果想让这个文件正常执行,记得给它加上可执行的权限

chmod a+x zxcar_PID.cfg

还要记得把CMakeList.txt文件中第92行的下面代码打开

## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
   cfg/zxcar_PID.cfg
 )

2.创建服务端节点
2.1python版本

#! /usr/bin/env python
#encoding:utf-8

import rospy
from dynamic_reconfigure.server import Server
from zxcar_pid.cfg import zxcar_PIDConfig
from std_msgs.msg import Float32MultiArray

class UpdatePID():

    def __init__(self):
        rospy.init_node("update_pid")

        rospy.on_shutdown(self.shutdown)

        rate = rospy.Rate(20)


        self.publisher_pid = rospy.Publisher("/zxcar/pid", Float32MultiArray, queue_size=100)
        # 启动参数修改服务器端
        dyn_server = Server(zxcar_PIDConfig, self.dynamic_callback)

        # 参数修改客户端
        rospy.loginfo("Bring up rqt_reconfigure to control the test.")

        while not rospy.is_shutdown():


            rate.sleep()

    def dynamic_callback(self,config, level):
        print config
        # TODO 在回调中获取PID的值， 把数据发送出去
        kp = config["p"]
        ki = config["i"]
        kd = config["d"]

        # 构建PID消息
        pidmsg = Float32MultiArray()
        pidmsg.data.append(kp)
        pidmsg.data.append(ki)
        pidmsg.data.append(kd)
        # 将消息发送出去
        self.publisher_pid.publish(pidmsg)
        return config

    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        rospy.sleep(1)


if __name__ == '__main__':
    try:
        UpdatePID()

    except Exception as e :
        rospy.loginfo("update pid terminated:{}".format(e))

2.1.2调试
首先启动一个roscore节点

roscore

启动参数配置服务节点,注意python文件要给它执行权限

rosrun zxcar_pid UpdatePIDNode.py 

调整界面参数,在topic中查看是否接收到数据

rosrun rqt_topic rqt_topic

运行图：

2.2C++版本
dynamic_reconfigure_node节点的代码实现server.cpp如下

#include <ros/ros.h>

#include <dynamic_reconfigure/server.h>
#include <zxcar_pid/zxcar_PIDConfig.h>

void callback(zxcar_pid::zxcar_PIDConfig &config, uint32_t level) {
    ROS_INFO("Reconfigure Request: %f %f %f",
             config.p,
             config.i,
             config.d);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "zxcar_pid");

    dynamic_reconfigure::Server<zxcar_pid::zxcar_PIDConfig> server;
    dynamic_reconfigure::Server<zxcar_pid::zxcar_PIDConfig>::CallbackType f;

    f = boost::bind(&callback, _1, _2);
    server.setCallback(f);

    ROS_INFO("Spinning node");
    ros::spin();
    return 0;
}

代码编辑完成后在CmakeLists.txt中加入以下编译规则：

# for dynamic reconfigure

add_executable(dynamic_reconfigure_node src/server.cpp)

 

# make sure configure headers are built before any node using them

add_dependencies(dynamic_reconfigure_node ${PROJECT_NAME}_gencfg)

 

# for dynamic reconfigure

target_link_libraries(dynamic_reconfigure_node ${catkin_LIBRARIES})

2.2.2调试编译成功后使用如下命令将roscore和dynamic_reconfigure_node运行起来：

roscore
rosrun dynamic_tutorials dynamic_reconfigure_node

这个时候参数动态配置的服务端就运行起来了，使用ROS提供的可视化参数配置工具来修改参数：

rosrun rqt_reconfigure rqt_reconfigure

运行结果与上图一样