2.控制手柄的ROS驱动代码
corvin_zhang
0x00 驱动代码概述
在第一篇文章中介绍了如何使用常见的硬件来搭建一个控制手柄，我们使用Arduino为核心控制板，获取各传感器数据。然后将数据组装成自定义的数据帧格式，通过串口将数据帧发送出来。这样就完成了控制手柄固件代码，但是这些数据帧我们并不能直接将其用来控制ROS下小车移动。我们需要解析数据帧内容，然后将其再次封装成ROS下控制小车移动的消息格式，最终要通过话题发送出来才可以。本篇文章将指导如何实现手柄的ROS驱动代码，首先我们需要来认识下整个控制手柄的数据流程示意图，通过该图就能对手柄的代码实现理解更为透彻：
0x01 创建驱动软件包
在这里由于我们增加了dynamic_reconfigure功能，这样就可以动态的更新节点参数了。这样就使得我们在创建软件包时新增了一个依赖项dynamic_reconfigure，完整的创建软件包命令如下：
catkin_create_pkg ros_handle_python rospy geometry_msgs dynamic_reconfigure
下面我们首先来看下整体代码的结构，这样就方便大家后面知道每个代码文件放置在什么路径下了：
在该软件包中，需要注意的是CMakeLists.txt文件，该文件需要确保有下图所示的部分才行，不然编译会出错的：
对于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通信是否正常。测试流程为首先连接串口，从中读取数据并打印出来，最后断开连接。
在进行后面的数据帧解析前，我们需要先确保该源码文件中提供的各函数功能可以正常工作。下面可以先测试下，如下图所示，如果可以从串口中读取到数据，则说明该代码是正常的：
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轴方向速度。另外就是如何来确定往前后走和左右转向，为了防止过于灵敏，我就设置了一个区间，超过该区间才认为控制有效。如下图所示：
-
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来查看当前的节点和话题通信图来确认节点启动是否正常：
通过该图发现，最左边是handle_dynamic_reconfig节点，该节点负责动态更新控制手柄的配置参数。该节点与handle_python_node通信是通过parameter_descriptions和parameter_updates这两个话题，一个是用来介绍更新的动态参数，一个是更新的参数内容。最终发布的话题是/turtle1/cmd_vel，这个话题中消息就是控制手柄发布出来的控制移动命令。
从最终的话题我们就得知，要想测试手柄是否正常工作就需要订阅该话题测试。这里就需要启动ROS下的turtlesim来看看手柄是否可以控制其移动：
查看下此时的节点话题图：
在控制小乌龟四处移动过程中，可以将各速度数据可视化显示。这里使用rqt_plot来查看数据：
在Topic中输入/turtle1/pose即可查看到所有的数据，默认显示的数据较多。这里我们可以先查看angular_velocity和linear_velocity是否正常，这里还需要调整显示的坐标轴量程。当我们控制小乌龟往前走时，可以发现linear_velocity一直在增大，通过旋钮可以控制该速度的大小。速度反馈正常。
下面来测试dynamic_reconfigure动态更新参数是否正常，我们在启动了launch文件后。只需要在终端中输入以下命令就可以开启动态更新节点：
rosrun rqt_reconfigure rqt_reconfigure
我们可以在不中断节点运行的情况下来更新发布控制命令的话题，如下动图所示：
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.
0x07 Feedback
大家在按照教程操作过程中有任何问题，可以直接在文章末尾给我留言，或者关注ROS小课堂的官方微信公众号，在公众号中给我发消息反馈问题也行。我基本上每天都会处理公众号中的留言！当然如果你要是顺便给ROS小课堂打个赏，我也会感激不尽的，打赏30块还会被邀请进ROS小课堂的微信群，与更多志同道合的小伙伴一起学习和交流！
[wshop_reward]
