動作是ROS中的一種非同步通訊形式,動作客戶端向動作伺服器傳送目標請求,目標伺服器向操作客戶端傳送目標反饋和結果。本文基於前一篇自定義動作博文。
1.建立一個action_turtorials_cpp
包
1.1 建立一個action_turtorials_cpp
包
在終端執行:
cd ~/action_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
1.2新增可見性控制元件
為了使包可以在Windows上編譯和工作,我們需要新增一些“可見性控制”。有關為什麼需要這樣做的詳細資訊,請參見這裡。
開啟action_tutorials_cpp/include/action_tutorials_cpp/ visbility_control .h
,並放入以下程式碼:
#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#ifdef __cplusplus
extern "C"
{
#endif
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport))
#define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport))
#else
#define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport)
#define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport)
#endif
#ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT
#else
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#else
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_IMPORT
#if __GNUC__ >= 4
#define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_LOCAL __attribute__ ((visibility("hidden")))
#else
#define ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE
#endif
#ifdef __cplusplus
}
#endif
#endif // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
2.編寫一個動作伺服器
接下來編寫一個動作伺服器,使用在前文建立的動作介面來計算斐波那契數列。
2.1編寫動作伺服器
開啟action_tutorials_cpp/src/fibonacci_action_server.cpp
(需要自己建立),輸入以下程式碼:
#include <functional>
#include <memory>
#include <thread>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "action_tutorials_cpp/visibility_control.h"
namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
ACTION_TUTORIALS_CPP_PUBLIC
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this,
"fibonacci",
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1));
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(1);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto & sequence = feedback->partial_sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// Update sequence
sequence.push_back(sequence[i] + sequence[i - 1]);
// Publish feedback
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
}; // class FibonacciActionServer
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)
前幾行包含需要編譯的所有標頭檔案。
接下來,建立一個rclcpp::Node
的派生類:
class FibonacciActionServer : public rclcpp::Node
FibonacciActionServer
類的建構函式初始化fibonacci_action_server
節點:
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
建構函式還例項化了一個新的動作伺服器:
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this,
"fibonacci",
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1));
這個動作服務有6樣東西:
- 模板化的動作型別名稱:
Fibonacci
。 - 將一個ROS2節點的動作新增到:this。
- 動作名稱:
fibonacci
。 - 處理目標的回撥函式:
handle_goal
。 - 處理取消的回撥函式:
handle_cancel
。 - 處理目標接收的函式:
handle_accept
。
該檔案的下一個內容是各種回撥的實現。請注意,所有的回撥都需要快速返回,否則就會有耗盡執行程式的風險。
處理新的目標的回撥函式:
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
這個實現僅僅接收目標。
處理取消的回撥函式:
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
這個實現只是告訴客戶機它接受了取消。
最後一個回撥函式接受一個新目標並開始處理它:
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
由於執行是一個長期執行的操作,所以派生出一個執行緒來執行實際工作,並從handle_accepted
快速返回。
所有進一步的處理和更新都在新執行緒的execute
方法中完成:
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(1);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto & sequence = feedback->partial_sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// Update sequence
sequence.push_back(sequence[i] + sequence[i - 1]);
// Publish feedback
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
這個工作執行緒每秒處理一個斐波那契數列序號,為每個步驟釋出一個反饋更新。當它完成處理時,它將goal_handle
標記為成功,然後退出。
2.2編譯動作伺服器
設定CMakeLists.txt
,以便編譯動作伺服器。開啟action_tutorials_cpp/CMakeLists.txt
,並在find_package
呼叫之後新增以下內容:
add_library(action_server SHARED
src/fibonacci_action_server.cpp)
target_include_directories(action_server PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(action_server
PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_server
"action_tutorials_interfaces"
"rclcpp"
"rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)
install(TARGETS
action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
現在可以編譯包了,進入action_ws
的根目錄,並執行:
colcon build
2.3執行動作伺服器
現在已經構建了動作伺服器,可以執行它:
ros2 run action_tutorials_cpp fibonacci_action_server
3.編寫動作客服端
3.1編寫動作客戶節點程式碼
開啟action_tutorials_cpp/src/fibonacci_action_client.cpp
(需要建立),加入以下程式碼:
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
{
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
}
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10;
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
auto goal_handle = future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void feedback_callback(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->partial_sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number : result.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::shutdown();
}
}; // class FibonacciActionClient
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)
前幾行包含需要編譯的所有標頭檔案。
接下來,建立一個rclcpp::Node
的派生類:
class FibonacciActionClient : public rclcpp::Node
FibonacciActionClient
類的建構函式初始化節點fibonacci_action_client
:
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
建構函式還例項化了一個新的動作客戶端:
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
一個動作客戶端需要3件東西:
-
動作型別名稱:
Fibonacci
。 -
將動作客戶端新增到的ROS2節點:
this
。 -
動作名:
fibonacci
。
例項化一個ROS定時器,它將啟動對send_goal
的唯一呼叫:
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
當計時器到期時,它將呼叫send_goal:
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10;
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
這個函式的功能如下:
-
取消計時器(因此只呼叫一次)。
-
等待動作伺服器啟動。
-
例項化一個新的
Fibonacci::Goal
。 -
設定響應、反饋和結果回撥。
-
將目標傳送到伺服器。
當伺服器接收並接受目標時,它將向客戶機傳送一個響應。該響應由goal_response_callback
處理:
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
auto goal_handle = future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
假設目標被伺服器接受,它將開始處理。任何給客戶端的反饋都將被feedback_callback
處理:
void feedback_callback(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->partial_sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
當伺服器完成處理後,它將向客戶機返回一個結果。結果由result_callback
處理:
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number : result.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::shutdown();
}
3.2編譯動作客戶端
開啟action_tutorials_cpp/CMakeLists.txt
,在find_package
下新增:
add_library(action_client SHARED
src/fibonacci_action_client.cpp)
target_include_directories(action_client PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(action_client
PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_client
"action_tutorials_interfaces"
"rclcpp"
"rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client)
install(TARGETS
action_client
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
編譯:
colcon build
3.3執行動作客戶端
現在已經構建了動作客戶端,可以執行它。首先,確保動作伺服器在單獨的終端中執行:
ros2 run action_tutorials_cpp fibonacci_action_server
執行動作客戶端:
ros2 run action_tutorials_cpp fibonacci_action_client
現在可以看到被接受的目標日誌訊息、列印的反饋和最終的結果。
4.總結
在本文中,編寫了一個C++動作伺服器和客戶端,併為它們配置目標、反饋和結果。
如果給您帶來幫助,希望能給點個關注,以後還會陸續更新有關機器人的內容,點個關注不迷路~歡迎大家一起交流學習。
都看到這了,點個推薦再走吧~
未經允許,禁止轉載。