ROS入門筆記（十二）：動作程式設計 (C++)

目錄

• 01 導讀
• 02 功能包的建立
• 03 在功能包中建立action（動作）
  • 3.1 自定義action
  • 3.2 在package.xml中新增功能包依賴
  • 3.3 在CMakeLists.txt新增編譯選項
• 04 功能包的原始碼編寫
  • 4.1 編寫action_server.cpp
  • 4.2 編寫action_client.cpp
• 05 功能包的編譯配置（編譯節點）
• 06 功能包的編譯
• 07 測試action_server和action_client
  • 7.1 執行action_server
  • 7.2 執行action_client

01 導讀

C++程式碼必須通過編譯生成可執行檔案；

python程式碼是可執行檔案，不需要編譯；

• 開發的功能包都放在catkin_ws這樣一個工作空間裡；
• 新建的功能包取名為action_example；

工作模式的結構示意圖如下：

什麼是動作（action）

• 一種問答通訊機制；
• 帶有連續反饋；
• 可以在任務過程中止執行；
• 基於ROS的訊息機制實現。

通訊雙方在ROS Action Protocal下進行交流通訊是通過介面來實現,如下圖:

Action的介面

• goal：釋出任務目標；
• cancel：請求取消任務；
• status：通知客戶端當前的狀態；
• feedback：週期反饋任務執行的監控資料；
• result：向客戶端傳送任務的執行結果，只發布一次。

我們可以看到,客戶端會向伺服器傳送目標指令和取消動作指令,而伺服器則可以給客戶端傳送實時的狀態資訊,結果資訊,反饋資訊等等,從而完成了service沒法做到的部分.

02 功能包的建立

在catkin_ws/src/目錄下新建功能包action_example，並在建立時顯式的指明依賴roscpp和actionlib actionlib_msgs，依賴actionlib actionlib_msgs將作為基本資料型別用於定義我們的服務型別。開啟命令列終端，輸入命令：

$ cd ~/catkin_ws/src

#建立功能包topic_example時，顯式的指明依賴roscpp和std_msgs，
#依賴會被預設寫到功能包的CMakeLists.txt和package.xml中
$ catkin_create_pkg action_example roscpp actionlib actionlib_msgs
 

03 在功能包中建立action（動作）

Action的工作原理是client-server模式，也是一個雙向的通訊模式。通訊雙方在ROS Action Protocol下通過訊息進行資料的交流通訊。client和server為使用者提供一個簡單的API來請求目標（在客戶端）或通過函式呼叫和回撥來執行目標（在伺服器端）。

3.1 自定義action

利用動作庫進行請求響應，動作的內容格式應包含三個部分，目標、反饋、結果。

在功能包action_example目錄下新建action目錄，然後在action_example/action/目錄中建立DoDishes.action檔案

# Define the goal  定義目標資訊
uint32 dishwasher_id  # Specify which dishwasher we want to use
---
# Define the result  定義結果資訊
uint32 total_dishes_cleaned
---
# Define a feedback message 定義週期反饋的資訊
float32 percent_complete

3.2 在package.xml中新增功能包依賴

action檔案被轉換成為C++，Python和其他語言的原始碼：

檢視package.xml, 確保它包含以下語句:

*部分ROS版本中的exec_depend需要改成run_depend

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>

3.3 在CMakeLists.txt新增編譯選項

##1 Find catkin macros and libraries
##1 if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
##1 is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  actionlib_msgs 
  actionlib
)


##2 Generate actions in the 'action' folder
add_action_files(
  FILES
  DoDishes.action
)



##3 Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  actionlib_msgs   
)

04 功能包的原始碼編寫

在新建的功能包action_example/src目錄下新建兩個檔案action_server.cpp和action_client.cpp，並將下面的程式碼分別填入。

4.1 編寫action_server.cpp

如何實現一個動作伺服器：

• 初始化ROS節點；
• 建立動作伺服器例項；
• 啟動伺服器，等待動作請求；
• 在回撥函式中完成動作服務功能的處理，並反饋進度資訊；
• 動作完成，傳送結束資訊。

在action_example/src包中建立action_server.cpp檔案：

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "action_example/DoDishesAction.h"

typedef actionlib::SimpleActionServer<action_example::DoDishesAction> Server;

// 收到action的goal後呼叫該回調函式
void execute(const action_example::DoDishesGoalConstPtr& goal, Server* as)
{
    ros::Rate r(1);
    action_example::DoDishesFeedback feedback;

    ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);

    // 假設洗盤子的進度，並且按照1hz的頻率釋出進度feedback
    for(int i=1; i<=10; i++)
    {
        feedback.percent_complete = i * 10;
        as->publishFeedback(feedback);
        r.sleep();
    }

    // 當action完成後，向客戶端返回結果
    ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
    as->setSucceeded();
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "do_dishes_server");
    ros::NodeHandle n;

    // 定義一個伺服器
    Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
    
    // 伺服器開始執行
    server.start();

    ros::spin();

    return 0;
}

