Skip to content

Commit 5ceed25

Browse files
mgonzs13mgonzs13
committed
initial commit
1 parent 23c552b commit 5ceed25

22 files changed

+1248
-0
lines changed

‎README.md‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
# Simple Node
2+
3+
This is a new ROS 2 node to simplify the use of the spin and the executors. This way, service, actions and topics can be used as in ROS 1. Besides, this node also includes predefined action servers and one client to make its use more user-friendly to developers.
4+
5+
6+
## Examples
7+
8+
### Node
9+
10+
```python
11+
from simple_node import Node
12+
import rclpy
13+
14+
class MyNode(Node):
15+
def __init__(self):
16+
super().__init__("my_node")
17+
18+
def main(args=None):
19+
rclpy.init(args=args)
20+
21+
node = MyNode()
22+
23+
node.join_spin()
24+
25+
rclpy.shutdown()
26+
27+
28+
if __name__ == "__main__":
29+
main()
30+
31+
```
32+
33+
### Action Server
34+
35+
The following example shows how to create an action server that treats only one goal at the same time. If a new goal is received, the server aborts the current goal and starts treating the new one.
36+
37+
```python
38+
class MyNode(Node):
39+
def __init__(self):
40+
super().__init__("my_node")
41+
42+
self.__action_server = self.create_action_server(ActionType,
43+
"action_name",
44+
self.__execute_cb)
45+
46+
def __execute_server(self, goal_handle):
47+
result = ActionType.Result()
48+
49+
succeed = self.do_something(goal_handle.request)
50+
51+
if self.__action_server.is_canceled():
52+
self.__action_server.wait_for_canceling()
53+
goal_handle.canceled()
54+
55+
else:
56+
if succeed:
57+
goal_handle.succeed()
58+
else:
59+
goal_handle.abort()
60+
61+
return result
62+
```
63+
64+
### Action Client
65+
66+
The following example shows how to create an action client.
67+
68+
```python
69+
class MyNode(Node):
70+
def __init__(self):
71+
super().__init__("my_node")
72+
73+
self.__action_client = self.create_action_client(
74+
ActionType, "action_name")
75+
76+
goal = ActionType.Goal()
77+
78+
self.__action_client.wait_for_server()
79+
self.__action_client.send_goal(goal)
80+
self.__action_client.wait_for_result()
81+
82+
self.__action_client.is_succeeded():
83+
```

‎simple_node/CMakeLists.txt‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(simple_node)
3+
4+
# Default to C++14
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
9+
add_compile_options(-Wall -Wextra -Wpedantic)
10+
endif()
11+
12+
set(THREADS_PREFER_PTHREAD_FLAG ON)
13+
14+
# Find dependencies
15+
find_package(ament_cmake REQUIRED)
16+
find_package(ament_cmake_python REQUIRED)
17+
find_package(rclcpp REQUIRED)
18+
find_package(rcl_action REQUIRED)
19+
find_package(rclcpp_action REQUIRED)
20+
find_package(action_msgs REQUIRED)
21+
find_package(rclpy REQUIRED)
22+
find_package(Threads REQUIRED)
23+
24+
# C++
25+
include_directories(include)
26+
include_directories(src)
27+
28+
set(LIB ${CMAKE_PROJECT_NAME}_lib)
29+
set(SOURCES
30+
src/simple_node/node.cpp
31+
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
34+
)
35+
36+
set(DEPENDENCIES
37+
rclcpp
38+
rcl_action
39+
rclcpp_action
40+
action_msgs
41+
)
42+
43+
add_library(${LIB} STATIC ${SOURCES})
44+
45+
ament_target_dependencies(${LIB} ${DEPENDENCIES} Threads)
46+
47+
install(TARGETS ${LIB}
48+
ARCHIVE DESTINATION lib
49+
LIBRARY DESTINATION lib
50+
RUNTIME DESTINATION bin
51+
)
52+
53+
install(DIRECTORY include/
54+
DESTINATION include/
55+
)
56+
57+
ament_export_include_directories(include)
58+
ament_export_libraries(${LIB})
59+
ament_export_dependencies(${DEPENDENCIES})
60+
61+
62+
# Python
63+
ament_python_install_package(${PROJECT_NAME})
64+
65+
66+
ament_package()

‎simple_node/include/simple_node/actions/action_client.hpp‎

