第十章服務機器人綜合應用目錄SCIENCEANDTECHNOLOGY01引言02多人辨識項目03GPSR項目04超市購物項目引言0110.1.1服務機器人發(fā)展背景技術融合驅動創(chuàng)新服務機器人的快速發(fā)展得益于人工智能、物聯網、5G通信等多項前沿技術的深度融合，例如計算機視覺技術使機器人能夠識別復雜環(huán)境，自然語言處理技術實現人機無障礙交互，為服務場景落地奠定基礎。市場需求持續(xù)爆發(fā)隨著人口老齡化加劇和勞動力成本上升，餐飲、醫(yī)療、物流等領域對自動化服務的需求激增，2023年全球服務機器人市場規(guī)模已突破200億美元，年復合增長率超過25%。政策支持加速普及中國"十四五"機器人產業(yè)發(fā)展規(guī)劃明確提出重點發(fā)展服務機器人，各地政府通過補貼政策推動機器人在社區(qū)養(yǎng)老、智慧醫(yī)院等場景的規(guī)?；瘧茫纬闪夹园l(fā)展生態(tài)。10.1.2機器人競賽推動技術進步算法優(yōu)化迭代平臺人才培育重要渠道硬件創(chuàng)新試驗場如RoboCopen等國際賽事通過設置家庭服務、醫(yī)療救援等實戰(zhàn)場景，推動SLAM（同步定位與建圖）算法精度從厘米級提升至毫米級，路徑規(guī)劃效率提高40%以上。競賽中涌現出柔性機械臂、多模態(tài)傳感器融合等突破性技術，DARPA機器人挑戰(zhàn)賽促使抗干擾通信模塊的傳輸穩(wěn)定性達到99.99%。清華大學團隊通過參加服務機器人競賽，開發(fā)出具有自主知識產權的室內導航系統(tǒng)，其動態(tài)避障響應時間縮短至0.3秒，培養(yǎng)出20余名專業(yè)研發(fā)人才。多人辨識項目0210.2.1任務目標技術需求多人辨識項目是測試居家服務機器人能夠在陌生環(huán)境中自主地識別人。不可以手動校準，機器人必須向一組人自我介紹，問他們的名字，記住他們，當再次遇見的時候能夠認出他們。關鍵步驟該測試的關鍵是人的檢測/識別，臉部的檢測/識別，安全導航，與陌生人的人機互動。需具備功能該項目需要實現機器人基礎移動功能、語音識別功能、人臉識別功能、建圖功能、導航功能、物品抓取功能，并將這些功能整合到ROS中。10.2.2任務、場景、硬件架構任務列表理解名字理解所需物品拿到所需物品判斷出一個人辨別人的名字識別出人將物品給予對應的人在指定時間內完成所有任務10.2.3多人辨識各項功能（1）基礎移動功能：啟動底盤驅動：#!/bin/bash

cd

/home/robot/catkin_ws/src/dashgo/src/dashgo_driver/launch

source

~/catkin_ws/devel_isolated/setup.bash

roslaunch

driver.launch

（2）語音識別功能：使用科大訊飛的語音聽寫（iat）和語音合成（tts）功能，啟動語音聽寫：ros::Subscriber

sub_sr

=

n.subscribe("/xfyun/iat",

10,

KeywordCB);

static

xfyun_waterplus::IATSwitch

srvIAT;

srvIAT.request.active

=

true;

srvIAT.request.duration

=

6;

clientIAT.call(srvIAT);

啟動語音合成：sound_play::SoundClient

sc;

sc.say(inStr);

10.2.3多人辨識各項功能（3）人臉識別功能：

使用華為云人臉識別，由于華為云人臉識別使用python語言編寫，故在ROS中使用腳本進行調用。

char

order[100];

