好久以前我寫過這麼一篇博客，當時是爲了單獨控制機械臂夾爪。前些天突然發現我一直都弄錯了，我以爲我用的是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控制舵機的區別
2.修改arduino_node.py（/ros_arduino_servo/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py）
正文
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小課堂官網