ros2动作编程-server_ros2 action 取消任务

我们这一节开始学习ros2的动作编程，（预留动作编程原理）将创建运行一个动作服务器节点和动作客户端节点实现斐波那契数列的运算。
首先我们进入我们的消息功能包添加一个action文件夹幷在里面新建一个Fibonacci.action文件，内容如下：

#goal
int32 order
---
#result
int32[] sequence
---
#feedback
int32[] partial_sequence
1
2
3
4
5
6
7
8

除了上述动作文件中找到的目标(goal)、结果(result)和反馈(feedback)之外,动作基本上还使用两个额外的消息:取消(cancel)和状态(status)。取消(cancel)消息使用actionlib_msgs/GoalID,它在动作运行时可以取消动作客户端和单独节点上的动作的执行。状态(status)消息可以根据状态转换 (如PENDING、ACTIVE、PREEMPTED和SUCCEEDED )检查当前动作的状态。
并且需要在该功能包CMakeLists.txt添加编译规则

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Cardata.msg"
  "msg/Carcmd.msg"
  "srv/SrvTutorial.srv"

  "action/Fibonacci.action"#这次添加这一句
  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
)
1
2
3
4
5
6
7
8

在package.xml文件添加

  <depend>action_msgs</depend>
1

编译生成动作消息文件。

然后创建tutorials_action功能包来编写节点：

ln@ln-pctogo:~/ros2_ws$ ros2 pkg create tutorials_action --build-type ament_cmake --dependencies rclcpp rclcpp_action rclcpp_components oryxbot2_msgs
1

然后在src文件夹创建fibonacci_action_server.cpp，代码如下：

#include <memory>

#include "oryxbot2_msgs/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"


class FibonacciAction : public rclcpp::Node
{

public:
  //类型重命名
  using Fibonacci = oryxbot2_msgs::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  //构造函数
  FibonacciAction(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("fibonacci_action_server", options)
  {
    using namespace std::placeholders;
    //初始化动作服务器
    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
      this->get_node_base_interface(),
      this->get_node_clock_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      "fibonacci",
      std::bind(&FibonacciAction::handle_goal, this, _1, _2),
      std::bind(&FibonacciAction::handle_cancel, this, _1),
      std::bind(&FibonacciAction::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;
    // Let's reject sequences that are over 9000
    if (goal->order > 9000) {
      return rclcpp_action::GoalResponse::REJECT;
    }
    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(&FibonacciAction::execute, this, _1), goal_handle}.detach();
  }

  //接收动作目标消息幷执行动作（此处为斐波那契数列）的函数
  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    //循环周期 ： 1HZ
    rclcpp::Rate loop_rate(1);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    //斐波那契数列初始化设置，也添加了反馈第一个第二个消息-01
    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) {
      // 查询客户端是否取消动作
      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");
    }
  }

};

//主函数
int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<FibonacciAction>());
    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
111
112
113
114
115

然后在CMakeLists.txt添加编译规则：

add_executable(fibonacci_action_server src/fibonacci_action_server.cpp)
ament_target_dependencies(fibonacci_action_server rclcpp rclcpp_action oryxbot2_msgs)

install(TARGETS
  fibonacci_action_server
  EXPORT export_${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME})
1
2
3
4
5
6
7

编译幷source。
运行节点

ln@ln-pctogo:~/ros2_ws$ ros2 run tutorials_action fibonacci_action_server 
1

什么也没有发生，是因为没有人调用动作服务，下节我们将编写客户端节点来调用服务。

（功能包在github上，使用需要同时下载编译oryxbot2_msgs功能包。git地址： https://github.com/DylanLN/oryxbot_ws-ros2）