首页 - 文章教程 - 正文

2.控制手柄的ROS驱动代码

0x00 驱动代码概述

在第一篇文章中介绍了如何使用常见的硬件来搭建一个控制手柄,我们使用Arduino为核心控制板,获取各传感器数据。然后将数据组装成自定义的数据帧格式,通过串口将数据帧发送出来。这样就完成了控制手柄固件代码,但是这些数据帧我们并不能直接将其用来控制ROS下小车移动。我们需要解析数据帧内容,然后将其再次封装成ROS下控制小车移动的消息格式,最终要通过话题发送出来才可以。本篇文章将指导如何实现手柄的ROS驱动代码,首先我们需要来认识下整个控制手柄的数据流程示意图,通过该图就能对手柄的代码实现理解更为透彻:

1542346588211338.png


0x01 创建驱动软件包

在这里由于我们增加了dynamic_reconfigure功能,这样就可以动态的更新节点参数了。这样就使得我们在创建软件包时新增了一个依赖项dynamic_reconfigure,完整的创建软件包命令如下:

catkin_create_pkg ros_handle_python rospy geometry_msgs dynamic_reconfigure

下面我们首先来看下整体代码的结构,这样就方便大家后面知道每个代码文件放置在什么路径下了:

Screenshot from 2018-11-16 13:53:59.png

在该软件包中,需要注意的是CMakeLists.txt文件,该文件需要确保有下图所示的部分才行,不然编译会出错的:

Screenshot from 2018-11-16 14:01:05.png

对于package.xml文件不用做修改,保持默认状态即可,下面来详细介绍每个源码文件的内容。


0x02 arduino通信代码

我们首先来介绍驱动部分的底层通信部分代码,由于控制手柄的主控板是arduino。那么我们的上层驱动程序就需要可以连接arduino的串口,并可以从串口读取到发送过来的数据。这部分代码是非常重要的,后续的部分解析的数据帧都是从这里获取的。我们首先来看下arduino_driver.py的完整代码,后面再对该代码进行解释说明:

#!/usr/bin/env python
# _*_ coding:utf-8 _*_

"""
 Copyright: 2016-2018 ROS小课堂 www.corvin.cn
 Author: corvin
 Description:
    A Python driver for the Arduino microcontroller
 History:
    20181024: initial this source code file.
"""

import os
import time
import sys, traceback
from serial.serialutil import SerialException
from serial import Serial


class Arduino:
    ''' Configuration Arduino Board Parameters
    '''
    def __init__(self, port="/dev/ttyACM0", baudrate=57600, timeout=0.5):
        self.port     = port
        self.baudrate = baudrate
        self.timeout  = timeout
        self.interCharTimeout = timeout/30.

    # connect arduino board
    def connect(self):
        try:
            self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout)
            # give the firmware time to wake up.
            time.sleep(1)
        except SerialException:
            print "Serial Exception:"
            print sys.exc_info()
            print "Traceback follows:"
            traceback.print_exc(file=sys.stdout)
            print "Cannot connect to Arduino!"
            os._exit(2)

    def close(self):
        ''' Close the serial port.
        '''
        self.port.close()

    def recv(self, timeout=0.5):
        timeout = min(timeout, self.timeout)
        ''' This command should not be used on its own: it is called by the execute commands
            below in a thread safe manner.  Note: we use read() instead of readline() since
            readline() tends to return garbage characters from the Arduino
        '''
        c = ''
        value = ''
        attempts = 0
        while c != '\r':
            c = self.port.read(1)
            value += c
            attempts += 1
            if attempts * self.interCharTimeout > timeout:
                return None

        value = value.strip('\r')
        return value


    def recv_int_array(self):
        try:
            values = self.recv(self.timeout).split()
            return map(int, values)
        except:
            return []

""" Basic test for connectivity """
if __name__ == "__main__":
    portName = "/dev/ttyACM0"
    baudRate = 57600

    myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
    print "Ready to connect arduino serial port..."
    myArduino.connect()

    print "Ready to read data ..."
    time.sleep(1)
    Data = []
    Data = myArduino.recv_int_array()
    print Data

    myArduino.close()
    print "Now disconnect arduino serial port..."

