-
Notifications
You must be signed in to change notification settings - Fork 2
Expand file tree
/
Copy pathnode.py
More file actions
147 lines (117 loc) · 4.55 KB
/
Copy pathnode.py
File metadata and controls
147 lines (117 loc) · 4.55 KB
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
# Copyright (C) 2023 Miguel Ángel González Santamarta
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
"""ROS2 Node to simulate ROS1 Node"""
import time
from threading import Thread
from typing import List, Callable, Type
import rclpy
from rclpy.client import Client
from rclpy.context import Context
from rclpy.node import Node as Node2
from rclpy.parameter import Parameter
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor, Executor
from simple_node.actions.action_client import ActionClient
from simple_node.actions.action_server import ActionServer
class Node(Node2):
"""Node Class"""
def __init__(
self,
node_name: str,
context: Context = None,
cli_args: List[str] = None,
namespace: str = "",
use_global_arguments: bool = True,
enable_rosout: bool = True,
start_parameter_services: bool = True,
parameter_overrides: List[Parameter] = None,
allow_undeclared_parameters: bool = False,
automatically_declare_parameters_from_overrides: bool = False,
executor: Executor = None,
) -> None:
super().__init__(
node_name,
context=context,
cli_args=cli_args,
namespace=namespace,
use_global_arguments=use_global_arguments,
enable_rosout=enable_rosout,
start_parameter_services=start_parameter_services,
parameter_overrides=parameter_overrides,
allow_undeclared_parameters=allow_undeclared_parameters,
automatically_declare_parameters_from_overrides=automatically_declare_parameters_from_overrides,
)
if not executor:
self._executor = MultiThreadedExecutor()
else:
self._executor = executor
self._spin_thread = Thread(target=self._run_executor, daemon=True)
self._spin_thread.start()
self._wake_thread = Thread(target=self._wake_node, daemon=True)
self._wake_thread.start()
def _run_executor(self) -> None:
"""run an executer with self (node)"""
self._executor.add_node(self)
try:
self._executor.spin()
finally:
self._executor.shutdown()
def _wake_node(self) -> None:
while rclpy.ok():
self._executor.wake()
time.sleep(1)
def join_spin(self) -> None:
"""wait for spin thread"""
try:
self._spin_thread.join()
self._wake_thread.join()
finally:
self.get_logger().info("Destroying node " + self.get_name())
self.destroy_node()
def create_client(self, srv_type: Type, srv_name: str) -> Client:
return super().create_client(
srv_type, srv_name, callback_group=ReentrantCallbackGroup()
)
def create_action_client(
self, action_type: Type, action_name: str, feedback_cb: Callable = None
) -> ActionClient:
"""create action client from node
Args:
action_type ([type]): action type (msg action)
action_name (str): action name
Returns:
ActionClient: client created
"""
return ActionClient(self, action_type, action_name, feedback_cb)
def create_action_server(
self,
action_type: Type,
action_name: str,
execute_callback: Callable,
cancel_callback: Callable = None,
) -> ActionServer:
"""create action server from node
Args:
action_type ([type]): action type (msg action)
action_name (str): action name
execute_callback ([type]): execute function
cancel_callback ([type], optional): cancel function. Defaults to None.
Returns:
ActionSingleServer: server created
"""
return ActionServer(
self,
action_type,
action_name,
execute_callback,
cancel_callback=cancel_callback,
)