ROS小车４

上位机程序：

主要介绍ros_arduino_python包

 ros_arduino_python                      #ROS相关的Python包，用于上位机，树莓派等开发板或电脑等。
    ├── CMakeLists.txt
    ├── config                              #配置目录
    │   └── arduino_params.yaml             #定义相关参数，端口，rate，PID，sensors等默认参数。由arduino.launch调用
    ├── launch
    │   └── arduino.launch                  #启动文件
    ├── nodes
    │   └── arduino_node.py                 #python文件,实际处理节点，由arduino.launch调用，即可单独调用。
    ├── package.xml
    ├── setup.py
    └── src                                 #Python类包目录
        └── ros_arduino_python
            ├── arduino_driver.py           #Arduino驱动类
            ├── arduino_sensors.py          #Arduino传感器类
            ├── base_controller.py          #基本控制类，订阅cmd_vel话题，发布odom话题
            └── __init__.py                 #类包默认空文件  

• arduino_params.yaml  ：

• 这里建议换个名字，比如我是　my_arduino_params.yaml 

//如果要直接复制使用，请删除注释
# For a direct USB cable connection, the port name is typically
# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
# For a wireless connection like XBee, the port is typically
# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.

port: /dev/ttyACM0 //arduino端口
baud: 57600 //波特率
timeout: 0.1　//超时时间

rate: 50
#sensorstate_rate: 10

use_base_controller: True //是否使用base_controller
base_controller_rate: 10 //base_controller控制频率

# For a robot that uses base_footprint, change base_frame to base_footprint
//base_link:机器人本体坐标系，与机器人中心重合，当然有些机器人(PR 2)是base_footprint,其实是一个意思。
base_frame: base_link

# === Robot drivetrain parameters
wheel_diameter: 0.0973 //车轮直径
wheel_track: 0.292 //两轮间距
encoder_resolution: 1322 # from Pololu for 30:1 motors
//编码器码盘一圈的脉冲数，可以用arduino的串口输入Ｅ获得 这里我的电机是有减速的
//但是我图方便，没有计算减速比，直接记录车轮转一圈的脉冲数
gear_reduction: 1　//减速比
motors_reversed: True //电机是否允许反向

# === PID parameters//PID参数
lKp: 25
lKd: 12
lKi: 0
lKo: 55

rKp: 25
rKd: 12
rKi: 5
rKo: 55
accel_limit: 0.5

# === Sensor definitions.  Examples only - edit for your robot.
#     Sensor type can be one of the follow (case sensitive!):
#	  * Ping
#	  * GP2D12
#	  * Analog
#	  * Digital
#	  * PololuMotorCurrent
#	  * PhidgetsVoltage
#	  * PhidgetsCurrent (20 Amp, DC)



sensors: {
  #motor_current_left:   {pin: 0, type: PololuMotorCurrent, rate: 5},
  #motor_current_right:  {pin: 1, type: PololuMotorCurrent, rate: 5},
  #ir_front_center:      {pin: 2, type: GP2D12, rate: 10},
  #sonar_front_center:   {pin: 5, type: Ping, rate: 10},
  #arduino_led:          {pin: 13, type: Digital, rate: 5, direction: output}
}

• arduino.launch

在这里把配置加载

•  arduino_node.py      实际处理节点：

将这行更改成这样：发布的topic名字改为 cmd_vel

# A cmd_vel publisher so we can stop the robot when shutting down
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

将base_frame改为base_link

self.base_frame = rospy.get_param("~base_frame", 'base_link')

 只要注意和前后对应即可

• arduino_driver.py  Arduino驱动类：

在Arduino类中添加：

def get_pidin(self):
    values = self.execute_array('i')
    if len(values) != 2:
        print "get_pidin count was not 2"
        raise SerialException
        return None
    else:
        return values

def get_pidout(self):
    values = self.execute_array('f')
    if len(values) != 2:
        print "get_pidout count was not 2"
        raise SerialException
        return None
    else:
        return values

将update_pid更改为下面

def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo):
        ''' Set the PID parameters on the Arduino
        '''
        print "Updating PID parameters"
        cmd = 'u ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo)+':'+ str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo)
        self.execute_ack(cmd)

•  base_controller.py      基本控制类

它订阅cmd_vel话题，发布odom话题

由于左右两轮使用不同的pid参数，所=所以需要做出如下改动：

在from tf.broadcaster import TransformBroadcaster下面增加