下面来对代码进行简要解析说明:

  • init()函数:为了初始化类的成员变量,这里提供一些默认的参数,包括默认的arduino端口/dev/ttyACM0,波特率57600,连接的超时时间等。

  • connect()函数:用于根据指定的连接参数来连接arduino串口,如果连接串口失败的话,会输出错误提示信息。

  • close()函数:用于断开当前的串口连接。

  • recv()函数:从串口中一个字符一个字符的读取内容,直到遇到换行符则认为读完一帧数据,将该一帧数据返回。

  • recv_int_array()函数:将调用recv()函数返回的数据帧给分割,然后存储到列表中,然后返回。

  • 最后的主函数,就是为了测试一下与arduino通信是否正常。测试流程为首先连接串口,从中读取数据并打印出来,最后断开连接。

在进行后面的数据帧解析前,我们需要先确保该源码文件中提供的各函数功能可以正常工作。下面可以先测试下,如下图所示,如果可以从串口中读取到数据,则说明该代码是正常的:

Screenshot from 2018-11-16 15:40:39.png


0x03 动态更新节点参数代码

为了方便及时更新节点运行参数,我就新增了这个dynamic_reconfigure功能。这里介绍要新增这个功能需要编写的代码,首先来看下需要在cfg文件夹中新增的配置文件的代码handle_params.cfg:

#!/usr/bin/env python
# _*_ coding:utf-8 _*_

"""
 Copyright: 2016-2018 ROS小课堂 www.corvin.cn
 Author: corvin
 Description:
    实现dynamic_reconfigure功能的需要的配置文件源码.
 History:
    20181116: initial this comment.
"""

from dynamic_reconfigure.parameter_generator_catkin import *

PACKAGE = "ros_handle_python"
gen = ParameterGenerator()

gen.add("cmd_topic_name",  str_t,    0, "set cmd topic name",  "/turtle1/cmd_vel")
gen.add("cmd_public_rate", int_t,    0, "send cmd rate",       20, 0, 20)
gen.add("angular_z",       double_t, 0, "set angular_z param", 1.0, 0, 2.0)
gen.add("linear_x",        double_t, 0, "set linear_x param",  1.0, 0, 2.0)

exit(gen.generate(PACKAGE, "handle_dynamic_reconfig", "handle"))

下面来对该代码进行简要解析说明:

  • gen = ParameterGenerator():创建一个动态参数生成器,下面就可以往该生成器中添加可以动态配置的参数了。

  • gen.add():第一个参数表示可以动态配置的参数名,字符串格式;第二个参数表明需要配置的参数类型,这里包括str_t(字符串)、int_t(整型)、double_t(浮点型)、bool_t(布尔型);第三个参数为要传入参数动态配置回调函数中的掩码,在回调函数中会修改所有参数的掩码,表示参数已经进行了修改,该值一般设置为0;第四个参数表示对该参数的说明描述信息。第五个参数表示参数设置的默认值;第六个参数表示参数可以设置的最小值;最后一个参数表示可以参数可以设置的最大值;

  • exit(gen.generate(PACKAGE,"handle_dynamic_reconfig", "handle")):用于生成所有C++和Python相关使用的头文件,并退出该程序。第一个参数表明当前软件包名称;第二个参数是动态参数更新的节点名;第三个参数是生成头文件使用的前缀,一般情况下是和配置文件名相同的(不带后面的.cfg后缀),当然也可以不一样。这样最终生成handleConfig.h这样的头文件,这样就可以在后面的代码中直接引用该头文件了。

  • 注意:需要给该文件加上执行权限,使用chmod +x handle_params.cfg命令增加执行权限。

然后来看一下动态更新参数服务端的代码dynamic_server.py,完整代码如下:

#!/usr/bin/env python
# _*_ coding:utf-8 _*_

'''
 Copyright: 2016-2018 ROS小课堂 www.corvin.cn
 Author: corvin
 Description:
    用于手柄可以使用dynamic_reconfigure,这样就可以动态的调整手柄的参数.
 History:
    20181115: initial this comment.
'''

import rospy

from dynamic_reconfigure.server import Server
from ros_handle_python.cfg import handleConfig

def callback(config, level):
    rospy.loginfo("Dynamic_reconfig_server:topic_name:{cmd_topic_name},public_rate:{cmd_public_rate},angular_z:{angular_z},linear_x:{linear_x}".format(**config))
    return config

