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

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程序。