Skip to content

Commit fbbac0e

Browse files
LaurieCheers-unityLaurieCheers-unity
authored
Merge pull request #108 from Unity-Technologies/laurie/RedoRos2
Standardize and fix merging from ROS1 to ROS2
2 parents 248e564 + 6fedd90 commit fbbac0e

16 files changed

+327
-98
lines changed

‎.github/workflows/pre-commit.yaml

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,4 +11,3 @@ jobs:
1111
- uses: actions/setup-python@v2
1212
with:
1313
python-version: 3.7.x
14-
- uses: pre-commit/action@v2.0.0

‎launch/endpoint.py

Lines changed: 10 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -3,15 +3,13 @@
33

44

55
def generate_launch_description():
6-
return LaunchDescription([
7-
Node(
8-
package='ros_tcp_endpoint',
9-
executable='default_server_endpoint',
10-
emulate_tty=True,
11-
parameters=[
12-
{'ROS_IP': '0.0.0.0'},
13-
{'ROS_TCP_PORT': 10000},
14-
],
15-
),
16-
])
17-
6+
return LaunchDescription(
7+
[
8+
Node(
9+
package="ros_tcp_endpoint",
10+
executable="default_server_endpoint",
11+
emulate_tty=True,
12+
parameters=[{"ROS_IP": "0.0.0.0"}, {"ROS_TCP_PORT": 10000}],
13+
)
14+
]
15+
)

‎ros_tcp_endpoint/client.py

Lines changed: 6 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -148,7 +148,7 @@ def send_ros_service_request(self, srv_id, destination, data):
148148
destination, self.tcp_server.ros_services_table.keys()
149149
)
150150
self.tcp_server.send_unity_error(error_msg)
151-
self.logerr(error_msg)
151+
self.tcp_server.logerr(error_msg)
152152
# TODO: send a response to Unity anyway?
153153
return
154154
else:
@@ -165,18 +165,12 @@ def service_call_thread(self, srv_id, destination, data, ros_communicator):
165165
if not response:
166166
error_msg = "No response data from service '{}'!".format(destination)
167167
self.tcp_server.send_unity_error(error_msg)
168-
self.logerr(error_msg)
168+
self.tcp_server.logerr(error_msg)
169169
# TODO: send a response to Unity anyway?
170170
return
171171

172172
self.tcp_server.unity_tcp_sender.send_ros_service_response(srv_id, destination, response)
173173

174-
def loginfo(self, text):
175-
self.tcp_server.get_logger().info(text)
176-
177-
def logerr(self, text):
178-
self.tcp_server.get_logger().error(text)
179-
180174
def run(self):
181175
"""
182176
Receive a message from Unity and determine where to send it based on the publishers table
@@ -192,7 +186,7 @@ def run(self):
192186
msg: the ROS msg type as bytes
193187
194188
"""
195-
self.loginfo("Connection from {}".format(self.incoming_ip))
189+
self.tcp_server.loginfo("Connection from {}".format(self.incoming_ip))
196190
halt_event = threading.Event()
197191
self.tcp_server.unity_tcp_sender.start_sender(self.conn, halt_event)
198192
try:
@@ -225,10 +219,10 @@ def run(self):
225219
destination, self.tcp_server.publishers_table.keys()
226220
)
227221
self.tcp_server.send_unity_error(error_msg)
228-
self.logerr(error_msg)
222+
self.tcp_server.logerr(error_msg)
229223
except IOError as e:
230-
self.logerr("Exception: {}".format(e))
224+
self.tcp_server.logerr("Exception: {}".format(e))
231225
finally:
232226
halt_event.set()
233227
self.conn.close()
234-
self.loginfo("Disconnected from {}".format(self.incoming_ip))
228+
self.tcp_server.loginfo("Disconnected from {}".format(self.incoming_ip))

‎ros_tcp_endpoint/communication.py

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ class RosSender(Node):
2020
"""
2121
Base class for ROS communication where data is sent to the ROS network.
2222
"""
23+
2324
def __init__(self, node_name):
2425
super().__init__(node_name)
2526
pass
@@ -32,6 +33,7 @@ class RosReceiver(Node):
3233
"""
3334
Base class for ROS communication where data is being sent outside of the ROS network.
3435
"""
36+
3537
def __init__(self, node_name):
3638
super().__init__(node_name)
3739
pass