if __name__ == "__main__":
    rospy.init_node("handle_dynamic_reconfig", anonymous=True)

    srv = Server(handleConfig, callback)
    rospy.spin()

该代码就是动态更新节点的服务端代码,只有该节点运行起来后才可以动态的更新节点参数。


0x04 主节点代码

下面来介绍主节点代码,该代码用来连接arduino串口,获取数据并解析数据帧。然后将获取的数据重新封装成ROS下控制移动的消息格式,然后将其发送到指定的话题中。下面来看下handle_node.py完整的代码:

#!/usr/bin/env python
# _*_ coding:utf-8 _*_

'''
 Copyright: 2016-2018 ROS小课堂 www.corvin.cn
 Author: corvin
 Description:
    ROS解析代码,用来通过串口来获取从arduino发来的数据.然后组装成控制移动的twist命令,
    通过话题发布出来.
 History:
    20181024: initial this code.
'''

import os
import rospy
import dynamic_reconfigure.client
from arduino_driver import Arduino
from geometry_msgs.msg import Twist
from serial.serialutil import SerialException


class HandleROS():
    def __init__(self):
        rospy.init_node('ros_handle_node', log_level=rospy.INFO)
        client = dynamic_reconfigure.client.Client("handle_dynamic_reconfig",timeout=30,config_callback=self.callback)

        # Get the name in case it is set in the launch file
        self.name = rospy.get_name()
        rospy.on_shutdown(self.shutdown)

        self.getConfigParam()

        # Connect arduino board
        self.controller = Arduino(self.port, self.baud, self.timeout)
        self.controller.connect()
        rospy.loginfo("Connected to handle on port " + self.port + " at " +
                      str(self.baud) + " baud!" +" cmd_topic name:" + self.cmd_topic)

        while not rospy.is_shutdown():
            dynamic_param  = client.get_configuration(timeout=3)
            self.dynamicReconfig(dynamic_param)

            self.cmd_vel_pub = rospy.Publisher(self.cmd_topic, Twist, queue_size=self.cmd_delay)
            loop_rate = rospy.Rate(self.rate)

            Data = []
            Data = self.controller.recv_int_array()
            if len(Data) == 5:
                map(float, Data)
                #rospy.loginfo("lock_status:" + str(Data[0])+" handle_x:" + str(Data[1]) + " handle_y:"+str(Data[2])
                #              + " handle_z:"+str(Data[3])+" rotate_percent:"+str(Data[4]))

                if (Data[0] == 1):
                  linear_x, angular_z = self.parseSerialData(Data[1], Data[2], Data[3], Data[4])
                  self.sendCmdTopic(linear_x, angular_z)

            loop_rate.sleep()


    def dynamicReconfig(self, paramList):
        self.cmd_topic = paramList['cmd_topic_name']
        self.rate = paramList['cmd_public_rate']
        self.angular_z = paramList['angular_z']
        self.linear_x  = paramList['linear_x']


    def callback(self, config):
        rospy.loginfo("{cmd_topic_name},{cmd_public_rate},{angular_z},{linear_x}".format(**config))


    def parseSerialData(self, handle_x, handle_y, handle_z, rotate_percent):
        if handle_z == 1:
            linear_x = self.linear_x
            linear_x *= (rotate_percent/100.0)
            self.cmd_linear_x = linear_x
        else:
            angular_z = self.angular_z
            angular_z *= (rotate_percent/100.0)
            self.cmd_angular_z = angular_z

        # config angular_z
        angular_z = self.cmd_angular_z
        if handle_x > 700:
            angular_z *= -1
        elif handle_x < 300:
            angular_z *= 1
        else:
            angular_z = 0.0

        # config linear x
        linear_x = self.cmd_linear_x
        if handle_y > 700:
            linear_x *= 1
        elif handle_y < 300:
            linear_x *= -1
        else:
            linear_x = 0.0

        #rospy.loginfo("Send linear_x: " + str(linear_x)+" angular_z:"+str(angular_z))
        return linear_x, angular_z


    def sendCmdTopic(self, linear_x, angular_z):
        '''Send cmd to topic
        '''
        cmd_msg = Twist()
        cmd_msg.linear.x = linear_x
        cmd_msg.linear.y = 0
        cmd_msg.linear.z = 0

        cmd_msg.angular.x = 0
        cmd_msg.angular.y = 0
        cmd_msg.angular.z = angular_z
        self.cmd_vel_pub.publish(cmd_msg)


    def getConfigParam(self):
        '''Get parameter from config file
        '''
        self.port = rospy.get_param("~port", "/dev/ttyACM0")
        self.baud = int(rospy.get_param("~baud", 57600))
        self.timeout = rospy.get_param("~timeout", 0.7)
        self.cmd_topic = rospy.get_param("~cmd_topic", "/cmd_vel")
        self.angular_z = rospy.get_param("~angular_z", 1.0)
        self.linear_x  = rospy.get_param("~linear_x",  1.0)
        self.rate = int(rospy.get_param("~public_rate", 20))
        self.cmd_delay = int(rospy.get_param("~cmd_delay", 1))
        self.cmd_linear_x  = 1.0
        self.cmd_angular_z = 1.0


    def shutdown(self):
        '''Shutdown the handle
        '''
        rospy.logwarn("Shutting down ros handle node...")
        try:
            self.cmd_vel_pub.publish(Twist())
            rospy.sleep(2)
        except:
            pass
        try:
            self.controller.close()
        except:
            pass
        finally:
            rospy.logwarn("Serial port closed")
            os._exit(0)


