ROS2 通信三大件之动作 -- Action
通信最后一个,也是不太容易理解的方式action,复杂且重要
1、创建action数据结构
创建工作空间和模块就不多说了
在模块 src/action_moudle/action/Counter.action 下创建文件 Counter.action
int32 target # Goal: 目标
---
int32 current_value # Result: 结果
---
int32[] sequence # Feedback: 中间状态反馈
需要注意的是 这几个值不要搞混,结果和中间状态容易搞错
添加依赖:src/action_moudle/package.xml
<depend>rclcpp_action</depend>
<depend>rosidl_default_generators</depend>
<depend>rosidl_default_runtime</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
src/action_moudle/CMakeLists.txt
find_package(rosidl_default_generators REQUIRED)
find_package(rclcpp_action REQUIRED)
# 设置消息和服务文件路径
set(action_FILES
"action/Counter.action"
)
# 添加消息和服务生成目标
rosidl_generate_interfaces(${PROJECT_NAME}
${action_FILES}
DEPENDENCIES std_msgs
)
# 安装 Action 文件
install(DIRECTORY action/
DESTINATION share/${PROJECT_NAME}/action
)
编译后生成install/action_moudle/include/action_moudle/action_moudle/action/counter.hpp
至此action文件生成
2、编写Count计时的Action服务端和客户端代码
AI生成的代码靠不住,改了很多才编译过
src/action_moudle/src/action_server.cpp
#include "rclcpp/rclcpp.hpp"
#include "action_moudle/action/counter.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include <memory>
#include <vector>
using Counter = action_moudle::action::Counter;
using namespace std::placeholders;
class CounterActionServer : public rclcpp::Node
{
public:
CounterActionServer() : Node("counter_action_server")
{
action_server_ =
rclcpp_action::create_server<Counter>(this, "counter", std::bind(&CounterActionServer::handle_goal, this, _1, _2),
std::bind(&CounterActionServer::handle_cancel, this, _1), std::bind(&CounterActionServer::handle_accepted, this, _1));
}
private:
rclcpp_action::Server<Counter>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const Counter::Goal> goal)
{
(void)uuid;
RCLCPP_INFO(this->get_logger(), "Received goal request with target: %ld", goal->target);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Counter>> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Counter>> goal_handle)
{
using namespace std::placeholders;
std::thread{std::bind(&CounterActionServer::execute, this, _1), goal_handle}.detach();
}
void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Counter>> 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<Counter::Feedback>();
auto result = std::make_shared<Counter::Result>();
result->current_value = 0;
feedback->sequence.push_back(0);
if (goal_handle->is_canceling())
{
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
goal_handle->publish_feedback(feedback);
for (int i = 1; i <= goal->target; ++i)
{
result->current_value = i;
feedback->sequence.push_back(i);
if (goal_handle->is_canceling())
{
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
goal_handle->publish_feedback(feedback);
loop_rate.sleep();
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Returning result");
}
};
int main(int argc, char* argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<CounterActionServer>());
rclcpp::shutdown();
return 0;
}
src/action_moudle/src/action_client.cpp
#include "rclcpp/rclcpp.hpp"
#include "action_moudle/action/counter.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include <memory>
using Counter = action_moudle::action::Counter;
using namespace std::placeholders;
class CounterActionClient : public rclcpp::Node
{
public:
CounterActionClient() : Node("counter_action_client")
{
action_client_ = rclcpp_action::create_client<Counter>(this, "counter");
while (!action_client_->wait_for_action_server())
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the action server. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "Action server not available, waiting again...");
}
auto goal = Counter::Goal();
goal.target = 10;
auto send_goal_options = rclcpp_action::Client<Counter>::SendGoalOptions();
send_goal_options.goal_response_callback = std::bind(&CounterActionClient::goal_response_callback, this, std::placeholders::_1);
send_goal_options.feedback_callback = std::bind(&CounterActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
send_goal_options.result_callback = std::bind(&CounterActionClient::result_callback, this, std::placeholders::_1);
action_client_->async_send_goal(goal, send_goal_options);
}
private:
rclcpp_action::Client<Counter>::SharedPtr action_client_;
void goal_response_callback(rclcpp_action::ClientGoalHandle<Counter>::SharedPtr goal_handle)
{
if (!goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Goal rejected");
}
else
{
RCLCPP_INFO(this->get_logger(), "Goal accepted");
}
}
// void goal_response_callback(std::shared_future<std::shared_ptr<rclcpp_action::ClientGoalHandle<Counter>>> future)
// {
// auto goal_handle = future.get();
// if (!goal_handle)
// {
// RCLCPP_INFO(this->get_logger(), "Goal rejected");
// }
// else
// {
// RCLCPP_INFO(this->get_logger(), "Goal accepted");
// }
// }
void feedback_callback(std::shared_ptr<rclcpp_action::ClientGoalHandle<Counter>>, const std::shared_ptr<const Counter::Feedback> feedback)
{
// RCLCPP_INFO(this->get_logger(), "Current value: %ld", feedback->current_value);
RCLCPP_INFO(this->get_logger(), "Sequence value: %s", print_sequence(feedback->sequence).c_str());
}
void result_callback(const rclcpp_action::ClientGoalHandle<Counter>::WrappedResult& result)
{
switch (result.code)
{
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "Result: %d", result.result->current_value);
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
break;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
break;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
break;
}
rclcpp::shutdown();
}
std::string print_sequence(const std::vector<int32_t>& sequence)
{
std::stringstream ss;
ss << "[";
for (size_t i = 0; i < sequence.size(); ++i)
{
if (i > 0)
{
ss << ", ";
}
ss << sequence[i];
}
ss << "]";
return ss.str();
}
};
int main(int argc, char* argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<CounterActionClient>());
rclcpp::shutdown();
return 0;
}
src/action_moudle/CMakeLists.txt 添加
find_package(action_moudle REQUIRED)
find_package(rclcpp_action REQUIRED)
# 创建可执行文件
add_executable(action_server src/action_server.cpp)
ament_target_dependencies(action_server rclcpp std_msgs action_moudle rclcpp_action)
add_executable(action_client src/action_client.cpp)
ament_target_dependencies(action_client rclcpp std_msgs action_moudle rclcpp_action)
# 安装目标
install(TARGETS
action_server action_client
DESTINATION lib/${PROJECT_NAME}
)
3、编译后测试