1 前言和资料
在ROS2高效学习第二章 – ros2常用命令和相关概念学习，熟练玩起来小乌龟样例这篇博客里，我们讲解了 ros2 action 的基本概念，介绍了命令行调试 action 的方法。本文我们讲解 ros2 action 编程，并复用ROS2高效学习第四章 – ros2 topic 编程之自定义 msg中的 diy_interface 包，在里面添加自定义 action，并实现 action server 和 client 通信测试。
本文参考资料如下：
（1）ROS2高效学习第二章 – ros2常用命令和相关概念学习，熟练玩起来小乌龟样例 第 2.7 节
（2）Creating-an-Action
（3）Writing-an-Action-Server-Client-Cpp
（4）Writing-an-Action-Server-Client-Python
（5）ros2 action design
2 正文
2.1 diy_interface 添加 Fibonacci.action
（1）斐波那契数列定义：
F(0) = 0,
F(1) = 1,
对于 n > 1, F(n) = F(n-1) + F(n-2)
（2）在 diy_interface 中，创建 Fibonacci.action
cd ~/colcon_ws/src/diy_interface
mkdir action
touch action/Fibonacci.action
（3）编写 Fibonacci.action
# Request
# order 表示要计算到的数列长度，由 client 发给 server
int32 order
---
# Result
# server 端最终计算出的数列，使用数组表示
int32[] sequence
---
# Feedback
# server 端运行过程中返回的数列
int32[] partial_sequence
（4）添加 CMakeLists.txt
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Student.msg"
"msg/Sphere.msg"
"srv/QuestionAndAnswer.srv"
"action/Fibonacci.action"
DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
)
（5）编译并测试
~/colcon_ws
colcon build --packages-select diy_interface
source install/local_setup.bash
ros2 interface show diy_interface/action/Fibonacci
2.2 action_cpp
（1）创建 service_cpp 包和相关文件
cd ~/colcon_ws/src
ros2 pkg create --build-type ament_cmake --license Apache-2.0 action_cpp --dependencies diy_interface rclcpp rclcpp_action
cd action_cpp
mkdir launch
touch launch/fibonacci_launch.py
touch src/fibonacci_server.cpp src/fibonacci_client.cpp
（2）编写 fibonacci_client.cpp
#include <functional>
#include <future>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "diy_interface/action/fibonacci.hpp"
namespace action_cpp {
class FibonacciActionClient : public rclcpp::Node {
public:
using Fibonacci = diy_interface::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
FibonacciActionClient() : Node("fibonacci_action_client") {
// 创建 client 句柄
client_ = rclcpp_action::create_client<Fibonacci>(this, "fibonacci");
// 使用定时器发送命令，但只发送一次
timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
RCLCPP_INFO(this->get_logger(), "Fibonacci action client has been started");
}
void send_goal() {
// 取消定时器，因此只调用一次，发送一次action goal 给 server
this->timer_->cancel();
RCLCPP_INFO(this->get_logger(), "waiting for action server");
// 发送器确认 server 已经活跃
if (!client_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "action server not available after waiting");
rclcpp::shutdown();
return;
}
RCLCPP_INFO(this->get_logger(), "action server is available");
auto goal_msg = Fibonacci::Goal();
// 任务目标：数列长度算到10
goal_msg.order = 10;
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
// 注册任务确定回调函数
send_goal_options.goal_response_callback = std::bind(&FibonacciActionClient::goal_response_callback, this, std::placeholders::_1);
// 注册定期反馈回调函数
send_goal_options.feedback_callback = std::bind(&FibonacciActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
// 注册任务结果回调函数
send_goal_options.result_callback = std::bind(&FibonacciActionClient::result_callback, this, std::placeholders::_1);
// 发送任务
client_->async_send_goal(goal_msg, send_goal_options);
RCLCPP_INFO(this->get_logger(), "send goal to server, order is %d", goal_msg.order);
}
private:
void goal_response_callback(const GoalHandleFibonacci::SharedPtr & goal_handle) {
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();
return;
}
}
private:
rclcpp_action::Client<Fibonacci>::SharedPtr client_;
rclcpp::TimerBase::SharedPtr timer_;
}; // class FibonacciActionClient
} // namespace action_cpp
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<action_cpp::FibonacciActionClient>());
rclcpp::shutdown();
return 0;
}
（3）编写 fibonacci_server.cpp
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "diy_interface/action/fibonacci.hpp"
namespace action_cpp {
class FibonacciActionServer : public rclcpp::Node {
public:
using Fibonacci = diy_interface::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
FibonacciActionServer() : Node("fibonacci_action_server") {
// 创建server句柄
action_server_ = rclcpp_action::create_server<Fibonacci>(
this,
"fibonacci",
// 注册任务确认回调函数
std::bind(&FibonacciActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
// 注册任务取消回调函数
std::bind(&FibonacciActionServer::handle_cancel, this, std::placeholders::_1),
// 注册任务执行回调函数
std::bind(&FibonacciActionServer::handle_accepted, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "Fibonacci action server has been started");
}
private:
// uuid 参数虽然没有使用，但是必须写上，否则编译器会报错
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal) {
(void)uuid;
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
// goal_handle 参数虽然没有使用，但是必须写上，否则编译器会报错
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
(void)goal_handle;
RCLCPP_INFO(this->get_logger(), "Received cancel request");
return rclcpp_action::CancelResponse::ACCEPT;
}
// 执行任务单独起了一个线程
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
std::thread{std::bind(&FibonacciActionServer::execute, this, std::placeholders::_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 result = std::make_shared<Fibonacci::Result>();
auto feedback = std::make_shared<Fibonacci::Feedback>();
feedback->partial_sequence.push_back(0);
feedback->partial_sequence.push_back(1);
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
if (goal_handle->is_canceling()) {
// 取消任务时，把 feedback 的斐波那契数组保存到 result 的斐波那契数组中
result->sequence = feedback->partial_sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
feedback->partial_sequence.push_back(feedback->partial_sequence[i] + feedback->partial_sequence[i-1]);
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "publish feedback");
loop_rate.sleep();
}
if (rclcpp::ok()) {
// 任务结束时，把 feedback 的斐波那契数组保存到 result 的斐波那契数组中
result->sequence = feedback->partial_sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
}; // class FibonacciActionServer
} // namespace action_cpp
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<action_cpp::FibonacciActionServer>());
rclcpp::shutdown();
return 0;
}
（4）编写CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(action_cpp)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_ros REQUIRED)
find_package(diy_interface REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
add_executable(action_client_diy src/fibonacci_client.cpp)
ament_target_dependencies(action_client_diy rclcpp rclcpp_action diy_interface)
add_executable(action_server_diy src/fibonacci_server.cpp)
ament_target_dependencies(action_server_diy rclcpp rclcpp_action diy_interface)
install(TARGETS
action_client_diy
action_server_diy
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
ament_package()
（5）编写 fibonacci_launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='action_cpp',
namespace='cpp',
executable='action_server_diy',
name='action_server_diy',
output="screen",
emulate_tty=True
),
Node(
package='action_cpp',
namespace='cpp',
executable='action_client_diy',
name='action_client_diy',
output="screen",
emulate_tty=True
)
])
（6）编译并运行
~/colcon_ws
colcon build --packages-select diy_interface action_cpp
source install/local_setup.bash
ros2 launch action_cpp fibonacci_launch.py
2.3 action_py
action_py 是用 python 实现的同等功能的软件包，这里不再赘述创建过程，具体实现请看源码：action_py
（1）编译并运行
3 总结
本文代码托管在本人的github上：diy_interface，action_cpp，action_py
4070
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
