ROS2高效学习第八章 -- ros2 action编程之自定义 action

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
本系列博客汇总：ROS2 高效学习系列。

2 正文

2.1 diy_interface 添加 Fibonacci.action

（1）斐波那契数列定义：

F(0) = 0,
F(1) = 1,
对于 n > 1, F(n) = F(n-1) + F(n-2)
1
2
3

（2）在 diy_interface 中，创建 Fibonacci.action

cd ~/colcon_ws/src/diy_interface
mkdir action
touch action/Fibonacci.action
1
2
3

（3）编写 Fibonacci.action

# Request
# order 表示要计算到的数列长度，由 client 发给 server
int32 order
---
# Result
# server 端最终计算出的数列，使用数组表示
int32[] sequence
---
# Feedback
# server 端运行过程中返回的数列
int32[] partial_sequence
1
2
3
4
5
6
7
8
9
10
11

（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
)
1
2
3
4
5
6
7

（5）编译并测试

~/colcon_ws
colcon build --packages-select diy_interface
source install/local_setup.bash
ros2 interface show diy_interface/action/Fibonacci
1
2
3
4

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
1
2
3
4
5
6

（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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110

（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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

（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()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

（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
        )
    ])
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

（6）编译并运行

~/colcon_ws
colcon build --packages-select diy_interface action_cpp
source install/local_setup.bash
ros2 launch action_cpp fibonacci_launch.py
1
2
3
4

2.3 action_py

action_py 是用 python 实现的同等功能的软件包，这里不再赘述创建过程，具体实现请看源码：action_py
（1）编译并运行

3 总结

本文代码托管在本人的github上：diy_interface，action_cpp，action_py