4.2 編寫action_client.cpp

如何實現一個動作客戶端：

• 初始化ROS節點；
• 建立動作客戶端例項；
• 連線動作服務端；
• 傳送動作目標；
• 根據不同型別的服務端反饋處理回撥函式。

在action_example/src包中建立action_client.cpp檔案，並在其中貼上以下內容：

#include <actionlib/client/simple_action_client.h>
#include "action_example/DoDishesAction.h"

typedef actionlib::SimpleActionClient<action_example::DoDishesAction> Client;

// 當action完成後會呼叫該回調函式一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const action_example::DoDishesResultConstPtr& result)
{
    ROS_INFO("Yay! The dishes are now clean");
    ros::shutdown();
}

// 當action啟用後會呼叫該回調函式一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}

// 收到feedback後呼叫該回調函式
void feedbackCb(const action_example::DoDishesFeedbackConstPtr& feedback)
{
    ROS_INFO(" percent_complete : %f ", feedback->percent_complete);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "do_dishes_client");

    // 定義一個客戶端
    Client client("do_dishes", true);

    // 等待伺服器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    // 建立一個action的goal
    action_example::DoDishesGoal goal;
    goal.dishwasher_id = 1;

    // 傳送action的goal給伺服器端，並且設定回撥函式
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

    ros::spin();

    return 0;
}

05 功能包的編譯配置（編譯節點）

說明：

• C++程式碼必須通過編譯生成可執行檔案；
• python程式碼是可執行檔案，不需要編譯；

如何編譯程式碼

• 設定需要編譯的程式碼和生成的可執行檔案；
• 設定連結庫；
• 設定依賴。

建立功能包action_example時，顯式的指明依賴roscpp和std_msgs，依賴會被預設寫到功能包的CMakeLists.txt和package.xml中。

在 CMakeLists.txt 檔案末尾加入幾條語句:

add_executable(action_client src/action_client.cpp)
target_link_libraries( action_client ${catkin_LIBRARIES})
add_dependencies(action_client ${${PROJECT_NAME}_EXPORTED_TARGETS})

add_executable(action_server src/action_server.cpp)
target_link_libraries( action_server ${catkin_LIBRARIES})
add_dependencies(action_server ${${PROJECT_NAME}_EXPORTED_TARGETS})

這會生成兩個可執行檔案, action_client 和 action_server, 預設儲存到 devel space 目錄下,具體是在~/catkin_ws/devel/lib/<package name> 中.

06 功能包的編譯

$ cd ~/catkin_ws
$ catkin_make -DCATKIN_WHITELIST_PACKAGES="action_example"    
$ source ~/catkin_ws/devel/setup.bash   # 重新整理環境

07 測試action_server和action_client

7.1 執行action_server

第一步，開啟一個命令列終端：

$ roscore

第二步，開啟第二個命令列終端：

# 用rosrun <package_name> <node_name>啟動功能包中的釋出節點。
$ source ~/catkin_ws/devel/setup.bash    # 啟用catkin_ws工作空間（必須有，必不可少） 
$ rosrun action_example action_server     

你將看到如下的輸出資訊:

[ INFO] [1588752334.514526874]: Dishwasher 1 is working.
[ INFO] [1588752344.515033939]: Dishwasher 1 finish working.  # Server節點啟動後的日誌資訊

7.2 執行action_client

開啟第三個命令列客戶端：

$ source ~/catkin_ws/devel/setup.bash     # 啟用catkin_ws工作空間（必須有，必不可少） 
$ rosrun action_example action_client        

你將會看到如下的輸出資訊:

[ INFO] [1588752334.233231877]: Waiting for action server to start.
[ INFO] [1588752334.513889608]: Action server started, sending goal.
[ INFO] [1588752334.514780017]: Goal just went active
[ INFO] [1588752334.515056866]:  percent_complete : 10.000000 
[ INFO] [1588752335.516336080]:  percent_complete : 20.000000 
[ INFO] [1588752336.516271562]:  percent_complete : 30.000000 
[ INFO] [1588752337.516315111]:  percent_complete : 40.000000 
[ INFO] [1588752338.515751638]:  percent_complete : 50.000000 
[ INFO] [1588752339.515473734]:  percent_complete : 60.000000 
[ INFO] [1588752340.516373053]:  percent_complete : 70.000000 
[ INFO] [1588752341.515448200]:  percent_complete : 80.000000 
[ INFO] [1588752342.515654876]:  percent_complete : 90.000000 
[ INFO] [1588752343.515571413]:  percent_complete : 100.000000 
[ INFO] [1588752344.516076162]: Yay! The dishes are now clean     

現在，你已經成功地運行了你的第一個action_server和action_client程式。

微信公眾號：喵哥解說
公眾號介紹：主要研究機器學習、計算機視覺、深度學習、ROS等相關內容，分享學習過程中的學習筆記和心得！期待您的關注，歡迎一起學習交流進步！同時還有1200G的Python視訊和書籍資料等你領取！！！