Skip to content

Commit fcd59fa

Browse files
mgonzs13mgonzs13
committed
queue server removed
1 parent 1da266a commit fcd59fa

File tree

14 files changed

+82
-392
lines changed

14 files changed

+82
-392
lines changed

‎simple_node/CMakeLists.txt‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,8 +29,7 @@ set(LIB ${CMAKE_PROJECT_NAME}_lib)
2929
set(SOURCES
3030
src/simple_node/node.cpp
3131
src/simple_node/actions/action_client.cpp
32-
src/simple_node/actions/action_queue_server.cpp
33-
src/simple_node/actions/action_single_server.cpp
32+
src/simple_node/actions/action_server.cpp
3433
)
3534

3635
set(DEPENDENCIES

‎simple_node/include/simple_node/actions/action_queue_server.hpp‎

Lines changed: 0 additions & 81 deletions
This file was deleted.

‎simple_node/include/simple_node/actions/action_server.hpp‎

Lines changed: 21 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,6 @@ class ActionServer : public rclcpp_action::Server<ActionT> {
3030
public:
3131
ActionServer(rclcpp::Node *node, std::string action_name,
3232
UserExecuteCallback execute_callback,
33-
UserAcceptedCallback accepted_callback,
3433
UserCancelCallback cancel_callback)
3534

3635
: rclcpp_action::Server<ActionT>(
@@ -39,66 +38,64 @@ class ActionServer : public rclcpp_action::Server<ActionT> {
3938
rcl_action_server_get_default_options(),
4039
std::bind(&ActionServer::handle_goal, this, _1, _2),
4140
std::bind(&ActionServer::handle_cancel, this, _1),
42-
accepted_callback) {
41+
std::bind(&ActionServer::handle_accepted, this, _1)) {
4342

4443
this->execute_callback = execute_callback;
4544
this->cancel_callback = cancel_callback;
46-
this->server_canceled = false;
4745
this->goal_handle = nullptr;
4846
}
4947

5048
ActionServer(rclcpp::Node *node, std::string action_name,
51-
UserExecuteCallback execute_callback,
52-
UserAcceptedCallback accepted_callback)
49+
UserExecuteCallback execute_callback)
5350

5451
: rclcpp_action::Server<ActionT>(
5552
node->get_node_base_interface(), node->get_node_clock_interface(),
5653
node->get_node_logging_interface(), action_name,
5754
rcl_action_server_get_default_options(),
5855
std::bind(&ActionServer::handle_goal, this, _1, _2),
5956
std::bind(&ActionServer::handle_cancel, this, _1),
60-
accepted_callback) {
57+
std::bind(&ActionServer::handle_accepted, this, _1)) {
6158

6259
this->execute_callback = execute_callback;
6360
this->cancel_callback = nullptr;
64-
this->server_canceled = false;
6561
this->goal_handle = nullptr;
6662
}
6763

6864
bool is_working() { return this->goal_handle != nullptr; }
6965

70-
bool is_canceled() { return this->server_canceled; }
71-
72-
void wait_for_canceling() {
73-
if (this->server_canceled && this->goal_handle != nullptr) {
74-
while (!this->goal_handle->is_canceling()) {
75-
std::this_thread::sleep_for(std::chrono::seconds(1));
76-
}
77-
}
78-
}
79-
8066
protected:
8167
std::shared_ptr<GoalHandle> goal_handle;
8268

8369
void execute_goal_handle(const std::shared_ptr<GoalHandle> goal_handle) {
84-
8570
std::thread{std::bind(&ActionServer::handle_execute, this, _1), goal_handle}
8671
.detach();
8772
}
8873

8974
private:
9075
UserExecuteCallback execute_callback;
9176
UserCancelCallback cancel_callback;
92-
bool server_canceled;
77+
std::mutex handle_accepted_mtx;
9378

9479
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid,
9580
std::shared_ptr<const Goal> goal) {
96-
9781
(void)uuid;
9882
(void)goal;
9983
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
10084
}
10185

86+
void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle) {
87+
88+
const std::lock_guard<std::mutex> lock(this->handle_accepted_mtx);
89+
90+
if (this->goal_handle != nullptr && this->goal_handle->is_active()) {
91+
this->goal_handle->abort(std::make_shared<typename ActionT::Result>());
92+
}
93+
94+
this->goal_handle = goal_handle;
95+
96+
this->execute_goal_handle(goal_handle);
97+
}
98+
10299
rclcpp_action::CancelResponse
103100
handle_cancel(const std::shared_ptr<GoalHandle> goal_handle) {
104101
(void)goal_handle;
@@ -108,26 +105,23 @@ class ActionServer : public rclcpp_action::Server<ActionT> {
108105
void handle_execute(const std::shared_ptr<GoalHandle> goal_handle) {
109106

110107
this->goal_handle = goal_handle;
111-
this->server_canceled = false;
112108

113-
using namespace std::placeholders;
114-
std::thread{std::bind(&ActionServer::execute, this, _1), goal_handle}
115-
.detach();
109+
std::thread(std::bind(&ActionServer::execute, this)).detach();
116110

117111
while (this->goal_handle != nullptr) {
118112
if (this->goal_handle->is_canceling()) {
119113

120114
if (this->cancel_callback != nullptr) {
121115
this->cancel_callback();
122116
}
123-
this->server_canceled = true;
124117
break;
125118
}
119+
std::this_thread::sleep_for(std::chrono::seconds(1));
126120
}
127121
}
128122

129-
void execute(const std::shared_ptr<GoalHandle> goal_handle) {
130-
this->execute_callback(goal_handle);
123+
void execute() {
124+
this->execute_callback(this->goal_handle);
131125
this->goal_handle = nullptr;
132126
}
133127
};

‎simple_node/include/simple_node/actions/action_single_server.hpp‎

Lines changed: 0 additions & 57 deletions
This file was deleted.

‎simple_node/include/simple_node/node.hpp‎

Lines changed: 8 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,7 @@
1111
#include "rclcpp/rclcpp.hpp"
1212

1313
#include "simple_node/actions/action_client.hpp"
14-
#include "simple_node/actions/action_queue_server.hpp"
15-
#include "simple_node/actions/action_single_server.hpp"
14+
#include "simple_node/actions/action_server.hpp"
1615

1716
namespace simple_node {
1817

@@ -45,16 +44,14 @@ class Node : public rclcpp::Node {
4544
}
4645

4746
template <typename ActionT>
48-
typename std::shared_ptr<actions::ActionSingleServer<ActionT>>
49-
create_action_server(
47+
typename std::shared_ptr<actions::ActionServer<ActionT>> create_action_server(
5048
std::string action_name,
5149
std::function<
5250
void(std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)>
5351
execute_callback) {
5452

55-
std::shared_ptr<actions::ActionSingleServer<ActionT>> action_server(
56-
new actions::ActionSingleServer<ActionT>(this, action_name,
57-
execute_callback),
53+
std::shared_ptr<actions::ActionServer<ActionT>> action_server(
54+
new actions::ActionServer<ActionT>(this, action_name, execute_callback),
5855
this->create_action_deleter<rclcpp_action::Server<ActionT>>());
5956

6057
this->get_node_waitables_interface()->add_waitable(action_server,
@@ -63,54 +60,16 @@ class Node : public rclcpp::Node {
6360
}
6461

6562
template <typename ActionT>
66-
typename std::shared_ptr<actions::ActionSingleServer<ActionT>>
67-
create_action_server(
63+
typename std::shared_ptr<actions::ActionServer<ActionT>> create_action_server(
6864
std::string action_name,
6965
std::function<
7066
void(std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)>
7167
execute_callback,
7268
std::function<void()> cancel_callback) {
7369

74-
std::shared_ptr<actions::ActionSingleServer<ActionT>> action_server(
75-
new actions::ActionSingleServer<ActionT>(
76-
this, action_name, execute_callback, cancel_callback),
77-
this->create_action_deleter<rclcpp_action::Server<ActionT>>());
78-
79-
this->get_node_waitables_interface()->add_waitable(action_server,
80-
this->group);
81-
return action_server;
82-
}
83-
84-
template <typename ActionT>
85-
typename std::shared_ptr<actions::ActionQueueServer<ActionT>>
86-
create_action_queue_server(
87-
std::string action_name,
88-
std::function<
89-
void(std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)>
90-
execute_callback) {
91-
92-
std::shared_ptr<actions::ActionQueueServer<ActionT>> action_server(
93-
new actions::ActionQueueServer<ActionT>(this, action_name,
94-
execute_callback),
95-
this->create_action_deleter<rclcpp_action::Server<ActionT>>());
96-
97-
this->get_node_waitables_interface()->add_waitable(action_server,
98-
this->group);
99-
return action_server;
100-
}
101-
102-
template <typename ActionT>
103-
typename std::shared_ptr<actions::ActionQueueServer<ActionT>>
104-
create_action_queue_server(
105-
std::string action_name,
106-
std::function<
107-
void(std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)>
108-
execute_callback,
109-
std::function<void()> cancel_callback) {
110-
111-
std::shared_ptr<actions::ActionQueueServer<ActionT>> action_server(
112-
new actions::ActionQueueServer<ActionT>(
113-
this, action_name, execute_callback, cancel_callback),
70+
std::shared_ptr<actions::ActionServer<ActionT>> action_server(
71+
new actions::ActionServer<ActionT>(this, action_name, execute_callback,
72+
cancel_callback),
11473
this->create_action_deleter<rclcpp_action::Server<ActionT>>());
11574

11675
this->get_node_waitables_interface()->add_waitable(action_server,

‎simple_node/simple_node/actions/__init__.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11

22
from .client import ActionClient
3-
from .server import (ActionQueueServer, ActionSingleServer)
3+
from .server import ActionServer

‎simple_node/simple_node/actions/server/__init__.py‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1 @@
1-
2-
from .action_queue_server import ActionQueueServer
3-
from .action_single_server import ActionSingleServer
1+
from .action_server import ActionServer

0 commit comments

Comments
 (0)