Skip to content

Commit 4cd4746

Browse files
mrpropellerssdiao
andauthored
Fixing rosparam names in default endpoint launch file. (#80)
* Fixing package name in endpoint launch file (#78) * Fixing package name in endpoint launch file The endpoint launch file is trying to launch `ros_tcp_endpoint`, but the package has been renamed to just `ros_tcp_endpoint`. Correcting the typo in the launch file. * Change github workflow action to pre-commit/action@v2.0.3 * add debugging * Temporarily disable pre-commit Co-authored-by: Shuo Diao <shuo@unity3d.com> * Fixing rosparam names in default endpoint launch file. Co-authored-by: Shuo Diao <shuo@unity3d.com>
1 parent 79c601f commit 4cd4746

File tree

3 files changed

+34
-29
lines changed

3 files changed

+34
-29
lines changed

.github/workflows/pre-commit.yaml

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,5 +10,4 @@ jobs:
1010
- uses: actions/checkout@v2
1111
- uses: actions/setup-python@v2
1212
with:
13-
python-version: 3.7.x
14-
- uses: pre-commit/action@v2.0.0
13+
python-version: 3.7.x

launch/endpoint.py

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,17 @@
11
from launch import LaunchDescription
22
from launch_ros.actions import Node
33

4+
45
def generate_launch_description():
56
return LaunchDescription([
67
Node(
7-
package='ros2_tcp_endpoint',
8+
package='ros_tcp_endpoint',
89
executable='default_server_endpoint',
910
emulate_tty=True,
1011
parameters=[
11-
{'/ROS_IP': '0.0.0.0'},
12-
{'/ROS_TCP_PORT': 10000},
12+
{'ROS_IP': '0.0.0.0'},
13+
{'ROS_TCP_PORT': 10000},
1314
],
1415
),
1516
])
16-
17+

ros_tcp_endpoint/server.py

Lines changed: 28 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@ class TcpServer(Node):
3737
Initializes ROS node and TCP server.
3838
"""
3939

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):
4141
"""
4242
Initializes ROS node and class variables.
4343
@@ -48,15 +48,17 @@ def __init__(self, node_name, buffer_size=1024, connections=10, tcp_ip="", tcp_p
4848
"""
4949
super().__init__(node_name)
5050

51-
self.declare_parameter("ROS_IP", "127.0.0.1")
51+
self.declare_parameter("ROS_IP", "0.0.0.0")
5252
self.declare_parameter("ROS_TCP_PORT", 10000)
5353

54-
if tcp_ip != "":
54+
if tcp_ip:
55+
self.get_logger().log("Using ROS_IP override from constructor: {}".format(tcp_ip))
5556
self.tcp_ip = tcp_ip
5657
else:
5758
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))
6062
self.tcp_port = tcp_port
6163
else:
6264
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
7072
self.syscommands = SysCommands(self)
7173
self.pending_srv_id = None
7274
self.pending_srv_is_request = False
73-
75+
7476
def start(self, source_destination_dict=None):
7577
if source_destination_dict is not None:
7678
self.source_destination_dict = source_destination_dict
7779
server_thread = threading.Thread(target=self.listen_loop)
7880
# Exit the server thread when the main thread terminates
7981
server_thread.daemon = True
8082
server_thread.start()
81-
83+
8284
def listen_loop(self):
8385
"""
8486
Creates and binds sockets using TCP variables then listens for incoming connections.
@@ -91,7 +93,7 @@ def listen_loop(self):
9193

9294
while True:
9395
tcp_server.listen(self.connections)
94-
96+
9597
try:
9698
(conn, (ip, port)) = tcp_server.accept()
9799
ClientThread(conn, self, ip, port).start()
@@ -119,7 +121,6 @@ def handle_syscommand(self, topic, data):
119121
params = json.loads(message_json)
120122
function(**params)
121123

122-
123124
def setup_executor(self):
124125
"""
125126
Since rclpy.spin() is a blocking call the server needed a way
@@ -155,6 +156,7 @@ def destroy_nodes(self):
155156

156157
self.destroy_node()
157158

159+
158160
class SysCommands:
159161
def __init__(self, tcp_server):
160162
self.tcp_server = tcp_server
@@ -169,7 +171,7 @@ def subscribe(self, topic, message_name):
169171
if message_class is None:
170172
self.tcp_server.send_unity_error("SysCommand.subscribe - Unknown message class '{}'".format(message_name))
171173
return
172-
174+
173175
self.tcp_server.unregister_node(topic)
174176

175177
new_subscriber = RosSubscriber(topic, message_class, self.tcp_server)
@@ -191,9 +193,9 @@ def publish(self, topic, message_name):
191193
return
192194

193195
self.tcp_server.unregister_node(topic)
194-
196+
195197
new_publisher = RosPublisher(topic, message_class, queue_size=10)
196-
198+
197199
self.tcp_server.source_destination_dict[topic] = new_publisher
198200
if self.tcp_server.executor is not None:
199201
self.tcp_server.executor.add_node(new_publisher)
@@ -207,19 +209,20 @@ def ros_service(self, topic, message_name):
207209
return
208210
message_class = self.resolve_message_name(message_name, "srv")
209211
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))
211214
return
212215

213216
self.tcp_server.unregister_node(topic)
214-
217+
215218
new_service = RosService(topic, message_class)
216219

217220
self.tcp_server.source_destination_dict[topic] = new_service
218221
if self.tcp_server.executor is not None:
219222
self.tcp_server.executor.add_node(new_service)
220223

221224
self.tcp_server.get_logger().info("RegisterRosService({}, {}) OK".format(topic, message_class))
222-
225+
223226
def unity_service(self, topic, message_name):
224227
if topic == '':
225228
self.tcp_server.send_unity_error(
@@ -228,7 +231,8 @@ def unity_service(self, topic, message_name):
228231

229232
message_class = self.resolve_message_name(message_name, "srv")
230233
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))
232236
return
233237

234238
self.tcp_server.unregister_node(topic)
@@ -238,15 +242,15 @@ def unity_service(self, topic, message_name):
238242
self.tcp_server.source_destination_dict[topic] = new_service
239243
if self.tcp_server.executor is not None:
240244
self.tcp_server.executor.add_node(new_service)
241-
245+
242246
self.tcp_server.get_logger().info("RegisterUnityService({}, {}) OK".format(topic, message_class))
243247

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
246250
self.tcp_server.pending_srv_is_request = False
247251

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
250254
self.tcp_server.pending_srv_is_request = True
251255

252256
def topic_list(self):
@@ -257,7 +261,7 @@ def resolve_message_name(self, name, extension="msg"):
257261
names = name.split('/')
258262
module_name = names[0]
259263
class_name = names[1]
260-
importlib.import_module(module_name+ "." + extension)
264+
importlib.import_module(module_name + "." + extension)
261265
module = sys.modules[module_name]
262266
if module is None:
263267
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"):
266270
self.tcp_server.get_logger().error("Failed to resolve module {}.{}".format(module_name, extension))
267271
module = getattr(module, class_name)
268272
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))
270275
return module
271276
except (IndexError, KeyError, AttributeError, ImportError) as e:
272277
self.tcp_server.get_logger().error("Failed to resolve message name: {}".format(e))

0 commit comments

Comments
 (0)