ROS服务Service通信机制在无人车中的具体应用

1、引言

我们在通信机制中，前面已有介绍这个Service服务：ROS通信机制之服务(Service)的应用
一般在一些通信处理比较简单的场景比较适合，这里我们在无人车上面来实际操作一些很常见的服务：发布电压数据、点亮与关闭LED灯、开启与关闭蜂鸣器、以及控制左右马达的速度等操作。

工作区与包的创建过程就忽略了，有兴趣的可以查阅：ROS新建工作区(workspace)与包(package)编译的实践(C++示例)

2、无人车应用

2.1、目录

先来了解下有哪些消息、服务、节点文件：

cd ~/workspace/catkin_ws/src
ls jetbotmini_msgs/msg
ls jetbotmini_msgs/srv
ls jetbot_ros/scripts

截图如下：

2.2、启动节点

然后我们直接开启ROSMASTER之后，启动无人车的节点，后续将主要代码贴出，更加清晰了解无人车的服务通信机制。

roscore

rosrun jetbot_ros jetbotmini_driver.py

好的，现在我们就可以对无人车进行通信了

2.3、查看话题

先来看下有哪些话题：rostopic list

这里可以看到，有了一个 voltage 电压的话题。 

2.4、查看服务

接着重点来看下提供了哪些服务列表：rosservice list

服务列表中有蜂鸣器，LED灯、马达、舵机等服务，接下来我们通过命令行来操作这些服务，让其和无人车进行通信。
服务的定义很简单，我们来具体查看下

2.5、蜂鸣器

输入命令：rossrv show Buzzer

输入类型跟输出类型之间使用三根小横短线隔开--- 

开启蜂鸣器：rosservice call /Buzzer "buzzer: 1"
result: True    #这个是返回值
关闭蜂鸣器：rosservice call /Buzzer "buzzer: 0"
result: True 

2.6、舵机

输入命令：rossrv show Servo 

我的无人车有两个舵机接口S1和S2，如下图：

舵机1角度90：rosservice call /Servo 1 90
result: True
舵机2角度90：rosservice call /Servo 2 90
result: True 

查看舵机参数：rosservice args Servo
servoid angle

舵机1旋转30度角：rosservice call /Servo 1 30

当然参数是多个的时候，也可以指定参数名称的写法，使用字典形式来执行，这样更直观：
rosservice call /Servo "{'servoid': 1, 'angle': 30}" 

2.7、LED灯

点亮LEB蓝灯：rosservice call /LEDBLUE 1
result: True
如下图：

点亮LED绿灯：rosservice call /LEDGREE 1
result: True
如下图： 

一起点亮： 

2.8、电机(马达) 

查看Motor马达的数据类型结构：rossrv show Motor

查看Motor马达的参数：rosservice args Motor

rightspeed leftspeed

左电机(马达轮子)50%速度，右马达80%速度：rosservice call /Motor 0.5 0.8 

查看有哪些话题：rostopic list 

可视化查看话题消息：rosrun rqt_topic rqt_topic
如下图，点击voltage话题的复选框进行勾选： 

这样就可以直观的看到电压是10.67V，频率是0.03赫兹(每30秒更新一次) 

3、jetbotmini_driver.py

重点来看下这个节点的代码，里面就是上述定义的各种服务，所以我们将消息和服务给导入进来：

from jetbotmini_msgs.msg import *
from jetbotmini_msgs.srv import *

jetbotmini_msgs.msg和jetbotmini_msgs.srv是自动生成的，关于消息的定义在前面也有介绍，有兴趣的可以查阅：ROS通信机制之话题(Topics)的发布与订阅以及自定义消息的实现
会根据你的Python版本生成到对应位置，比如我这里是在下面这个目录
~/workspace/catkin_ws/devel/lib/python2.7/dist-packages/jetbotmini_msgs
分别生成在这里面的msg和srv文件夹里面。
截图如下：

接着回到jetbotmini_driver.py这个文件，源码如下： 

#!/usr/bin/env python3
# encoding: utf-8
import rospy
import random
import threading
from time import sleep
from jetbotmini_msgs.msg import *
from jetbotmini_msgs.srv import *
import RPi.GPIO as GPIO # Raspberry Pi 的GPIO库
import smbus

bus = smbus.SMBus(1) # 使用编号为1的总线，用于I2C通信
ADDRESS = 0x1B
Led_G_pin = 24 # 引脚编号
Led_B_pin = 23
Key1_pin = 8
GPIO.setmode(GPIO.BCM) # BCM模式是指使用Broadcom SOC通道编号的方式
GPIO.setup(Led_G_pin, GPIO.OUT, initial=GPIO.HIGH) # 引脚设置为输出模式，初始化为高电平
GPIO.setup(Led_B_pin, GPIO.OUT, initial=GPIO.HIGH)
GPIO.setup(Key1_pin, GPIO.IN) #输入模式，可以读取引脚的状态