sprintf(order,

"cd

~/HWFace

&&

python3

face_reco.py");

n

=

system(order);

（4）建圖功能：多人辨識項目需要提前構建好地圖，啟動建圖：#!

/bin/bash

source

~/catkin_ws/devel_isolated/setup.bash

roslaunch

cartographer_ros

demo_revo_lds.launch

（5）導航功能：多人辨識項目需要在運行主程序前啟動導航并校正自身位姿，啟動導航：#!

/bin/bash

cd

/home/robot/bash

source

~/catkin_ws/devel_isolated/setup.bash

gnome-terminal

--

bash

imu.sh

sleep

1

gnome-terminal

--

bash

lidar.sh

sleep

1

gnome-terminal

--

bash

3to2.sh

sleep

1

gnome-terminal

--

bash

start_navigation.sh

sleep

1

gnome-terminal

--

bash

add_waypoint.sh

sleep

1

標定航點，啟動導航后，使用Addwaypoints標定一個或幾個航點，完成后使用如下腳本保存航點至waypoints.xml文件：#!

/bin/bash

source

~/catkin_ws/devel_isolated/setup.bash

rosrun

waterplus_map_tools

wp_saver

10.2.3多人辨識各項功能（6）程序的運行由有限狀態(tài)機判斷當前狀態(tài)，并進行切換：//有限狀態(tài)機

#define

STATE_READY

0

#define

STATE_WAIT_ENTR

1

#define

STATE_WAIT_RECO

2

#define

STATE_CONFIRM

3

#define

STATE_GOTO_EXIT

4

#define

STATE_GOTO_LOCATION

5

#define

STATE_GOTO_CMD

6

#define

STATE_WAIT_FACE

7

#define

STATE_CATCH

8

#define

STATE_PHOTO

9

#define

STATE_GOTO_BACK

10

（7）設置人名和物品關鍵詞：//識別關鍵詞

static

vector<string>

arKWPerson;

static

vector<string>

arKWConfirm;

static

vector<string>

arKWObject;

static

void

Init_keywords()

{

//人名關鍵詞

arKWPerson.push_back("Alex");

arKWPerson.push_back("Angel");

arKWPerson.push_back("Edward");

//物品關鍵詞

arKWObject.push_back("herbal

tea");

arKWObject.push_back("water");

arKWObject.push_back("cola");

//yes

or

no

arKWConfirm.push_back("yes");

arKWConfirm.push_back("Yes");

arKWConfirm.push_back("Yeah");

arKWConfirm.push_back("year");

arKWConfirm.push_back("no");

arKWConfirm.push_back("No");

}

（1）進門并介紹自己：bool

aArrived

=

Goto("start");

Speak("Please

tell

me

your

name

and

what

you

want");

10.2.4多人辨識流程切換狀態(tài)，等待語音識別：nState

=

STATE_WAIT_RECO;

獲取客人說的物品和人名信息并將其保存在內存中，等待客人確認：string

person

=

FindWord(strListen,

arKWPerson);

string

object

=

FindWord(strListen,

arKWObject);

string

strRepeat

=

"your

name

is

"

+

person

+

","

+

"you

want

"

+

object;

strcpy(obj[objnum],

object.c_str());

strcpy(per[pernum],

person.c_str());

Speak(strRepeat);

string

where

=

"please

confirm";

nState

=

STATE_CONFIRM;

10.2.4多人辨識流程（2）客人確認后進入拍照狀態(tài)：nState

=

STATE_PHOTO;

（3）啟動攝像頭拍照，并上傳到華為云人臉庫中：Speak("Please

don't

move");

sprintf(order,

"cd

~/HWFace

&&

python3

photo.py

%s",

per[pernum

-

1]);

//格式化命令，替換

n

=

system(order);

如此，依次識別三個客人，切換狀態(tài)：nState

=

STATE_GOTO_LOCATION;

到達物品所在的地點并抓取物品：fArrived

=

Goto(placestr);

if

(fArrived

==

true)

{

Speak("I

am

in

the"

+

placestr);

nState

=

STATE_CATCH;

}

10.2.4多人辨識流程（4）返回指定地點，啟動人臉識別，識別客人：bool

fArrived;

fArrived

=

Goto("back");

char

order[100];

sprintf(order,

"cd

~/HWFace

&&

python3

face_reco.py");

n

=

system(order);

nState

=

STATE_WAIT_FACE;

（5）依次將拿到的物品給相應的客人后，離開場地：if

(times

==

3)

nState

=

STATE_GOTO_EXIT;

GPSR項目0310.3.1任務目標技術需求該項目是為了測試機器人的綜合能力，測試重點包括跟隨、定位、導航、抓取、人的探測以及識別、物體的探測以及識別等。在這個測試中機器人需要解決被要求的多個任務。技術難點是，在這個測試中，沒有預定義場景和預定義的可以由確定的基本動作序列完成的任務。完成這個測試需要的動作和任務都是由裁判現場抽取的。關鍵步驟(1)沒有特定順序的動作集(因此這個任務不能由預先定義好的狀態(tài)機編程來完成)。(2)增強的語音識別、處理能力(由于任務是不確定的，因此語音也是不確定的，任務不是單純的動作或單個物體，可能包含多個物體和動作，例如：“將杯子放在廚房的桌子上(putthecuponthekitchentable)”。需具備功能通常來說，機器人開展業(yè)務工作需要從行動類、物體類和位置類的集合中生成具體能力：(1)行動集合A，例如尋找特定人、跟隨等，抓取和運送物體等。(2)物體集合B，該集合由放在場地中的10個物體構成。(3)位置集合L，該集合根據測試場地條件確定，如果涉及到抓取任務，那么抓取的位置需在機器人的可達高度范圍內。10.3.2任務、硬件架構、軟件流程任務列表理解部分命令完成部分任務理解全部命令完成全部任務由陌生人下達指令10.3.3GPSR項目功能(1)程序啟動時開啟語音識別、地圖導航功能typedef

actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>

MoveBaseClient;

static

ros::Publisher

spk_pub;

static

ros::ServiceClient

clientIAT;

static

xfyun_waterplus::IATSwitch

srvIAT;

static

ros::ServiceClient

cliGetWPName;

static

waterplus_map_tools::GetWaypointByName

srvName;

static

int

nPersonCount

=

0;

(2)定義有限狀態(tài)機#define

STATE_WAIT_ENTR

0

//初始狀態(tài)

#define

STATE_WAIT_RECO

1

//從任務語句中判斷需要到達的位置

#define

STATE_GOTO_LOCATION

2

//到某一地點

#define

STATE_WAIT_ASK

3

//等待詢問

#define

STATE_CONFIRM

4

//記錄任務到txt文件中，并執(zhí)行小任務

#define

STATE_GOTO_EXIT

5

//到出口

#define

STATE_GOTO_LOCATION2

6

//到第二個地點

10.3.3GPSR項目功能(3)定義全局變量int

taskflag

=

0;//任務標志

1-回答問題

2-回答小問題3-follow4-抓取物品

5-找東西char

strOrder[100];//記錄聽到的命令

char

obj[20];//記錄物品

char

pla[20];//記錄地點

char

pla2[20];//記錄拆分地點2

static

int

nState

=

STATE_WAIT_ENTR;

//程序啟動時初始狀態(tài)

(4)存儲指令關鍵詞的結構體static

vector<string>

arKWPerson;//人名關鍵詞

static

vector<string>

arKWConfirm;//回應關鍵詞

static

vector<string>

arKWPlacement;//大地點(房間)關鍵詞

static

vector<string>

arKWPlacement2;//大地點中包含的小地點關鍵詞

static

vector<string>

arKWObject;//物品關鍵詞

static

vector<string>

arKWAction1;//行為關鍵詞1，對應大地點

static

vector<string>

arKWAction2;//行為關鍵詞2，對應小地點

static

vector<string>

arKWQuestion;//大問題關鍵詞(需要單獨詢問的問題)

static

vector<string>

arKWSmallQuestion;//小問題關鍵詞(包含在指令中的問題)

10.3.4GPSR項目流程程序運行過程以機器人收到指令“grasptheHerbalteafromthediningtableanddeliverittotheleftcookingbench，將涼茶從餐桌帶到左邊灶臺”為例，進行接下來的程序執(zhí)行。10.3.4GPSR項目流程起始狀態(tài)：機器人自行移動到發(fā)令點me，等待發(fā)令人發(fā)出指令。bool

bArrived

=

Goto("me");

Speak("Tell

me

the

command.");

nState

=

STATE_WAIT_RECO;

10.3.4GPSR項目流程（2）機器人接到發(fā)令人發(fā)送的指令后，從聽到的句子里尋找指令關鍵詞，并確定機器人接下來的任務(到哪些地點，找什么人或物品，回答什么問題)，播報聽到的指令，等待語音確認。string

location

=

FindWord(strListen,

arKWPlacement);

string

location2

=

FindWord(strListen,

arKWPlacement2);

string

action1

=

FindWord(strListen,

arKWAction1);

string

action2

=

FindWord(strListen,

arKWAction2);

string

object

=

FindWord(strListen,

arKWObject);

Speak("Please

confirm!");

nState

=

STATE_CONFIRM;

10.3.4GPSR項目流程（3）發(fā)令人反饋語音確認指令后，機器人對收到的確認語句進行判斷。如果確認語句是yes、yeah等，則解析第一個任務關鍵詞，確定taskflag（此處taskflag為4-拿東西），并導航到第一個目標點（此處為diningtable）；如果語句是no，則要求重新下發(fā)指令。10.3.4GPSR項目流程if

(confirm

==

"yes"

||

confirm

==

"Yes"

||

confirm

==

"Yeah")

{

if

(strstr(strOrder,

"answer")

!=

NULL)

{

taskflag

=

1;

//回答大問題

}

else

if

(strstr(strOrder,

"say")

!=

NULL

||

strstr(strOrder,

"tell")

!=

NULL

||

strstr(strOrder,

"speak")

!=

NULL)

{

taskflag

=

2;

//回答小問題

}

else

if

(action2

==

"follow"

||

action2

==

"followed")

{

taskflag

=

3;

//follow

}

else

if

(action2

==

"take"

||

action2

==

"Take"

||

action2

==

"carry"

||

action2

==

"Carry"

||

action2

==

"get"

||

action2

==

"Get"

||

action2

==

"grasp"

||

action2

==

"Grasp")

{

taskflag

=

4;

//拿東西

}

else

if

(strstr(strOrder,

"look

for")

!=

NULL

||

strstr(strOrder,

"find")

!=

NULL

||

strstr(strOrder,

"Get

into")

!=

NULL)

{

taskflag

=

5;

//找東西

}

nState

=

STATE_GOTO_LOCATION;

}

if

(confirm

==

"no"

||

confirm

==

"No")

{

Speak("ok,tell

me

the

command

again");

nState

=

STATE_WAIT_RECO;

}

（4）從發(fā)令點導航到第一個目標點dinningtable10.3.4GPSR項目流程（5）機器人導航到目標點后，根據taskflag的值完成相應的小任務（此處任務為將涼茶帶到下一目標點leftcookingbench，taskflag為4：抓取涼茶），切換狀態(tài)為STATE_GOTO_LOCATION2。

if

(fArrived1

==

true)

{

fArrived2

=

Goto(placestr);

if

(fArrived2

==

true)

{

Speak("I

am

in

the

"

+

placestr);

usleep(1

*

1000

*

1000);

if

(taskflag

==

1)

{

//大問題

Speak("I

have

found

you");

usleep(1

*

1000

*

1000);

Speak("Please

tell

me

the

question");

nState

=

STATE_WAIT_ASK;

bGotoExit

=

true;

}

else

if

(taskflag

==

2)

{

//小問題

if

(smallQ

==

"your

team")

{

Speak("SDJU

team");

printf("SDJU

team");

m

=

fprintf(fp1,

"answer:

%s\n",

"SDJU

team");

}

nState

=

STATE_GOTO_LOCATION2;

}

}10.3.4GPSR項目流程（6）機器人完成任務后，如果指令中需要去到另一個地點，則導航到該點，如果機械爪抓取了一個物品，則到達導航點后運行fang.sh腳本將機械爪上的物品放下，切換狀態(tài)為STATE_GOTO_EXIT。if

(nState

==

STATE_GOTO_LOCATION2)

{

int

n;

bool

fArrived;

string

placestr2

=

pla2;

printf("pla2:%s\n",

pla2);

fArrived

=

Goto(placestr2);

if

(fArrived

==

true)

{

Speak("Here

you

are!");

n

=

system("bash

/home/robot/fang.sh");

nState

=

STATE_GOTO_EXIT;

bGotoExit

=

true;

}

}

10.3.4GPSR項目流程（7）程序判別如果狀態(tài)為STATE_GOTO_EXIT，則離場if

(nState

==

STATE_GOTO_EXIT

&&

bGotoExit

==

true)

{

bGotoExit

=

false;

bool

fArrived;

//識別完畢,關閉語音識別

srvIAT.request.active

=

false;

clientIAT.call(srvIAT);

Speak("I

am

leaving

to

exit.");

system("cd

/home/robot/gpsrorder

&&

sh

gpsr_pdf.sh");

fArrived

=

Goto("exit");

}

超市購物項目0410.4.1任務目標引導階段機器人通過引導穿過商店（為確保比賽的順利進行，允許引導者為其中一名隊員）。起點為一些固定的入口，比如超市的主入口，引導者向機器人介紹3個地點。在每個地點，引導者引導機器人到一個特定的貨架處，告訴機器人需要獲取貨架上的哪一個物體。該物體取自機器人可操作的物品欄（由該隊伍自行指定）。引導者需要在至少50cm處告知機器人可操作的物體。至于引導者使用何種方式告知機器人是沒有限制的，比如，可以用手指指向物體或者僅僅是看向物體，機器人也需要有明確的反應，語音方式回復，如果回復不清楚，可以查看PDF記錄文件。3個地點都到達后，引導者引導機器人去（預先指定的）收銀臺。操作和導航階段該階段，由2位在收銀臺附近位置的志愿者告知機器人去貨架拿取物體。但機器人要求走到這2位志愿者面前詢問所需的物品。機器人需要從相應的貨架處取回裁判指定的2個物體，并且將它交給在收銀臺等候的志愿者。設想一下，比如你忘了一些東西，想要機器人去幫你拿過來。2個物體都取回后，機器人到達收銀臺區(qū)域，結束。其他引導者應該以自然的方式行走，例如：不能往回走。該比賽可以安排在任何真正的商店或超市進行。如果后者沒有可能的話，可在任意一個包含多個貨架的房間進行測試。唯一的要求是，這個房間不是其他項目比賽場地的一部分，在該項比賽開始前所有隊伍應該不知道該比賽場地。比賽用的場地將會由組織委員會決定，如貨架位置，收銀臺位置，比賽開始和結束位置。出于安全原因，參賽隊應該派出第二個成員需要跟著機器人及其引導者。10.4.2原理分析環(huán)境地圖構建機器人來到未知環(huán)境中的未知位置，需跟隨引導人邊移動邊描繪出環(huán)境的完全地圖，所謂完全的地圖是指不受障礙行進到房間可進入的每個角落。在此項目的跟隨引導階段，機器人需開啟slam功能對周圍環(huán)境進行掃描并將場景保存為二維柵格地圖，期間需要開啟激光雷達產生掃描點云生成地圖，同時還需要輪式里程計、IMU等相對位姿推估傳感器進行位移測量，以及開啟輪式底盤移動機器人。導航機器人需要通過語音交互獲取導航點的信息，待導航點的坐標獲取完成，需要找到一條最合適的路徑，這屬于路徑規(guī)劃的內容，主要包括全局路徑規(guī)劃與局部規(guī)劃。人體跟隨機器人人體跟隨功能包含了兩個步驟：1.知道目標在哪2.能跟著目標運動。在跟隨的過程中，需要處理障礙物的躲避，因此需要添加兩個模塊，識別障礙，躲避障礙。10.4.3硬件及軟件流程10.4.4超市購物項目功能（1）人體跟隨在比賽開始階段，機器人模仿在超市購物的場景，跟隨人體并開啟語音交互。這個階段主要完成以下兩個任務：1）在跟隨的同時對場景進行二維平面建圖；2）通過語音交互，記錄下“超市”貨架上的物品在地圖上的坐標。ros::init(argc,

argv,

"wpb_home_shopping");

ros::NodeHandle

n;

ros::Subscriber

sub_sr

=

n.subscribe("/xfyun/iat",

10,

KeywordCB);

cliGetWPName

=

n.serviceClient<waterplus_map_tools::GetWaypointByName>("/waterplus/get_waypoint_name");

add_waypoint_pub

=

n.advertise<waterplus_map_tools::Waypoint>("/waterplus/add_waypoint",

1);

spk_pub

=

n.advertise<sound_play::SoundRequest>("/robotsound",

20);

spk_msg.sound

=

sound_play::SoundRequest::SAY;

spk_mand

=

sound_play::SoundRequest::PLAY_ONCE;

vel_pub

=

n.advertise<geometry_msgs::Twist>("/cmd_vel",

10);

10.4.4超市購物項目功能follow.sh腳本中包含兩部分腳本，它們分別是video1.sh和follower.sh。video1.sh是kinectV1驅動程序，它的主要功能是使能kinect攝像頭。video1.sh

#!

/bin/bash

cd

/home/robot/batkin/src/freenect_stack

source

~/batkin/devel_isolated/setup.bash

roslaunch

freenect_launch

freenect.launch

跟隨：這部分主要通過腳本follow.sh運行，腳本內容如下：follow.sh

#!

/bin/bash

gnome-terminal

--

bash

video1.sh

sleep

1

gnome-terminal

--

bash

follower.sh

sleep

1

follower.sh腳本的主要功能是啟動follow跟隨功能。follower.sh

#!

/bin/bash

cd

/home/robot/batkin/src/turtlebot_apps/turtlebot_follower/launch

source

~/batkin/devel/setup.bash

roslaunch

follower.launch

10.4.4超市購物項目功能（2）建圖階段在此階段，機器人在跟隨人的同時完成同步定位與建圖slam，此功能主要包括激光雷達、慣性測量單元IMU與機器人底盤輪式里程計互相協(xié)同完成。其中激光雷達用到的腳本包括lidar.sh、imu.sh和mapping.sh三個文件。運行l(wèi)idar.sh腳本文件的代碼如下：

#!

/bin/bash

source

~/batkin/devel_isolated/setup.bash

roslaunch

/home/robot/batkin/src/ros_rslidar/rslidar_pointcloud/launch/rs_lidar_16.launch

運行imu.sh腳本文件的代碼如下：

#!

/bin/bash

source

~/batkin/devel_isolated/setup.bash

roslaunch

stim

stim.launch

10.4.4超市購物項目功能（2）建圖階段在此階段，機器人在跟隨人的同時完成同步定位與建圖slam，此功能主要包括激光雷達、慣性測量單元IMU與機器人底盤輪式里程計互相協(xié)同完成。其中激光雷達用到的腳本包括lidar.sh、imu.sh和mapping.sh三個文件。運行l(wèi)idar.sh腳本文件的代碼如下：

#!

/bin/bash

source

~/batkin/devel_isolated/setup.bash

roslaunch

/home/robot/batkin/src/ros_rslidar/rslidar_pointcloud/launch/rs_lidar_16.launch

運行imu.sh腳本文件的代碼如下：

#!

/bin/bash

source

~/batkin/devel_isolated/setup.bash

roslaunch

stim

stim.launch

運行建圖包mapping.sh腳本文件的代碼如下：

#!

/bin/bash

gnome-terminal

--

bash

imu.sh

sleep

1

gnome-terminal

--

bash

lidar.sh

sleep

1

gnome-terminal

--

bash

3to2.sh

sleep

1

gnome-terminal

--

bash

creat_map.sh

10.4.4超市購物項目功能（2）建圖階段在此階段，機器人在跟隨人的同時完成同步定位與建圖slam，此功能主要包括激光雷達、慣性測量單元IMU與機器人底盤輪式里程計互相協(xié)同完成。在建圖階段，還需要通過語音交互的方式對特定點進行記錄，其中監(jiān)聽語音的主要函數是FindWord(stringinSentence,vector<string>&arWord)，主要功能為監(jiān)聽語音中的關鍵語句，通過識別關鍵詞進行語義識別，函數定義為：

//

從句子里找arKeyword里存在的關鍵詞

voidFindWord(stringinSentence,vector<string>&arWord)

static

string

FindKeyword(string

inSentence)

{

string

res

=

"";

int

nSize

=

arKeyword.size();

for

(int

i

=

0;

i

<

nSize;

i++)

{

int

nFindIndex

=

inSentence.find(arKeyword[i]);

if

(nFindIndex

>=

0)

{

res

=

arKeyword[i];

break;

}

}

return

res;

}

10.4.4超市購物項目功能當機器人獲取到相應語音信息，需要將當前位置保存為新航點，主要核心函數為AddNewWaypoint(stringinStr)，其主要功能為記錄下當前坐標，

并與語音中的地名相匹配。函數定義如下：

//

將機器人當前位置保存為新航點

void

AddNewWaypoint(string

inStr)

{

tf::TransformListener

listener;

tf::StampedTransform

transform;

try

{

listener.waitForTransform("/map",

"/base_link",

ros::Time(0),

ros::Duration(10.0));

listener.lookupTransform("/map",

"/base_link",

ros::Time(0),

transform);

}

catch

(tf::TransformException

&ex)

{

ROS_ERROR("[lookupTransform]

%s",

ex.what());

return;

}

float

tx

=

transform.getOrigin().x();

float

ty

=

transform.getOrigin().y();

tf::Stamped<tf::Pose>

p

=

tf::Stamped<tf::Pose>(tf::Pose(transform.getRotation(),

tf::Point(tx,

ty,

0.0)),

ros::Time::now(),

"map");

geometry_msgs::PoseStamped

new_pos;

tf::poseStampedTFToMsg(p,

new_pos);

waterplus_map_tools::Waypoint

new_waypoint;

new_

=

inStr;

new_waypoint.pose

=

new_pos.pose;

add_waypoint_pub.publish(new_waypoint);

ROS_WARN("[New

Waypoint]

%s

(

%.2f

,

%.2f

)",

new_.c_str(),

tx,

ty);

}

10.4.4超市購物項目功能（3）語音播報 語音播報是指機器人獲取人的語音指令之后，需要做出的語音反饋，如“ok”、”Whatdoyouwant?”、”O(jiān)K.Iwillgotogetitforyou.”等等，通常需要用到語音播放函數speak，函數體為staticvoidSpeak(stringinStr)。以下是函數定義：

static

void

Speak(string

inStr)

int

tmp;

char

order[100];

sprintf(order,

"rosrun

sound_play

say.py

'%s'",

inStr.c_str());

printf("order:%s\n",

order);

int

n

=

system(order);

通常需要調用say.py腳本來播報相應order指令。10.4.4超市購物項目功能（4）定點導航待機器人到達收銀臺，完成建圖與保存相應點的任務，此時需要獲取志愿者的指令，并從指令中提取關鍵地點（如apple，lemontea），隨后返回這些導航點取回相應物品。定點導航主要用到了Goto函數。定義如下：

static

bool

Goto(string

inStr)

string

strGoto

=

inStr;

srvN

=

strGoto;

if

(cliGetWPName.call(srvName))

{

std::string

name

=

srvN;

float

x

=

srvName.response.pose.position.x;

float

y

=

srvName.response.pose.position.y;

ROS_INFO("Get_wp_name:

name

=

%s

(%.2f,%.2f)",

strGoto.c_str(),

x,

y);

MoveBaseClient

ac("move_base",

true);

if

(!ac.waitForServer(ros::Duration(5.0)))

{

ROS_INFO("The

move_base

action

server

is

no

running.

action

abort...");

return

false;

}

else

{

move_base_msgs::MoveBaseGoal

goal;

goal.target_pose.header.frame_id

=

"map";

goal.target_pose.header.stamp

=

ros::Time::now();

goal.target_pose.pose

=

srvName.response.pose;

ac.sendGoal(goal);

ac.waitForResult();

if

(ac.getState()

==

actionlib::SimpleClientGoalState::SUCCEEDED)

{

ROS_INFO("Arrived

at

%s!",

strGoto.c_str());

return

true;

}

else

{

ROS_INFO("Failed

to

get

to

%s

...",

strGoto.c_str());

return

false;

}

}

}

else

{

ROS_ERROR("Failed

to

call

service

GetWaypointByName");

return

false;

}

10.4.4超市購物項目功能待機器人獲取到這些導航點，需要自主導航到這些點：

//

從識別結果句子中查找物品（航點）關鍵詞

string

strKeyword

=

FindKeyword(msg->data);

int

nLenOfKW

=

strlen(strKeyword.c_str());

if

(nLenOfKW

>

0)

{

//

發(fā)現物品（航點）關鍵詞

strGoto

=

strKeyword;

string

strSpeak

=

strKeyword

+

"

.

OK.

I

will

go

to

get

it

for

you.";

Speak(strSpeak);

printf("

OK.

I

will

go

to

get

it

for

you.\n");

nState

=

STATE_GOTO;

}

然后返回到收銀臺：if

(ac.getState()

==

actionlib::SimpleClientGoalState::SUCCEEDED)

{

ROS_INFO("Arrived

at

%s!",

strGoto.c_str());

Speak("Hi,master.

This

is

what

you

wanted.");

nState

=

STATE_PASS;

//nDelay

=

0;

}

else

{

ROS_INFO("Failed

to

get

to

%s

...",

strGoto.c_str());

Speak("Failed

to

go

to

the

master.");

//nState

=

STATE_ASK;

}

10.4.4超市購物項目功能離場：待導航任務完成，結束任務，機器人離場GPSR項目0510.5.1技術需求項目概要該項目是為了測試機器人的綜合能力，測試重點包括跟隨、定位、導航、抓取、人的探測以及識別、物體的探測以及識別等。在這個測試中機器人需要解決被要求的多個任務。技術難點是，在這個測試中，沒有預定義場景和預定義的可以由確定的基本動作序列完成的任務。完成這個測試需要的動作和任務都是由裁判現場抽取的。重點考察(1)沒有特定順序的動作集(因此這個任務不能由預先定義好的狀態(tài)機編程來完成)。(2)增強的語音識別、處理能力(由于任務是不確定的，因此語音也是不確定的，任務不是單純的動作或單個物體，可能包含多個物體和動作，例如：“將杯子放在廚房的桌子上(putthecuponthekitchentable)”。所有機器人的能力形成通常來說，機器人開展業(yè)務工作需要從行動類、物體類和位置類的集合中生成具體能力：(1)行動集合A，例如尋找特定人、跟隨等，抓取和運送物體等。(2)物體集合B，該集合由放在場地中的10個物體構成。(3)位置集合L，該集合根據測試場地條件確定，如果涉及到抓取任務，那么抓取的位置需在機器人的可達高度范圍內。10.5.2原理分析自主進場機器人需自主進場，其任務可以由測試人員對機器人下達，可以考慮相應加分。任務是由一個標準的生成器生成的。下達任務要求完全按照給定的任務一字不漏地下達給機器人，向機器人提出的問題不能是類似“第一個任務是什么？”“第二個任務是什么？”，所有給定的任務都是一個基本任務的組合。開門為啟動信號。過程在下達任務過程中，如果沒有按照給定的句子一字不漏的下達，視為失敗。如果重復下達任務3次之后，機器人依然沒有正確理解，則視為本次測試失敗。機器人首先自主到達場內的指定位置。到達指定地點之后，機器人被給予包含中文或者英文的指令，該句子包含2-3個任務。獲取任務機器人識別整個句子之后，應該完整的將其復述（意思一樣就可以）出來才能被認為是理解了命令。機器人理解的任務則記錄到pdf文件中供實驗者查看。10.5.3硬件及軟件流程10.5.4GPSR項目初始化(1)程序啟動時開啟語音識別、地圖導航功能typedef

actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>

MoveBaseClient;

static

ros::Publisher

spk_pub;

static

ros::ServiceClient

clientIAT;

static

xfyun_waterplus::IATSwitch

srvIAT;

static

ros::ServiceClient

cliGetWPName;

static

waterplus_map_tools::GetWaypointByName

srvName;

static

int

nPersonCount

=

0;

(2)定義有限狀態(tài)機#define

STATE_WAIT_ENTR

0

//初始狀態(tài)

#define

STATE_WAIT_RECO

1

//從任務語句中判斷需要到達的位置

#define

STATE_GOTO_LOCATION

2

//到某一地點

#define

STATE_WAIT_ASK

3

//等待詢問

#define

STATE_CONFIRM

4

//記錄任務到txt文件中，并執(zhí)行小任務

#define

STATE_GOTO_EXIT

5

//到出口

#define

STATE_GOTO_LOCATION2

6

//到第二個地點

10.5.4GPSR項目初始化(3)定義全局變量int

taskflag

=

0;//任務標志

1-回答問題

2-回答小問題3-follow4-抓取物品

5-找東西char

strOrder[100];//記錄聽到的命令

char

obj[20];//記錄物品

char

pla[20];//記錄地點

char

pla2[20];//記錄拆分地點2

static

int

nState

=

STATE_WAIT_ENTR;

//程序啟動時初始狀態(tài)

(4)存儲指令關鍵詞的結構體static

vector<string>

arKWPerson;//人名關鍵詞

static

vector<string>

arKWConfirm;//回應關鍵詞

static

vector<string>

arKWPlacement;//大地點(房間)關鍵詞

static

vector<string>

arKWPlacement2;//大地點中包含的小地點關鍵詞

static

vector<string>

arKWObject;//物品關鍵詞

static

vector<string>

arKWAction1;//行為關鍵詞1，對應大地點

static

vector<string>

arKWAction2;//行為關鍵詞2，對應小地點

static

vector<string>

arKWQuestion;//大問題關鍵詞(需要單獨詢問的問題)

static

vector<string>

arKWSmallQuestion;//小問題關鍵詞(包含在指令中的問題)

10.5.5GPSR項目流程程序運行過程以機器人收到指令“grasptheHerbalteafromthediningtableanddeliverittotheleftcookingbench，將涼茶從餐桌帶到左邊灶臺”為例，進行接下來的程序執(zhí)行。10.5.5GPSR項目流程起始狀態(tài)：機器人自行移動到發(fā)令點me，等待發(fā)令人發(fā)出指令。bool

bArrived

=

Goto("me");

Speak("Tell

me

the

command.");

nState

=

STATE_WAIT_RECO;

10.5.5GPSR項目流程（2）機器人接到發(fā)令人發(fā)送的指令后，從聽到的句子里尋找指令關鍵詞，并確定機器人接下來的任務(到哪些地點，找什么人或物品，回答什么問題)，播報聽到的指令，等待語音確認。string

location

=

FindWord(strListen,

arKWPlacement);

string

location2

=

FindWord(strListen,

arKWPlacement2);

string

action1

=

FindWord(strListen,

arKWAction1);

string

action2

=

FindWord(strListen,

arKWAction2);

string

object

=

FindWord(strListen,

arKWObject);

Speak("Please

confirm!");

nState

=

STATE_CONFIRM;

10.5.5GPSR項目流程（3）發(fā)令人反饋語音確認指令后，機器人對收到的確認語句進行判斷。如果確認語句是yes、yeah等，則解析第一個任務關鍵詞，確定taskflag（此處taskflag為4-拿東西），并導航到第一個目標點（此處為diningtable）；如果語句是no，則要求重新下發(fā)指令。10.5.5GPSR項目流程if

(confirm

==

"yes"

||

confirm

==

"Yes"

||

confirm

==

"Yeah")

{

if

(strstr(strOrder,

"answer")

!=

NULL)

{

taskflag

=

1;

//回答大問題

}

else

if

(strstr(strOrder,

"say")

!=

NULL

||

strstr(strOrder,

"tell")

!=

NULL

||

strstr(strOrder,

"speak")

!=

NULL)

{

taskflag

=

2;

//回答小問題

}

else

if

(action2

==

"follow"

||

action2

==

"followed")

{

taskflag

=

3;

//follow

}

else

i