if __name__ == '__main__':
    try:
        myHandle = HandleROS()
    except SerialException:
        rospy.logerr("Serial exception trying to open port !")
        os._exit(1)

下面来对该代码进行简要解析说明:

  • init()函数:该函数为类的初始化函数,包括初始化dynamic_reconfigure客户端、从launch文件获取配置参数、连接控制手柄的arduino串口。当连接好串口后,开始不断的检测dynamic_reconfigure中参数有没有动态更新,如果更新的话就取最新参数值。使用最新参数值来初始化发布者对象和发布频率对象。然后从串口中取出数据帧,将其解析判断后重新组装成Twist消息,最后通过发布者将该控制移动的命令发布出去。

  • dynamicReconfig()函数:从dynamic_reconfigure客户端返回的列表中解析出各参数值。

  • callback()回调函数:该回调函数是dynamic_reconfigure客户端数据更新时自动调用的函数,这里的代码只是打印下最新的参数。

  • parseSerialData()函数:根据串口数据的摇杆数据值和旋转编码器值来确定线性速度x轴方向速度、角速度z轴旋转速度,这里需要注意的是只有在按下摇杆,即摇杆的hanle_z反馈为0时,旋转旋钮控制的是角速度的大小。在默认状态handle_z没有按下,反馈的是1时,控制的是线性速度x轴方向速度。另外就是如何来确定往前后走和左右转向,为了防止过于灵敏,我就设置了一个区间,超过该区间才认为控制有效。如下图所示:

     

    handle.jpeg

  • sendCmdTopic()函数:将计算得到的linear_x和angular_z填写到Twist消息中,然后通过发布者发布到指定话题中。

  • getConfigParam()函数:从launch文件中获取的配置参数。我这里的代码可以使用两种方式来修改控制手柄的参数,一个就是launch文件加载的配置参数,另外一个就是dynamic_reconfigure可以动态更新配置参数。

  • shutdown()函数:当节点停止运行时执行的函数,这里是向话题发送一个空移动命令,为了让小车现在可以停止移动。然后断开连接的串口。

  • 最后的main函数:就是初始化一个HandleROS()类对象,这样就会直接调用init()函数了,节点就开始不断运行了。

最后来看一下launch文件ros_handle.launch,通过该文件就可以来使手柄节点正常启动了:

<!--
  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
  Author: corvin
  Description:
    启动ROS控制手柄的launch文件,用来解析下位机arduino的各传感器数据,然后组装
    并发布控制命令.
  History:
    20181024: init this launch file.
    20181105: add dynamic server node.
-->
<launch>
  <!--startup ros control handle dynamic server node -->
  <node name="handle_dynamic_reconfig" pkg="ros_handle_python" type="dynamic_server.py" output="screen" />

  <!-- startup contorl handle client node -->
  <node name="handle_python_node" pkg="ros_handle_python" type="handle_node.py" output="screen">
    <rosparam file="$(find ros_handle_python)/cfg/handle_params.yaml" command="load"/>
  </node>
</launch>