print("Starting now!")

class transbot_driver:
    def __init__(self):
        rospy.on_shutdown(self.cancel)
        self.srv_Buzzer = rospy.Service("/Buzzer", Buzzer, self.Buzzercallback)
        self.srv_Servo = rospy.Service("/Servo", Servo, self.Servocallback)
        self.srv_LEDBLUE = rospy.Service("/LEDBLUE", LEDBLUE, self.LEDBLUEcallback)
        self.srv_LEDGREE = rospy.Service("/LEDGREE", LEDGREE, self.LEDGREEcallback)
        self.srv_Motor = rospy.Service("/Motor", Motor, self.Motorcallback)
        self.volPublisher = rospy.Publisher("/voltage", Battery, queue_size=10)

    def cancel(self):
        self.srv_Buzzer.shutdown()
        self.srv_Servo.shutdown()
        self.srv_LEDBLUE.shutdown()
        self.srv_LEDGREE.shutdown()
        self.srv_Motor.shutdown()
        self.volPublisher.unregister()
        GPIO.cleanup()
        rospy.loginfo("Close the robot...")
        rospy.sleep(1)

    def pub_data(self):
        # 发布电池电压
        while not rospy.is_shutdown():
            sleep(30)
            AD_value = bus.read_i2c_block_data(ADDRESS,0x00,2)
            voltage = ((AD_value[0] << 8) + AD_value[1]) * 13.3 / 1023.0
            battery = Battery()
            battery.Voltage = voltage
            self.volPublisher.publish(battery)

    def Buzzercallback(self, request):
        #蜂鸣器控制，服务器回调函数
        if not isinstance(request, BuzzerRequest): return
        bus.write_byte_data(ADDRESS,0x06,request.buzzer)
        sleep(0.01)
        bus.write_byte_data(ADDRESS,0x06,request.buzzer)
        response = BuzzerResponse()
        response.result = True
        return response

    def Servocallback(self, request):
        # 控制舵机
        if not isinstance(request, ServoRequest): return
        bus.write_i2c_block_data(ADDRESS,3,[request.servoid,request.angle])
        sleep(0.01)
        bus.write_i2c_block_data(ADDRESS,3,[request.servoid,request.angle])
        response = ServoResponse()
        response.result = True
        return response

    def LEDBLUEcallback(self, request):
        # 控制蓝色LED
        if not isinstance(request, LEDBLUERequest): return
        if request.ledblue == 1:
            GPIO.output(Led_B_pin, GPIO.LOW)
        elif request.ledblue == 0:
            GPIO.output(Led_B_pin, GPIO.HIGH)
        response = LEDBLUEResponse()
        response.result = True
        return response

    def LEDGREEcallback(self, request):
        # 控制绿色LED
        if not isinstance(request, LEDGREERequest): return
        if request.ledgree == 1:
            GPIO.output(Led_G_pin, GPIO.LOW)
        elif request.ledgree == 0:
            GPIO.output(Led_G_pin, GPIO.HIGH)
        response = LEDGREEResponse()
        response.result = True
        return response

    def Motorcallback(self, request):
        # 控制马达，速度小于0就反方向
        if not isinstance(request, MotorRequest): return
        if request.leftspeed < 0:
            request.leftspeed = -request.leftspeed
            leftdir = 0
        else:
            leftdir = 1
        if request.rightspeed < 0:
            request.rightspeed = -request.rightspeed
            rightdir = 0
        else:
            rightdir = 1
        bus.write_i2c_block_data(ADDRESS,0x01,[leftdir,int(request.leftspeed*255),rightdir,int(request.rightspeed*255)])
        sleep(0.01)
        bus.write_i2c_block_data(ADDRESS,0x01,[leftdir,int(request.leftspeed*255),rightdir,int(request.rightspeed*255)])
        response = MotorResponse()
        response.result = True
        return response

if __name__ == '__main__':
    rospy.init_node("driver_node", anonymous=False)
    try:
        driver = transbot_driver()
        driver.pub_data()
        rospy.spin()
    except:
        rospy.loginfo("Final!!!")

首先初始化一些相关操作，将GPIO设置成BCM模式，然后根据引脚编号来初始化成高电平，模式是输出模式，这样就可以根据高低电平来控制LED灯，如果想要控制亮度，就需要使用到PWM脉宽调制方法。需要注意的是，cancel函数里面的GPIO.cleanup()，让引脚恢复到原来的状态，释放所有引脚不再有输出，避免一些电气的损坏与火灾的发生。
其余的都是一些回调函数，主要是一些写入和判断操作，通过这些回调函数就可以操作对应的元器件了。

这节主要是对Service服务做一个实操，巩固其作为通信机制之一的用法，在实际当中的具体运用。
其中代码里面有一些关于GPIO通用输入输出的知识点，有兴趣的可以查阅：
JetsonNano板子的RPi.GPIO接口实物图和功能介绍