Lines changed: 186 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,186 @@
1+
2+
#ifndef ACTION_CLIENT_HPP
3+
#define ACTION_CLIENT_HPP
4+
5+
#include <chrono>
6+
#include <future>
7+
#include <string>
8+
#include <thread>
9+
10+
#include "rclcpp/rclcpp.hpp"
11+
#include "rclcpp_action/client_goal_handle.hpp"
12+
#include "rclcpp_action/rclcpp_action.hpp"
13+
14+
#include <action_msgs/msg/goal_status.hpp>
15+
16+
namespace simple_node {
17+
namespace actions {
18+
19+
template <typename ActionT>
20+
class ActionClient : public rclcpp_action::Client<ActionT> {
21+
22+
using Goal = typename ActionT::Goal;
23+
using GoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
24+
using Result = typename GoalHandle::Result::SharedPtr;
25+
26+
public:
27+
ActionClient(rclcpp::Node *node, std::string action_name)
28+
: rclcpp_action::Client<ActionT>(
29+
node->get_node_base_interface(), node->get_node_graph_interface(),
30+
node->get_node_logging_interface(), action_name) {
31+
32+
this->status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN;
33+
34+
this->goal_handle = nullptr;
35+
this->goal_thread = nullptr;
36+
this->result = nullptr;
37+
}
38+
39+
~ActionClient() { delete this->goal_thread; }
40+
41+
void wait_for_result() { this->goal_thread->join(); }
42+
43+
Result get_result() { return this->result; }
44+
45+
void send_goal(Goal goal) {
46+
this->set_status(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
47+
this->goal_thread = new std::thread(&ActionClient::_send_goal, this, goal);
48+
}
49+
50+
bool cancel_goal() {
51+
if (this->goal_handle != nullptr) {
52+
return this->_cancel_goal();
53+
} else {
54+
return false;
55+
}
56+
}
57+
58+
// **************
59+
// STATUS
60+
// **************
61+
int8_t get_status() {
62+
const std::lock_guard<std::mutex> lock(this->status_mtx);
63+
return this->status;
64+
}
65+
66+
bool is_accepted() {
67+
return this->get_status() == action_msgs::msg::GoalStatus::STATUS_ACCEPTED;
68+
}
69+
70+
bool is_executing() {
71+
return this->get_status() == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
72+
}
73+
74+
bool is_canceling() {
75+
return this->get_status() == action_msgs::msg::GoalStatus::STATUS_CANCELING;
76+
}
77+
78+
bool is_succeeded() {
79+
return this->get_status() == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
80+
}
81+
82+
bool is_canceled() {
83+
return this->get_status() == action_msgs::msg::GoalStatus::STATUS_CANCELED;
84+
}
85+
86+
bool is_aborted() {
87+
return this->get_status() == action_msgs::msg::GoalStatus::STATUS_ABORTED;
88+
}
89+
90+
bool is_working() {
91+
return (this->is_executing() || this->is_canceling() ||
92+
this->is_accepted());
93+
}
94+
95+
bool is_terminated() {
96+
return (this->is_succeeded() || this->is_canceled() || this->is_aborted());
97+
}
98+
99+
private:
100+
int8_t status;
101+
std::mutex status_mtx;
102+
std::thread *goal_thread;
103+
Result result;
104+
std::shared_ptr<GoalHandle> goal_handle;
105+
106+
void set_status(int8_t status) {
107+
const std::lock_guard<std::mutex> lock(this->status_mtx);
108+
this->status = status;
109+
}
110+
111+
void _send_goal(Goal goal) {
112+
113+
this->result = nullptr;
114+
115+
auto send_goal_future = this->async_send_goal(goal);
116+
117+
// wait for acceptance
118+
while (send_goal_future.wait_for(std::chrono::seconds(1)) !=
119+
std::future_status::ready) {
120+
}
121+
122+
// check acceptance
123+
this->goal_handle = send_goal_future.get();
124+
if (!this->goal_handle) {
125+
126+
// change status
127+
if (this->is_canceled()) {
128+
return;
129+
}
130+
this->set_status(action_msgs::msg::GoalStatus::STATUS_ABORTED);
131+
return;
132+
}
133+
134+
// change status
135+
if (this->is_canceled()) {
136+
return;
137+
}
138+
this->set_status(action_msgs::msg::GoalStatus::STATUS_ACCEPTED);
139+
140+
// get result
141+
auto get_result_future = this->async_get_result(this->goal_handle);
142+
143+
// change status
144+
if (this->is_canceled()) {
145+
return;
146+
}
147+
this->set_status(action_msgs::msg::GoalStatus::STATUS_EXECUTING);
148+
149+
// wait for result
150+
while (get_result_future.wait_for(std::chrono::seconds(1)) !=
151+
std::future_status::ready) {
152+
}
153+
154+
// change status
155+
if (this->is_canceled()) {
156+
return;
157+
}
158+
159+
this->set_status((uint8_t)get_result_future.get().code);
160+
this->result = get_result_future.get().result;
161+
}
162+
163+
bool _cancel_goal() {
164+
int8_t old_status = this->get_status();
165+
166+
auto cancel_goal_future = this->async_cancel_goal(this->goal_handle);
167+
this->set_status(action_msgs::msg::GoalStatus::STATUS_CANCELING);
168+
169+
while (cancel_goal_future.wait_for(std::chrono::seconds(1)) !=
170+
std::future_status::ready) {
171+
}
172+
173+
auto result = cancel_goal_future.get();
174+
if (result->goals_canceling.size() == 0) {
175+
this->set_status(old_status);
176+
return false;
177+
}
178+
179+
this->set_status(action_msgs::msg::GoalStatus::STATUS_CANCELED);
180+
181+
return true;
182+
}
183+
};
184+
} // namespace actions
185+
} // namespace simple_node
186+
#endif

0 commit comments

Comments
 (0)