动态调整PID¶

在前面的学习过程中,参数我们都是写死在代码中的,如果想要修改参数,需要先停止代码,然后再代码中修改参数,这给我们调试带来很大的麻烦.

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

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

cd src
catkin_create_pkg zxcar_pid roscpp rospy rosmsg dynamic_reconfigure std_msgs

创建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"))

下面我们对文件内容做一些说明,这个cfg文件是用python实现的,所以需要python头部声明

#! /usr/bin/env python

我们要使用dynamic_reconfigure所以要导入它的依赖

from dynamic_reconfigure.parameter_generator_catkin import *

其实上面这一行最主要的是导入了ParameterGenerator

然后调用它的api,来定义参数

#       名称        类型        标记   参数描述     默认      最小    最大
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)

gen.add(name,type, level,description, default, min,max)的参数说明如下:

name:变量名称
type: 类型名称,常用类型有: int_t double_t str_t bool_t
level: 一个标记位,只要参数被修改了,就会改为这个值
description : 参数的描述
default : 默认值
min:可选, 参数最小值
max:可选, 参数最大值

最后的这行代码是用于生成python/cpp需要的头文件

PACKAGE : 包名
参数2: 自定义的节点名称
参数3: 与文件名相同

exit(gen.generate(PACKAGE, "zxcar_pid_node", "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
 )

创建服务端节点¶

我们首先需要导入相应的依赖

from zxcar_pid.cfg import zxcar_PIDConfig
from dynamic_reconfigure.server import Server

然后我们新建一个节点

rospy.init_node("update_pid")

启动参数服务器

Server(zxcar_PIDConfig, self.dynamic_callback)

编写回调函数,在回调函数中,我们就可以获取界面上的数值啦!

def dynamic_callback(self,config, level):
    # TODO 
    print config,level
    return config

调试¶

首先启动一个roscore节点

roscore

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

rosrun zxcar_pid UpdatePIDNode.py

启动界面客户端

rosrun rqt_reconfigure rqt_reconfigure

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

rosrun rqt_topic rqt_topic

完整示例代码如下

#! /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))

执行完上述代码之后,我们还需要在zxcar_driver中将pid数据发送给下位机

为此,我们需要订阅上面的消息/zxcar/pid

# 订阅pid消息
rospy.Subscriber("/zxcar/pid",Float32MultiArray,pid_callback);

然后在回调函数中处理响应的逻辑

def pid_callback(msg):
    rospy.loginfo("pid:{},{},{}".format(msg.data[0],msg.data[1],msg.data[2]))
    kp = int(msg.data[0]*100)
    ki = int(msg.data[1]*100)
    kd = int(msg.data[2]*100)

    # 对数据进行拆解
    kp_params = bytearray(struct.pack('h',kp))
    ki_params = bytearray(struct.pack('h',ki))
    kd_params = bytearray(struct.pack('h',kd))

    #  帧头0  帧头1  帧类型
    cmd = [0xce,0xfa,0x06]
    # 添加帧长度
    cmd.append(0x06);

    cmd.append(kp_params[0])
    cmd.append(kp_params[1])

    cmd.append(ki_params[0])
    cmd.append(ki_params[1])

    cmd.append(kd_params[0])
    cmd.append(kd_params[1])

    # 添加帧尾
    cmd.append(0xad);
    # 【ce fa 06 06 00 00 00 00 00 00 ad】
    ser.write(cmd)