就如各位所見(jiàn)，使用ROS之后，一般的循環(huán)函數(shù)就會(huì)變得相當(dāng)簡(jiǎn)單。循環(huán)函數(shù)只會(huì)訂閱（subscribe）數(shù)據(jù)，任何Arduino循環(huán)都一樣。設(shè)定時(shí)要將ROS初始化，將各個(gè)ROS訊息訂閱者的訂閱叫出來(lái)。每個(gè)訂閱者會(huì)占據(jù)Arduino的RAM，數(shù)量取決于要用程序代碼做什么，以6個(gè)到12個(gè)為限。
Servo servo;
void servo_cb( const std_msgs::Float32& msg )
{
const float min = 45;
const float range = 90;
float v = msg.data;
if( v > 1 ) v = 1;
if( v < 0 ) v = 0;
float angle = min + (range * v);
servo.write(angle);
}
ros::Subscriber
ros::NodeHandle nh;
void setup()
{
servo.attach(SERVOPIN);
nh.initNode();
nh.subscribe(sub);
void loop()
{
nh.spinOnce();
delay(1); }
接下來(lái)要設(shè)法透過(guò)Arduino在ROS的世界說(shuō)話。簡(jiǎn)單的方法是使用機(jī)器人啟動(dòng)檔。雖然以下的檔案內(nèi)容非常簡(jiǎn)單，但是這里要追加啟動(dòng)檔，如此一來(lái)即使是非常復(fù)雜的機(jī)器人，也能用一個(gè)指令啟動(dòng)。
$ cat rosservo.launch
$ roslaunch ./rosservo.lanch
rostopic指令可以看出ROS訊息傳送到機(jī)器人的哪個(gè)部位?？戳讼旅娴某绦虼a就會(huì)發(fā)現(xiàn)，「/head/tilt」可以透過(guò)Arduino使用。訊息要使用「rostopic」傳送。-1的選項(xiàng)只會(huì)發(fā)布（publish）訊息一次，通知/head/tilt傳送一個(gè)浮點(diǎn)數(shù)。
$ rostopic list
/diagnostics
/head/tilt
/rosout
/rosout_agg
$ rostopic pub -1 /head/tiltstd_msgs/Float32 0.4
$ rostopic pub -1 /head/tilt std_msgs/Float320.9
這個(gè)階段當(dāng)中，能夠?qū)⑺邪l(fā)布數(shù)值到ROS的已知方法用在控制伺服機(jī)上。假如從0改成1，伺服機(jī)就會(huì)全速運(yùn)行。這本來(lái)并沒(méi)有問(wèn)題，但實(shí)際上我們想要逐漸加速以達(dá)到全速，然后再逐漸減速，停在目標(biāo)角度上。假如伺服機(jī)驟然運(yùn)轉(zhuǎn)，機(jī)器人的動(dòng)作就會(huì)變得僵硬，讓周?chē)娜藝樢惶?/p>
Terry和Houndbot都是ROS機(jī)器人，以6061個(gè)鋁合金零件制造而成。項(xiàng)目的目標(biāo)是要盡量讓這些機(jī)器人自主運(yùn)動(dòng)。
以下的Python腳本程序會(huì)監(jiān)聽(tīng)「/head/tilt/smooth」的訊息，朝「/head/tilt」發(fā)布許多訊息，好讓伺服機(jī)轉(zhuǎn)到目標(biāo)角度之前慢慢加速，再慢慢延遲旋轉(zhuǎn)。當(dāng)訊息抵達(dá)「/head/tilt/smooth」時(shí)一定會(huì)呼叫「moveServo_cb」。這個(gè)回調(diào)函式會(huì)從-90到+90度之間每10度產(chǎn)生1個(gè)數(shù)值，追加到角度數(shù)組當(dāng)中?！竤in()」會(huì)取這個(gè)角度，數(shù)值從-1到+1慢慢增加。該數(shù)值加1之后，范圍就會(huì)變成0到+2，再除以2之后， 0到+1的曲線數(shù)值數(shù)組就完成了。然后再看看m數(shù)組當(dāng)中，每當(dāng)發(fā)布訊息時(shí)，就會(huì)稍微前進(jìn)一點(diǎn)，范圍在r之內(nèi)，直到1*r或是全范圍為止。
#!/usr/bin/env python
from time import sleep
import numpy as np
import rospy
from std_msgs.msg import Float32
currentPosition = 0.5
pub = None
def moveServo_cb(data):
global currentPosition, pub
targetPosition = data.data
r = targetPosition - currentPosition
angles = np.array( (range(190)) [0::10]) -90
m = ( np.sin( angles * np.pi/ 180. ) + 1 )/2
for mi in np.nditer(m):
pos = currentPosition + mi*r
A06B-0128-B075 A06B-0502-B751 A06B-6079-H208
A06B-0128-B076 A06B-0502-B855 A06B-6079-H301
A06B-0128-B077 A06B-0503-B006 A06B-6079-H304
A06B-0128-B175 A06B-0503-B202 A06B-6079-H401
A06B-0128-B177 A06B-0503-B251 A06B-6079-K815
A06B-0128-B575 A06B-0503-B304 A06B-6081-H101
A06B-0128-B577 A06B-0505-B001 A06B-6081-H103
A06B-0147-B175 A06B-0514-B504 A06B-6088-H230
A06B-0147-B177 A06B-0521-B052 A06B-6088-H245IC200UDR064完整IC200UDR064完整IC200UDR064完整
A06B-0147-B675 A06B-0521-B541 A06B-6089-H104
A06B-0148-B075 A06B-0532-B001 A06B-6089-H203
A06B-0148-B175 A06B-0572-B003 A06B-6090-H234
A06B-0148-B177 A06B-0572-B004 A06B-6091-H118