‎ros_tcp_endpoint/default_server_endpoint.py

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

88
def main(args=None):
99
rclpy.init(args=args)
10-
tcp_server = TcpServer('TCPServer')
10+
tcp_server = TcpServer("UnityEndpoint")
1111

1212
tcp_server.start()
1313

‎ros_tcp_endpoint/server.py

Lines changed: 28 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -52,15 +52,13 @@ def __init__(self, node_name, buffer_size=1024, connections=10, tcp_ip=None, tcp
5252
self.declare_parameter("ROS_TCP_PORT", 10000)
5353

5454
if tcp_ip:
55-
self.get_logger().log("Using ROS_IP override from constructor: {}".format(tcp_ip))
55+
self.loginfo("Using ROS_IP override from constructor: {}".format(tcp_ip))
5656
self.tcp_ip = tcp_ip
5757
else:
5858
self.tcp_ip = self.get_parameter("ROS_IP").get_parameter_value().string_value
5959

6060
if tcp_port:
61-
self.get_logger().log(
62-
"Using ROS_TCP_PORT override from constructor: {}".format(tcp_port)
63-
)
61+
self.loginfo("Using ROS_TCP_PORT override from constructor: {}".format(tcp_port))
6462
self.tcp_port = tcp_port
6563
else:
6664
self.tcp_port = self.get_parameter("ROS_TCP_PORT").get_parameter_value().integer_value
@@ -93,7 +91,7 @@ def listen_loop(self):
9391
Creates and binds sockets using TCP variables then listens for incoming connections.
9492
For each new connection a client thread will be created to handle communication.
9593
"""
96-
self.get_logger().info("Starting server on {}:{}".format(self.tcp_ip, self.tcp_port))
94+
self.loginfo("Starting server on {}:{}".format(self.tcp_ip, self.tcp_port))
9795
tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
9896
tcp_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
9997
tcp_server.bind((self.tcp_ip, self.tcp_port))
@@ -105,7 +103,7 @@ def listen_loop(self):
105103
(conn, (ip, port)) = tcp_server.accept()
106104
ClientThread(conn, self, ip, port).start()
107105
except socket.timeout as err:
108-
self.get_logger().error("ros_tcp_endpoint.TcpServer: socket timeout")
106+
self.logerr("ros_tcp_endpoint.TcpServer: socket timeout")
109107

110108
def send_unity_error(self, error):
111109
self.unity_tcp_sender.send_unity_error(error)
@@ -128,6 +126,15 @@ def handle_syscommand(self, topic, data):
128126
params = json.loads(message_json)
129127
function(**params)
130128

129+
def loginfo(self, text):
130+
self.get_logger().info(text)
131+
132+
def logwarn(self, text):
133+
self.get_logger().warning(text)
134+
135+
def logerr(self, text):
136+
self.get_logger().error(text)
137+
131138
def setup_executor(self):
132139
"""
133140
Since rclpy.spin() is a blocking call the server needed a way
@@ -136,7 +143,13 @@ def setup_executor(self):
136143
MultiThreadedExecutor allows us to set the number of threads
137144
needed as well as the nodes that need to be spun.
138145
"""
139-
num_threads = len(self.publishers_table.keys()) + len(self.subscribers_table.keys()) + len(self.ros_services_table.keys()) + len(self.unity_services_table.keys()) + 1
146+
num_threads = (
147+
len(self.publishers_table.keys())
148+
+ len(self.subscribers_table.keys())
149+
+ len(self.ros_services_table.keys())
150+
+ len(self.unity_services_table.keys())
151+
+ 1
152+
)
140153
executor = MultiThreadedExecutor(num_threads)
141154

142155
executor.add_node(self)
@@ -204,9 +217,7 @@ def subscribe(self, topic, message_name):
204217
if self.tcp_server.executor is not None:
205218
self.tcp_server.executor.add_node(new_subscriber)
206219

207-
self.tcp_server.get_logger().info(
208-
"RegisterSubscriber({}, {}) OK".format(topic, message_class)
209-
)
220+
self.tcp_server.loginfo("RegisterSubscriber({}, {}) OK".format(topic, message_class))
210221

211222
def publish(self, topic, message_name, queue_size=10, latch=False):
212223
if topic == "":
@@ -234,9 +245,7 @@ def publish(self, topic, message_name, queue_size=10, latch=False):
234245
if self.tcp_server.executor is not None:
235246
self.tcp_server.executor.add_node(new_publisher)
236247

237-
self.tcp_server.get_logger().info(
238-
"RegisterPublisher({}, {}) OK".format(topic, message_class)
239-
)
248+
self.tcp_server.loginfo("RegisterPublisher({}, {}) OK".format(topic, message_class))
240249

241250
def ros_service(self, topic, message_name):
242251
if topic == "":
@@ -265,9 +274,7 @@ def ros_service(self, topic, message_name):
265274
if self.tcp_server.executor is not None:
266275
self.tcp_server.executor.add_node(new_service)
267276

268-
self.tcp_server.get_logger().info(
269-
"RegisterRosService({}, {}) OK".format(topic, message_class)
270-
)
277+
self.tcp_server.loginfo("RegisterRosService({}, {}) OK".format(topic, message_class))
271278

272279
def unity_service(self, topic, message_name):
273280
if topic == "":
@@ -297,9 +304,7 @@ def unity_service(self, topic, message_name):
297304
if self.tcp_server.executor is not None:
298305
self.tcp_server.executor.add_node(new_service)
299306

300-
self.tcp_server.get_logger().info(
301-
"RegisterUnityService({}, {}) OK".format(topic, message_class)
302-
)
307+
self.tcp_server.loginfo("RegisterUnityService({}, {}) OK".format(topic, message_class))
303308

304309
def response(self, srv_id): # the next message is a service response
305310
self.tcp_server.pending_srv_id = srv_id
@@ -320,20 +325,18 @@ def resolve_message_name(self, name, extension="msg"):
320325
importlib.import_module(module_name + "." + extension)
321326
module = sys.modules[module_name]
322327
if module is None:
323-
self.tcp_server.get_logger().error(
324-
"Failed to resolve module {}".format(module_name)
325-
)
328+
self.tcp_server.logerr("Failed to resolve module {}".format(module_name))
326329
module = getattr(module, extension)
327330
if module is None:
328-
self.tcp_server.get_logger().error(
331+
self.tcp_server.logerr(
329332
"Failed to resolve module {}.{}".format(module_name, extension)
330333
)
331334
module = getattr(module, class_name)
332335
if module is None:
333-
self.tcp_server.get_logger().error(
336+
self.tcp_server.logerr(
334337
"Failed to resolve module {}.{}.{}".format(module_name, extension, class_name)
335338
)
336339
return module
337340
except (IndexError, KeyError, AttributeError, ImportError) as e:
338-
self.tcp_server.get_logger().error("Failed to resolve message name: {}".format(e))
341+
self.tcp_server.logerr("Failed to resolve message name: {}".format(e))
339342
return None

‎ros_tcp_endpoint/service.py

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -24,21 +24,21 @@ class RosService(RosSender):
2424
"""
2525
Class to send messages to a ROS service.
2626
"""
27+
2728
def __init__(self, service, service_class):
2829
"""
2930
Args:
3031
service: The service name in ROS
3132
service_class: The service class in catkin workspace
3233
"""
33-
strippedService = re.sub('[^A-Za-z0-9_]+', '', service)
34-
node_name = f'{strippedService}_RosService'
34+
strippedService = re.sub("[^A-Za-z0-9_]+", "", service)
35+
node_name = f"{strippedService}_RosService"
3536
RosSender.__init__(self, node_name)
36-
37+
3738
self.service_topic = service
3839
self.cli = self.create_client(service_class, service)
3940
self.req = service_class.Request()
4041

41-
4242
def send(self, data):
4343
"""
4444
Takes in serialized message data from source outside of the ROS network,
@@ -55,9 +55,11 @@ def send(self, data):
5555
message = deserialize_message(data, message_type)
5656

5757
if not self.cli.service_is_ready():
58-
self.get_logger().error('Ignoring service call to {} - service is not ready.'.format(self.service_topic))
58+
self.get_logger().error(
59+
"Ignoring service call to {} - service is not ready.".format(self.service_topic)
60+
)
5961
return None
60-
62+
6163
self.future = self.cli.call_async(message)
6264

6365
while rclpy.ok():
@@ -66,13 +68,12 @@ def send(self, data):
6668
response = self.future.result()
6769
return response
6870
except Exception as e:
69-
self.get_logger().info(f'Service call failed {e}')
71+
self.get_logger().info(f"Service call failed {e}")
7072

7173
break
7274

7375
return None
7476

75-
7677
def unregister(self):
7778
"""
7879

‎ros_tcp_endpoint/subscriber.py

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -35,8 +35,8 @@ def __init__(self, topic, message_class, tcp_server, queue_size=10):
3535
message_class: The message class in catkin workspace
3636
queue_size: Max number of entries to maintain in an outgoing queue
3737
"""
38-
strippedTopic = re.sub('[^A-Za-z0-9_]+', '', topic)
39-
self.node_name = f'{strippedTopic}_RosSubscriber'
38+
strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic)
39+
self.node_name = f"{strippedTopic}_RosSubscriber"
4040
RosReceiver.__init__(self, self.node_name)
4141
self.topic = topic
4242
self.msg = message_class
@@ -47,10 +47,7 @@ def __init__(self, topic, message_class, tcp_server, queue_size=10):
4747

