action消息的构建
首先进行功能包的创建
mkdir -p ros2_ws/src
cd ros2_ws/src
ros2 pkg create action_tutorials_interfaces
action消息的类型
# Request
---
# Result
---
# Feedback
动作定义由三个消息定义组成,以---分隔。
- 从动作客户机向动作服务器发送请求消息以启动新目标。
- 当目标完成时,从动作服务器向动作客户端发送结果消息。
- 反馈消息周期性地从动作服务器发送到动作客户端,其中包含关于目标的更新。
创建消息
cd action_tutorials_interfaces
mkdir action
创建一个Fibonacci.action文件指定action的消息
int32 order
---
int32[] sequence
---
int32[] partial_sequence
编译消息
在我们可以在代码中使用新的Fibonacci动作类型之前,我们必须将定义传递给Rosidl代码生成管道。这可以通过在CMakeLists.txt中的ament_package()行之前添加以下行来实现,即action_tutorials_interfaces中的行:
CMAkeLists.txt文件进行修改
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/Fibonacci.action"
)
xml文件进行修改
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
在这里action文件就算是生成了
action_server进行使用
进行action使用的功能包的创建
os2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components为该功能包的相关依赖
windows编译
为了使软件包能够在Windows上编译和工作,我们需要添加一些“可见性控制”。有关详细信息,请参见Windows提示和技巧文档中的Windows符号可见性。
打开action_tutorials_cpp/include/action_tutorials_cpp/visibility_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_
创建server文件
#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)
几个关键的数据类型
ClientGoalHandle:
用于与客户端发送过来的目标进行交互。可以使用此类检查目标的状态和结果,这个类不是通过用户创建的,而是在目标进行接受。
// 使用这个类去检查goal 的状态和这个结果,并不意味着被用户创建,当一个用户的goal被接受的时候才能被创建
template<typename ActionT>
class ServerGoalHandle : public ServerGoalHandleBase
{
public:
// 发送一个更新数据,处理真个进行关于goal目标的
// 这个函数只能当goal被调用的时候才能被调用
//
void
publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
{
auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
feedback_message->goal_id.uuid = uuid_;
feedback_message->feedback = *feedback_msg;
publish_feedback_(feedback_message);
}
// 当goal无法达到,并且已中止。
// 只有当goal正在执行但无法完成时才调用此函数。
// 这是一个终端状态,在这之后不应该再对目标句柄调用任何方法
// param[in]result_msg要发送给客户端的最终结果。
void
abort(typename ActionT::Result::SharedPtr result_msg)
{
_abort();
auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
response->result = *result_msg;
on_terminal_state_(uuid_, response);
}
/// 当这个goal执行成功的时候执行
// param[in] result_msg the final result to send to clients.
void
succeed(typename ActionT::Result::SharedPtr result_msg)
{
_succeed();
auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
response->result = *result_msg;
on_terminal_state_(uuid_, response);
}
/// Indicate that a goal has been canceled.
/**
* Only call this if the goal is executing or pending, but has been canceled.
* This is a terminal state, no more methods should be called on a goal handle after this is
* called.
* \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
*
* \param[in] result_msg the final result to send to clients.
*/
void
canceled(typename ActionT::Result::SharedPtr result_msg)
{
_canceled();
auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
response->result = *result_msg;
on_terminal_state_(uuid_, response);
}
/// Indicate that the server is starting to execute a goal.
// throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
void
execute()
{
_execute();
on_executing_(uuid_);
}
/// Get the user provided message describing the goal.
const std::shared_ptr<const typename ActionT::Goal>
get_goal() const
{
return goal_;
}
/// Get the unique identifier of the goal
const GoalUUID &
get_goal_id() const
{
return uuid_;
}
virtual ~ServerGoalHandle()
{
// Cancel goal if handle was allowed to destruct without reaching a terminal state
if (try_canceling()) {
auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
on_terminal_state_(uuid_, null_result);
}
}
protected:
/// \internal
ServerGoalHandle(
std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
GoalUUID uuid,
std::shared_ptr<const typename ActionT::Goal> goal,
std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
std::function<void(const GoalUUID &)> on_executing,
std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback
)
: ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
on_terminal_state_(on_terminal_state), on_executing_(on_executing),
publish_feedback_(publish_feedback)
{
}
/// The user provided message describing the goal.
const std::shared_ptr<const typename ActionT::Goal> goal_;
/// A unique id for the goal request.
const GoalUUID uuid_;
friend Server<ActionT>;
std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;
std::function<void(const GoalUUID &)> on_executing_;
std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;
};
/// A response returned by an action server callback when a goal is requested.
enum class GoalResponse : int8_t
{
/// The goal is rejected and will not be executed.
REJECT = 1,
/// The server accepts the goal, and is going to begin execution immediately.
ACCEPT_AND_EXECUTE = 2,
/// The server accepts the goal, and is going to execute it later.
ACCEPT_AND_DEFER = 3,
};
/// A response returned by an action server callback when a goal has been asked to be canceled.
enum class CancelResponse : int8_t
{
/// The server will not try to cancel the goal.
REJECT = 1,
/// The server has agreed to try to cancel the goal.
ACCEPT = 2,
};
action client定义
#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)
配置对应的CMakeLists.txt文件
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)
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)
通过编译之后就可以得到对应的模型