forked from ros2/examples
-
Notifications
You must be signed in to change notification settings - Fork 0
/
member_functions.cpp
165 lines (143 loc) · 5.59 KB
/
member_functions.cpp
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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <inttypes.h>
#include <memory>
#include "example_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_msgs/msg/empty.hpp"
class MinimalActionServer : public rclcpp::Node
{
public:
using Fibonacci = example_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("minimal_action_server", options)
{
auto internal_options = rclcpp::NodeOptions().arguments(
{"--ros-args",
"-r", std::string("__node:=") + get_name() + "_internal_node",
"--"});
internal_node_ = std::make_shared<rclcpp::Node>("_", internal_options);
action_client_to_self_ = rclcpp_action::create_client<Fibonacci>(internal_node_, "fibonacci");
using namespace std::placeholders;
trigger_action_sub_ = create_subscription<std_msgs::msg::Empty>(
"/trigger",
rclcpp::SystemDefaultsQoS(),
std::bind(&MinimalActionServer::on_trigger_received, this, _1));
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(&MinimalActionServer::handle_goal, this, _1, _2),
std::bind(&MinimalActionServer::handle_cancel, this, _1),
std::bind(&MinimalActionServer::handle_accepted, this, _1));
// Launch a thread to spin the internal node
thread_ = std::make_unique<std::thread>(
[&](rclcpp::Node::SharedPtr node)
{
executor_.add_node(node->get_node_base_interface());
executor_.spin();
executor_.remove_node(node->get_node_base_interface());
}, internal_node_);
}
~MinimalActionServer()
{
RCLCPP_INFO(get_logger(), "Destroying");
executor_.cancel();
thread_->join();
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
rclcpp::Node::SharedPtr internal_node_;
rclcpp::executors::SingleThreadedExecutor executor_;
std::unique_ptr<std::thread> thread_;
rclcpp_action::Client<Fibonacci>::SharedPtr action_client_to_self_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr trigger_action_sub_;
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 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->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");
}
}
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(&MinimalActionServer::execute, this, _1), goal_handle}.detach();
}
void on_trigger_received(const std_msgs::msg::Empty::SharedPtr /*msg*/)
{
RCLCPP_INFO(this->get_logger(), "Sending goal request to self");
auto goal = Fibonacci::Goal();
goal.order = 10;
action_client_to_self_->async_send_goal(goal);
}
}; // class MinimalActionServer
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto action_server = std::make_shared<MinimalActionServer>();
rclcpp::spin(action_server);
rclcpp::shutdown();
return 0;
}