4848
# Start Subscriber listener function
4949
self.subscription = self.create_subscription(
50-
self.msg,
51-
self.topic,
52-
self.send,
53-
qos_profile #queue_size
50+
self.msg, self.topic, self.send, qos_profile # queue_size
5451
)
5552
self.subscription
5653

@@ -74,4 +71,4 @@ def unregister(self):
7471
7572
"""
7673
self.destroy_subscription(self.subscription)
77-
self.destroy_node()
74+
self.destroy_node()

‎ros_tcp_endpoint/tcp_sender.py

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -138,12 +138,11 @@ def send_topic_list(self):
138138
topic_list.topics = [item[0] for item in topics_and_types]
139139
for i in topics_and_types:
140140
node = self.get_registered_topic(i[0])
141-
if (len(i[1]) > 1):
142-
if (node is not None):
141+
if len(i[1]) > 1:
142+
if node is not None:
143143
self.tcp_server.get_logger().warning(
144144
"Only one message type per topic is supported, but found multiple types for topic {}; maintaining {} as the subscribed type.".format(
145-
i[0],
146-
self.parse_message_name(node.msg),
145+
i[0], self.parse_message_name(node.msg)
147146
)
148147
)
149148
topic_list.types = [
@@ -186,7 +185,7 @@ def sender_loop(self, conn, tid, halt_event):
186185
try:
187186
conn.sendall(item)
188187
except Exception as e:
189-
self.tcp_server.get_logger().info("Exception {}".format(e))
188+
self.tcp_server.logerr("Exception {}".format(e))
190189
break
191190
finally:
192191
halt_event.set()
@@ -202,7 +201,7 @@ def parse_message_name(self, name):
202201
class_name = names[-1].split("_")[-1][:-2]
203202
return "{}/{}".format(module_name, class_name)
204203
except (IndexError, AttributeError, ImportError) as e:
205-
self.tcp_server.get_logger().error("Failed to resolve message name: {}".format(e))
204+
self.tcp_server.logerr("Failed to resolve message name: {}".format(e))
206205
return None
207206

208207

‎ros_tcp_endpoint/unity_service.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ class UnityService(RosReceiver):
2424
"""
2525
Class to register a ROS service that's implemented in Unity.
2626
"""
27+
2728
def __init__(self, topic, service_class, tcp_server, queue_size=10):
2829
"""
2930
@@ -32,8 +33,8 @@ def __init__(self, topic, service_class, tcp_server, queue_size=10):
3233
service_class: The message class in catkin workspace
3334
queue_size: Max number of entries to maintain in an outgoing queue
3435
"""
35-
strippedTopic = re.sub('[^A-Za-z0-9_]+', '', topic)
36-
node_name = f'{strippedTopic}_service'
36+
strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic)
37+
node_name = f"{strippedTopic}_service"
3738
RosReceiver.__init__(self, node_name)
3839

3940
self.topic = topic
@@ -53,8 +54,7 @@ def send(self, request, response):
5354
Returns:
5455
The response message
5556
"""
56-
response = self.tcp_server.send_unity_service(self.topic, self.service_class, request)
57-
return response
57+
return self.tcp_server.send_unity_service(self.topic, self.service_class, request)
5858

5959
def unregister(self):
6060
"""

0 commit comments

Comments
 (0)