在ROS中除了常見的話題(topic)通信、服務(server)通信等方式,還有action通信這一方式,由于可以實時反饋任務完成情況,該通信方式被廣泛運用于機器人導航等任務中。本文將通過三個小節的分享,實現基于action通信的階乘運算。
第一節:自定義action文件
第二節:基于C++實現action通信的服務端
第三節:基于C++實現action通信的客戶端
本節為第二節:基于C++實現action通信的服務端
詳細步驟如下:
步驟1:在action_ws/src/factorial_pkg/src下創建factorial_server.cpp文件
cd action_ws/src/factorial_pkg/src
touch factorial_server.cpp
步驟2: 編寫factorial_server.cpp文件
作者已在代碼中進行了詳細注釋
#include "ros/ros.h"
// 創建一個簡單行為服務器(simple_action_server)
#include "actionlib/server/simple_action_server.h"
// 導入自定義action文件
#include "factorial_pkg/FactorialAction.h"/* 由于 actionlib::SimpleActionServer<factorial_pkg::FactorialAction>
名稱太長,使用typedef給它一個簡短的命名 */
typedef actionlib::SimpleActionServer<factorial_pkg::FactorialAction> as;// 定義FactorialAction類,階乘運算將在這個類內實現
class FactorialAction
{
protected:ros::NodeHandle nh_; // ros句柄as as_; // 定義行為服務器 as_// action通信的話題名稱(類似于話題通信中的topic name)std::string action_name_; // 定義action中的結果與反饋變量factorial_pkg::FactorialResult result_;factorial_pkg::FactorialFeedback feedback_;public:// 構造函數(使用參數列表進行參數初始化)FactorialAction(std::string name):/* as_的第一個參數是NodeHandle的名稱第二個參數是action通信的話題名稱第三個參數用于將回調函數與當前的對象進行綁定<個人想法:理解不了就背一下(反正作者是背的Hhh...)>第四個參數用于選擇是否自動啟動如果是false,需要as_.start()啟動如果true,則不需要改行代碼注意, as_()內的參數位置不能改變 */as_(nh_, name, boost::bind(&FactorialAction::executeCB, this, _1), false),action_name_(name){as_.start();}~FactorialAction(){}/*回調函數的實現1 需要定義頻率 ros::Rate r(10),主要是feedback需要使用2 具體實現階乘*/void executeCB(const factorial_pkg::FactorialGoalConstPtr &goal){ros::Rate r(10);bool success = true;// 獲取goal -> goalint64_t g = goal->goal;// 實現階乘int64_t result = 1;for(int i=1; i<=g; i++){// 檢查進程是否需要關閉if(as_.isPreemptRequested() || !ros::ok()){as_.setPreempted();success = false;break;}result = result * i;feedback_.feedback_num = i / (double)g;// 發送feedbackas_.publishFeedback(feedback_);r.sleep();}if(success){result_.result = result;ROS_INFO("%s: Succeeded", action_name_.c_str());// 發送feedbackas_.setSucceeded(result_);}}};int main(int argc, char *argv[])
{ros::init(argc, argv, "factorial_server");// 實例化一個對象,傳入行為服務器話題名稱FactorialAction factorial_action("factorial_action"); ros::spin();return 0;
}
步驟3:編寫factorial_pkg下的CMakeLists.txt文件
add_executable(factorial_server src/factorial_server.cpp)
target_link_libraries(factorial_server ${catkin_LIBRARIES})
步驟4:檢查是否成功創建simple_action_server
啟動roscore,
新打開一個終端運行factorial_server節點,
與此同時,再新打開一個終端執行rostopic list -v
如果沒有問題會有如下效果
a@melodic:~/desk/action_ws$ rostopic list -vPublished topics:* /factorial_action/result [factorial_pkg/FactorialActionResult] 1 publisher* /rosout [rosgraph_msgs/Log] 1 publisher* /factorial_action/status [actionlib_msgs/GoalStatusArray] 1 publisher* /rosout_agg [rosgraph_msgs/Log] 1 publisher* /factorial_action/feedback [factorial_pkg/FactorialActionFeedback] 1 publisherSubscribed topics:* /rosout [rosgraph_msgs/Log] 1 subscriber* /factorial_action/cancel [actionlib_msgs/GoalID] 1 subscriber* /factorial_action/goal [factorial_pkg/FactorialActionGoal] 1 subscribera@melodic:~/desk/action_ws$ catkin_create_pkg