ros_arduino_bridge控制舵机
好久以前我写过这么一篇博客，当时是为了单独控制机械臂夹爪。前些天突然发现我一直都弄错了，我以为我用的是ros_arduino_bridge，事实上我用的是rosserial_arduino。
这是当初的标题，现在已经改掉了。
这篇博客会先提一下在ros中使用ros_arduino_bridge和rosserial_arduino控制舵机的区别，然后根据ros_arduino_bridge包进行修改写一个ros_arduino_bridge控制舵机的例子。
Table of Contents
正文
一、ros_arduino_bridge和rosserial_arduino控制舵机的区别
二、arduino部分
三、ros部分
1.下载ros_arduino_bridge包
2.修改arduino_node.py（/ros_arduino_servo/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py）
3.修改 arduino_driver.py（ros_arduino_servo/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py）
4.新建srv文件
5.编译并运行
正文
github连接：https://github.com/zhangjiali1201/ros_and_arduino
一、ros_arduino_bridge和rosserial_arduino控制舵机的区别
最主要的区别就是rosserial_arduino将控制舵机这件事视为一个话题，通过往话题发消息来控制舵机，这种方法易操作，也不需要获知通信协议。我之前博客有写过rosserial_arduino控制舵机。
ros_arduino_bridge如其名所言，在ros和arduino直接搭建了一座桥梁，可以直接从ros通过串口和arduino通信，稳定性高。如果下位机代码都是自己写的，知道通信协议，用这种方式更好。然后用服务，或者用话题都可以控制舵机。
二、arduino部分
arduino部分代码主要参考了ROS小课堂人脸追踪云台的开源代码。具体链接我会全部列在参考2里。
ROS小课堂使用的是DFROBOT的arduino uno+拓展版，我们这里就简单一些，直接使用arduino uno接个小舵机。接线方式还是老样子，舵机的红线接5V，黑线接地，数据线（棕色或者橙色居多）接9或10。
在arduino主程序代码中，增加了一个z指令方便控制舵机，在串口监视器输入z 1舵机旋转到45°，输入z 2旋转到90°，以此类推。
case SERVO_TEST: // 'z'
if (serialObj.arg1 == 0)
{
myServos[0].setTargetPos(0, 10);
}
else if (serialObj.arg1 == 1)
{
myServos[0].setTargetPos(45, 10);
}
else if (serialObj.arg1 == 2)
{
myServos[0].setTargetPos(90, 10);
}
else if (serialObj.arg1 == 3)
{
myServos[0].setTargetPos(120, 10);
}
Serial.println("Done");
break; 在servos.h文件中根据实际情况，把舵机引脚设为9、10.
//Define All Servos's Pins
byte myServoPins[N_SERVOS] = {9, 10};
三、ros部分
1.下载ros_arduino_bridge包
ros_arduino_bridge的wiki界面：http://wiki.ros.org/ros_arduino_bridge
$ cd ~/catkin_ws/src
$ git clone https://github.com/hbrobotics/ros_arduino_bridge.git下载之后可以看到这个包本身就比较完备，我们只要基于这个包修改就可以。
2.修改arduino_node.py（/ros_arduino_servo/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py）
self.port = rospy.get_param("~port", "/dev/ttyACM0")45行这里获取了arduino端口信息的参数，后一个双引号内的是默认值，如果在参数文件没有设定就会采用这个默认值 ，实际使用中可以在ros_arduino_servo/ros_arduino_bridge/ros_arduino_python/config/arduino_params.yaml文件修改，无需编译直接生效。
self.baud = int(rospy.get_param("~baud", 57600))这里同样获取了一个参数波特率，要和arduino代码一致。另外，使用arduino ide的串口监视器时，也要设置对波特率，不然发送的指令是无效的，像下图右下角这样设置即可。
八十行左右写了很多服务，跟在后面加上一个用来控制舵机的服务。
# A service to control servo
rospy.Service('~servo_control', ServoControl, self.ServoControlHandler)两百行左右的地方同样增加一下
def ServoControlHandler(self, req):
self.controller.servo_control(req.value)
return ServoControlResponse()3.修改 arduino_driver.py（ros_arduino_servo/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py）
在330行左右新增
def servo_control(self, operation):
''' control gripper: 0:0 degree 1:45 degrees 2:90 degrees 3:120 degrees
'''
return self.execute_ack('z %d' %(operation))这个函数用来构建和arduino通信规则之间的联系。
4.新建srv文件
在ros_arduino_servo/ros_arduino_bridge/ros_arduino_msgs/srv新建ServoControl.srv文件
uint8 value
---因为我们的操作指令只有0,1,2,3，所以八位整型就行。
不要忘记在CMakeLists.txt（ros_arduino_servo/ros_arduino_bridge/ros_arduino_msgs）中加入这个srv文件，否则编译的时候将不会编译这个文件。
add_service_files(FILES
DigitalSetDirection.srv
DigitalWrite.srv
DigitalRead.srv
ServoRead.srv
ServoWrite.srv
AnalogWrite.srv
AnalogRead.srv
ServoControl.srv #add srv
)5.编译并运行
返回~/catkin_ws目录编译一下，如果没有在.bashrc文件加入source的话还得source一下。
然后运行launch文件
$ roslaunch ros_arduino_python arduino.launch运行成功如下图
可以打开service列表查看一下，可以看到有我们之前新增的servo_control服务了。
$ rosservice list
/arduino/analog_read
/arduino/analog_write
/arduino/digital_read
/arduino/digital_set_direction
/arduino/digital_write
/arduino/get_loggers
/arduino/servo_control
/arduino/servo_read
/arduino/servo_write
/arduino/set_logger_level
/rosout/get_loggers
/rosout/set_logger_level
往该服务发个消息就能看到舵机运动。
例如：
$ rosservice call /arduino/servo_control 2就可以看到舵机转为90度。
参考：
1.ROS技巧系列 - ROS与Arduino进行高效稳定的串口通信（两种通信方式对比）
2.ROS小课堂官网
人脸跟踪底层固件代码教程文章
人脸追踪平台代码