同时来看下在启动handle_python_node节点时加载的配置文件handle_params.yaml是如何编写的:

# Copyright: 2016-2018 ROS小课堂 www.corvin.cn
# Author: corvin
# Description:
#   该文件为手柄的配置参数,包括:端口号,波特率,话题发布频率等.
#   下面来依次介绍下各参数的意义:
#   port:arduino板的连接端口号.
#   baud:连接arduino的波特率,这个是在arduino代码中设置的.
#   timeout:读取数据时超时时间.
#   public_rate:发布话题的频率.
#   cmd_topic:发布控制移动的话题名称.
#   angular_z:发送角速度的最大值.
#   linear_x:发送线速度的最大值.
#   send_cmd_delay:话题中命令执行的延迟,最小需要设置为1,不能为0.
#       最大无上限,一般不超过10.这里默认设置为1,即以最快速度执行话题中命令.
# History:
#   20181024: initial this config file.

port: /dev/ttyACM0
baud: 57600
timeout: 0.5
public_rate: 20
cmd_topic: /turtle1/cmd_vel
angular_z: 1.0
linear_x: 1.0
cmd_delay: 1

到这里手柄的ROS驱动代码就全介绍完了。


0x05 运行测试

在介绍完所有代码后,接下来就可以来编译和运行测试了。编译命令就是直接在工作区根目录下执行catkin_make命令即可,测试也很简单,我们只有运行launch文件即可。运行launch文件的命令如下:

roslaunch ros_handle_python ros_handle.launch

当运行该launch文件后,通过rqt_graph来查看当前的节点和话题通信图来确认节点启动是否正常:

1542530140232365.jpeg

通过该图发现,最左边是handle_dynamic_reconfig节点,该节点负责动态更新控制手柄的配置参数。该节点与handle_python_node通信是通过parameter_descriptions和parameter_updates这两个话题,一个是用来介绍更新的动态参数,一个是更新的参数内容。最终发布的话题是/turtle1/cmd_vel,这个话题中消息就是控制手柄发布出来的控制移动命令。

从最终的话题我们就得知,要想测试手柄是否正常工作就需要订阅该话题测试。这里就需要启动ROS下的turtlesim来看看手柄是否可以控制其移动:

1.gif

2.gif

查看下此时的节点话题图:

1542530955467819.jpeg

在控制小乌龟四处移动过程中,可以将各速度数据可视化显示。这里使用rqt_plot来查看数据:

3.gif

在Topic中输入/turtle1/pose即可查看到所有的数据,默认显示的数据较多。这里我们可以先查看angular_velocity和linear_velocity是否正常,这里还需要调整显示的坐标轴量程。当我们控制小乌龟往前走时,可以发现linear_velocity一直在增大,通过旋钮可以控制该速度的大小。速度反馈正常。

下面来测试dynamic_reconfigure动态更新参数是否正常,我们在启动了launch文件后。只需要在终端中输入以下命令就可以开启动态更新节点:

rosrun rqt_reconfigure rqt_reconfigure

我们可以在不中断节点运行的情况下来更新发布控制命令的话题,如下动图所示:

4.gif


0x06 References

[1].corvin_zhang 1.使用dynamic_reconfigure实现节点参数动态更新.  https://www.corvin.cn/651.html

[2].ROS Wiki. Setting up Dynamic Reconfigure for a Node (python).

http://wiki.ros.org/dynamic_reconfigure/Tutorials/SettingUpDynamicReconfigureForANode%28python%29

[3].ROS Wiki. Using the Dynamic Reconfigure Python Client. 

http://wiki.ros.org/dynamic_reconfigure/Tutorials/UsingTheDynamicReconfigurePythonClient

[4].ROS API. Package dynamic_reconfigure. 

http://docs.ros.org/kinetic/api/dynamic_reconfigure/html/dynamic_reconfigure.client.Client-class.html


0x07 Feedback

大家在按照教程操作过程中有任何问题,可以直接在文章末尾给我留言,或者关注ROS小课堂的官方微信公众号,在公众号中给我发消息反馈问题也行。我基本上每天都会处理公众号中的留言!当然如果你要是顺便给ROS小课堂打个赏,我也会感激不尽的,打赏30块还会被邀请进ROS小课堂的微信群,与更多志同道合的小伙伴一起学习和交流!

发表评论