@@ -37,7 +37,7 @@ class TcpServer(Node):
37
37
Initializes ROS node and TCP server.
38
38
"""
39
39
40
- def __init__ (self , node_name , buffer_size = 1024 , connections = 10 , tcp_ip = "" , tcp_port = - 1 , timeout = 10 ):
40
+ def __init__ (self , node_name , buffer_size = 1024 , connections = 10 , tcp_ip = None , tcp_port = None ):
41
41
"""
42
42
Initializes ROS node and class variables.
43
43
@@ -48,15 +48,17 @@ def __init__(self, node_name, buffer_size=1024, connections=10, tcp_ip="", tcp_p
48
48
"""
49
49
super ().__init__ (node_name )
50
50
51
- self .declare_parameter ("ROS_IP" , "127 .0.0.1 " )
51
+ self .declare_parameter ("ROS_IP" , "0 .0.0.0 " )
52
52
self .declare_parameter ("ROS_TCP_PORT" , 10000 )
53
53
54
- if tcp_ip != "" :
54
+ if tcp_ip :
55
+ self .get_logger ().log ("Using ROS_IP override from constructor: {}" .format (tcp_ip ))
55
56
self .tcp_ip = tcp_ip
56
57
else :
57
58
self .tcp_ip = self .get_parameter ("ROS_IP" ).get_parameter_value ().string_value
58
-
59
- if tcp_port != - 1 :
59
+
60
+ if tcp_port :
61
+ self .get_logger ().log ("Using ROS_TCP_PORT override from constructor: {}" .format (tcp_port ))
60
62
self .tcp_port = tcp_port
61
63
else :
62
64
self .tcp_port = self .get_parameter ("ROS_TCP_PORT" ).get_parameter_value ().integer_value
@@ -70,15 +72,15 @@ def __init__(self, node_name, buffer_size=1024, connections=10, tcp_ip="", tcp_p
70
72
self .syscommands = SysCommands (self )
71
73
self .pending_srv_id = None
72
74
self .pending_srv_is_request = False
73
-
75
+
74
76
def start (self , source_destination_dict = None ):
75
77
if source_destination_dict is not None :
76
78
self .source_destination_dict = source_destination_dict
77
79
server_thread = threading .Thread (target = self .listen_loop )
78
80
# Exit the server thread when the main thread terminates
79
81
server_thread .daemon = True
80
82
server_thread .start ()
81
-
83
+
82
84
def listen_loop (self ):
83
85
"""
84
86
Creates and binds sockets using TCP variables then listens for incoming connections.
@@ -91,7 +93,7 @@ def listen_loop(self):
91
93
92
94
while True :
93
95
tcp_server .listen (self .connections )
94
-
96
+
95
97
try :
96
98
(conn , (ip , port )) = tcp_server .accept ()
97
99
ClientThread (conn , self , ip , port ).start ()
@@ -119,7 +121,6 @@ def handle_syscommand(self, topic, data):
119
121
params = json .loads (message_json )
120
122
function (** params )
121
123
122
-
123
124
def setup_executor (self ):
124
125
"""
125
126
Since rclpy.spin() is a blocking call the server needed a way
@@ -155,6 +156,7 @@ def destroy_nodes(self):
155
156
156
157
self .destroy_node ()
157
158
159
+
158
160
class SysCommands :
159
161
def __init__ (self , tcp_server ):
160
162
self .tcp_server = tcp_server
@@ -169,7 +171,7 @@ def subscribe(self, topic, message_name):
169
171
if message_class is None :
170
172
self .tcp_server .send_unity_error ("SysCommand.subscribe - Unknown message class '{}'" .format (message_name ))
171
173
return
172
-
174
+
173
175
self .tcp_server .unregister_node (topic )
174
176
175
177
new_subscriber = RosSubscriber (topic , message_class , self .tcp_server )
@@ -191,9 +193,9 @@ def publish(self, topic, message_name):
191
193
return
192
194
193
195
self .tcp_server .unregister_node (topic )
194
-
196
+
195
197
new_publisher = RosPublisher (topic , message_class , queue_size = 10 )
196
-
198
+
197
199
self .tcp_server .source_destination_dict [topic ] = new_publisher
198
200
if self .tcp_server .executor is not None :
199
201
self .tcp_server .executor .add_node (new_publisher )
@@ -207,19 +209,20 @@ def ros_service(self, topic, message_name):
207
209
return
208
210
message_class = self .resolve_message_name (message_name , "srv" )
209
211
if message_class is None :
210
- self .tcp_server .send_unity_error ("RegisterRosService({}, {}) - Unknown service class '{}'" .format (topic , message_name , message_name ))
212
+ self .tcp_server .send_unity_error (
213
+ "RegisterRosService({}, {}) - Unknown service class '{}'" .format (topic , message_name , message_name ))
211
214
return
212
215
213
216
self .tcp_server .unregister_node (topic )
214
-
217
+
215
218
new_service = RosService (topic , message_class )
216
219
217
220
self .tcp_server .source_destination_dict [topic ] = new_service
218
221
if self .tcp_server .executor is not None :
219
222
self .tcp_server .executor .add_node (new_service )
220
223
221
224
self .tcp_server .get_logger ().info ("RegisterRosService({}, {}) OK" .format (topic , message_class ))
222
-
225
+
223
226
def unity_service (self , topic , message_name ):
224
227
if topic == '' :
225
228
self .tcp_server .send_unity_error (
@@ -228,7 +231,8 @@ def unity_service(self, topic, message_name):
228
231
229
232
message_class = self .resolve_message_name (message_name , "srv" )
230
233
if message_class is None :
231
- self .tcp_server .send_unity_error ("RegisterUnityService({}, {}) - Unknown service class '{}'" .format (topic , message_name , message_name ))
234
+ self .tcp_server .send_unity_error (
235
+ "RegisterUnityService({}, {}) - Unknown service class '{}'" .format (topic , message_name , message_name ))
232
236
return
233
237
234
238
self .tcp_server .unregister_node (topic )
@@ -238,15 +242,15 @@ def unity_service(self, topic, message_name):
238
242
self .tcp_server .source_destination_dict [topic ] = new_service
239
243
if self .tcp_server .executor is not None :
240
244
self .tcp_server .executor .add_node (new_service )
241
-
245
+
242
246
self .tcp_server .get_logger ().info ("RegisterUnityService({}, {}) OK" .format (topic , message_class ))
243
247
244
- def response (self , srv_id ): # the next message is a service response
245
- self .tcp_server .pending_srv_id = srv_id
248
+ def response (self , srv_id ): # the next message is a service response
249
+ self .tcp_server .pending_srv_id = srv_id
246
250
self .tcp_server .pending_srv_is_request = False
247
251
248
- def request (self , srv_id ): # the next message is a service request
249
- self .tcp_server .pending_srv_id = srv_id
252
+ def request (self , srv_id ): # the next message is a service request
253
+ self .tcp_server .pending_srv_id = srv_id
250
254
self .tcp_server .pending_srv_is_request = True
251
255
252
256
def topic_list (self ):
@@ -257,7 +261,7 @@ def resolve_message_name(self, name, extension="msg"):
257
261
names = name .split ('/' )
258
262
module_name = names [0 ]
259
263
class_name = names [1 ]
260
- importlib .import_module (module_name + "." + extension )
264
+ importlib .import_module (module_name + "." + extension )
261
265
module = sys .modules [module_name ]
262
266
if module is None :
263
267
self .tcp_server .get_logger ().error ("Failed to resolve module {}" .format (module_name ))
@@ -266,7 +270,8 @@ def resolve_message_name(self, name, extension="msg"):
266
270
self .tcp_server .get_logger ().error ("Failed to resolve module {}.{}" .format (module_name , extension ))
267
271
module = getattr (module , class_name )
268
272
if module is None :
269
- self .tcp_server .get_logger ().error ("Failed to resolve module {}.{}.{}" .format (module_name , extension , class_name ))
273
+ self .tcp_server .get_logger ().error (
274
+ "Failed to resolve module {}.{}.{}" .format (module_name , extension , class_name ))
270
275
return module
271
276
except (IndexError , KeyError , AttributeError , ImportError ) as e :
272
277
self .tcp_server .get_logger ().error ("Failed to resolve message name: {}" .format (e ))
0 commit comments