from std_msgs.msg import Int32

将pid_params = dict()后的字典进行更改

pid_params = dict()
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        pid_params['lkp'] = rospy.get_param("~lkp", 20)
        pid_params['lkd'] = rospy.get_param("~lkd", 12)
        pid_params['lki'] = rospy.get_param("~lki", 0)
        pid_params['lko'] = rospy.get_param("~lko", 50)

        pid_params['rkp'] = rospy.get_param("~rkp", 20)
        pid_params['rkd'] = rospy.get_param("~rkd", 12)
        pid_params['rki'] = rospy.get_param("~rki", 0)
        pid_params['rko'] = rospy.get_param("~rko", 50)

 将setup_pid进行如下修改：

def setup_pid(self, pid_params):
        # Check to see if any PID parameters are missing
        missing_params = False
        for param in pid_params:
            if pid_params[param] == "":
                print("*** PID Parameter " + param + " is missing. ***")
                missing_params = True
        
        if missing_params:
            os._exit(1)
                
        self.wheel_diameter = pid_params['wheel_diameter']
        self.wheel_track = pid_params['wheel_track']
        self.encoder_resolution = pid_params['encoder_resolution']
        self.gear_reduction = pid_params['gear_reduction']
        
        self.lkp = pid_params['lkp']
        self.lkd = pid_params['lkd']
        self.lki = pid_params['lki']
        self.lko = pid_params['lko']

        self.rkp = pid_params['rkp']
        self.rkd = pid_params['rkd']
        self.rki = pid_params['rki']
        self.rko = pid_params['rko']
        
        self.arduino.update_pid(self.lkp, self.lkd, self.lki, self.lko,self.rkp, self.rkd, self.rki, self.rko)

self.odomPub = rospy.Publisher(‘odom’, Odometry) 上面添加如下内容：

self.lEncoderPub = rospy.Publisher('Lencoder', Int32)
self.rEncoderPub = rospy.Publisher('Rencoder', Int32)
self.lPidoutPub = rospy.Publisher('Lpidout', Int32)
self.rPidoutPub = rospy.Publisher('Rpidout', Int32)
self.lVelPub = rospy.Publisher('Lvel', Int32)
self.rVelPub = rospy.Publisher('Rvel', Int32)

在poll(self)函数的，if now > self.t_next: 下添加

try:
    left_pidin, right_pidin = self.arduino.get_pidin()
except:
    rospy.logerr("getpidout exception count: ")
    return

self.lEncoderPub.publish(left_pidin)
self.rEncoderPub.publish(right_pidin)
try:
    left_pidout, right_pidout = self.arduino.get_pidout()
except:
    rospy.logerr("getpidout exception count: ")
    return
self.lPidoutPub.publish(left_pidout)
self.rPidoutPub.publish(right_pidout)

在poll(self)函数的，if not self.stopped: 下添加：

self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)

 运行

将小车的电源等接好

每打开一个新终端就要刷新一下环境：（进入到工作目录运行）

source devel/setup.bash 

打开新终端，进入到ros_arduino_bridge所在工作空间，刷新工作环境后运行如下命令：

roslaunch ros_arduino_python arduino.launch

再打开一个终端，运行键盘控制程序，这个程序可以使用turtlebot包中的robot_keyboard_teleop.py。将其发送的话题重命名即可

这时如果一切正常，小车就可以正常移动，如果转动方向什么的有问题，根据个人经验调整arduino电机驱动代码

如果一切正常，进行下一步

再打开一个新终运行如下：

rqt_plot /Lencoder /Lpidout /Lvel 

可以看到实时的速度曲线，方便进行pid参数的调整。

如果小车速度不稳定，通过校准PID来改进

问题：

• 如果发现小车速度忽快忽慢，或者走不了直线，那是因为PID参数给的不合理造成的。
• PID的目的是通过改变电机PWM值，使电机实际的转速基本等于期望的转速。
• 如果参数不合理，就会出现实际的转速和期望的转速相差很远。
• 也就是说，我们没办法精准控制小车。

校准方法：

• 把电机PWM值、期望的转速和实际的转速这三者的值用图表实时地描绘出来。
• 根据PWM值和实际的转速的运动轨迹，不停地修改PID的参数，让期望的转速和实际的转速能在很短时间内的达到一致。
• 调节顺序，先调P,再调I，最后调D，通常只需要P和I两个参数就可以了。

参考链接：https://www.ncnynl.com/archives/201612/1208.html