From 7c52691e25b45fd1a3a83b0ff3a80723334020c5 Mon Sep 17 00:00:00 2001 From: Richardvdketterij Date: Thu, 15 Feb 2024 17:21:52 +0100 Subject: [PATCH 1/7] don't spin inside the node, let the outside user determine how to spin this node and how to catch exceptions properly --- smach_ros/smach_ros/introspection.py | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/smach_ros/smach_ros/introspection.py b/smach_ros/smach_ros/introspection.py index 038d645..1e969d0 100644 --- a/smach_ros/smach_ros/introspection.py +++ b/smach_ros/smach_ros/introspection.py @@ -24,14 +24,6 @@ class IntrospectionClient(Node): def __init__(self, node_name='introspection_client', **kwargs): Node.__init__(self, node_name, **kwargs) - self._executor = SingleThreadedExecutor() - self._executor.add_node(self) - self._spinner = threading.Thread(target=self._executor.spin) - self._spinner.start() - - def __del__(self): - self._executor.shutdown() - self._spinner.join() def get_servers(self): """Get the base names that are broadcasting smach states.""" @@ -277,15 +269,6 @@ def __init__(self, server_name, state, path): self._state = state self._path = path - self._executor = SingleThreadedExecutor() - self._executor.add_node(self) - self._spinner = threading.Thread(target=self._executor.spin) - self._spinner.start() - - def __del__(self): - self._executor.shutdown() - self._spinner.join() - def start(self): # Construct proxies self.construct(self._server_name, self._state, self._path) From e420cb36d3b20fc58d23d188b054260ab984d620 Mon Sep 17 00:00:00 2001 From: Richardvdketterij Date: Fri, 16 Feb 2024 15:37:34 +0100 Subject: [PATCH 2/7] fix decoding messages --- smach_ros/smach_ros/introspection.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/smach_ros/smach_ros/introspection.py b/smach_ros/smach_ros/introspection.py index 1e969d0..11004dd 100644 --- a/smach_ros/smach_ros/introspection.py +++ b/smach_ros/smach_ros/introspection.py @@ -65,7 +65,7 @@ def set_initial_state(self, initial_status_msg = SmachContainerInitialStatusCmd( path = path, initial_states = initial_states, - local_data = bytearray(pickle.dumps(initial_userdata._data, 2))) + local_data = base64.b64encode(pickle.dumps(initial_userdata._data, 2))) # A status message to receive confirmation that the state was set properly msg_response = SmachContainerStatus() @@ -216,7 +216,7 @@ def _publish_status(self, info_str=''): path=path, initial_states=self._container.get_initial_states(), active_states=self._container.get_active_states(), - local_data=bytearray(pickle.dumps(self._container.userdata._data, 2)), + local_data=base64.b64encode(pickle.dumps(self._container.userdata._data, 2)), info=info_str) # Publish message self._status_pub.publish(state_msg) @@ -241,7 +241,7 @@ def _init_cmd_cb(self, msg): if msg.path == self._path: if all(s in self._container.get_children() for s in initial_states): ud = smach.UserData() - ud._data = pickle.loads(msg.local_data) + ud._data = pickle.loads(base64.b64decode(msg.local_data)) self._server_node.get_logger().debug("Setting initial state in smach path: '"+self._path+"' to '"+str(initial_states)+"' with userdata: "+str(ud._data)) # Set the initial state @@ -270,8 +270,8 @@ def __init__(self, server_name, state, path): self._path = path def start(self): - # Construct proxies - self.construct(self._server_name, self._state, self._path) + # Construct proxies + self.construct(self._server_name, self._state, self._path) def stop(self): for proxy in self._proxies: From 41923ff58fd6fe163faba37af9551d99d21aaf2f Mon Sep 17 00:00:00 2001 From: Richardvdketterij Date: Fri, 16 Feb 2024 15:38:41 +0100 Subject: [PATCH 3/7] fix test --- smach_ros/test/introspection_test.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/smach_ros/test/introspection_test.py b/smach_ros/test/introspection_test.py index 7acc7ee..8ad5789 100755 --- a/smach_ros/test/introspection_test.py +++ b/smach_ros/test/introspection_test.py @@ -41,8 +41,6 @@ def test_introspection(self): node = rclpy.create_node("sm_node") node.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) executor = SingleThreadedExecutor() - def spin(): - rclpy.spin(node, executor=executor) # Construct state machine sm = StateMachine(['done']) @@ -67,13 +65,22 @@ def spin(): # Run introspector intro_server = IntrospectionServer('intro_test', sm, '/intro_test') intro_server.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) - server_thread = threading.Thread(target=intro_server.start) - server_thread.start() + intro_server.start() intro_client = IntrospectionClient() intro_client.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) servers = intro_client.get_servers() + executor.add_node(node) + executor.add_node(intro_server) + executor.add_node(intro_client) + + def spin(executor): + executor.spin() + + spinner = threading.Thread(target=spin, args=(executor,)) + spinner.start() + rate = intro_client.create_rate(10) while '/intro_test' not in servers and rclpy.ok(): servers = intro_client.get_servers() @@ -93,9 +100,6 @@ def spin(): assert init_set - spinner = threading.Thread(target=spin) - spinner.start() - outcome = sm.execute() assert outcome == 'done' From 317d13e712be12950a90d088fc71dbbb0f1daa58 Mon Sep 17 00:00:00 2001 From: Richardvdketterij Date: Fri, 16 Feb 2024 15:39:53 +0100 Subject: [PATCH 4/7] fix indent --- smach_ros/smach_ros/introspection.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/smach_ros/smach_ros/introspection.py b/smach_ros/smach_ros/introspection.py index 11004dd..e9557ac 100644 --- a/smach_ros/smach_ros/introspection.py +++ b/smach_ros/smach_ros/introspection.py @@ -270,8 +270,8 @@ def __init__(self, server_name, state, path): self._path = path def start(self): - # Construct proxies - self.construct(self._server_name, self._state, self._path) + # Construct proxies + self.construct(self._server_name, self._state, self._path) def stop(self): for proxy in self._proxies: From 3e16727fcf0bd70dee6d051ccecc6f2db533b620 Mon Sep 17 00:00:00 2001 From: Richardvdketterij Date: Fri, 16 Feb 2024 15:42:39 +0100 Subject: [PATCH 5/7] remove import --- smach_ros/smach_ros/introspection.py | 1 - 1 file changed, 1 deletion(-) diff --git a/smach_ros/smach_ros/introspection.py b/smach_ros/smach_ros/introspection.py index e9557ac..327d696 100644 --- a/smach_ros/smach_ros/introspection.py +++ b/smach_ros/smach_ros/introspection.py @@ -2,7 +2,6 @@ import rclpy from rclpy.node import Node from rclpy.clock import ROSClock -from rclpy.executors import SingleThreadedExecutor from std_msgs.msg import Header import base64 From 1be2caa37df1b12e9c3aa7cd30e57663aae60119 Mon Sep 17 00:00:00 2001 From: Richardvdketterij Date: Fri, 16 Feb 2024 15:45:42 +0100 Subject: [PATCH 6/7] only import modules that we need and shutdown test properly --- smach_ros/test/introspection_test.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/smach_ros/test/introspection_test.py b/smach_ros/test/introspection_test.py index 8ad5789..b9db743 100755 --- a/smach_ros/test/introspection_test.py +++ b/smach_ros/test/introspection_test.py @@ -6,10 +6,8 @@ import unittest -from smach import * -from smach_ros import * - -from smach_msgs.msg import * +from smach import StateMachine, UserData +from smach_ros import RosState, IntrospectionServer, IntrospectionClient ### Custom state classe class Setter(RosState): @@ -110,9 +108,11 @@ def spin(executor): node.get_logger().info("Client destroyed") intro_server.destroy_node() node.get_logger().info("Server destroyed") - #executor.shutdown() - #spinner.join() + + executor.shutdown() node.destroy_node() + spinner.join() + def main(): rclpy.init() From 54adb18bb28097004654731c2780fe9e1ce447a6 Mon Sep 17 00:00:00 2001 From: Richardvdketterij Date: Fri, 16 Feb 2024 17:19:38 +0100 Subject: [PATCH 7/7] dont use rate in singlethreadedexecuter and split data if multiple --- smach_ros/smach_ros/introspection.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/smach_ros/smach_ros/introspection.py b/smach_ros/smach_ros/introspection.py index 327d696..f44a51f 100644 --- a/smach_ros/smach_ros/introspection.py +++ b/smach_ros/smach_ros/introspection.py @@ -5,6 +5,7 @@ from std_msgs.msg import Header import base64 +import time import pickle import threading from functools import partial @@ -91,13 +92,11 @@ def local_cb(msg_response, msg): 1) init_pub.publish(initial_status_msg) - clock = ROSClock() - rate = self.create_rate(4, clock) - start_time = clock.now() + start_time = self.get_clock().now() # Block until we get a new state back if timeout is not None: - while clock.now() - start_time < timeout: + while self.get_clock().now() - start_time < timeout: # Send the initial state command init_pub.publish(initial_status_msg) @@ -115,7 +114,7 @@ def local_cb(msg_response, msg): if state_match and ud_match: return True - rate.sleep() + time.sleep(0.25) return False class ContainerProxy(): @@ -207,15 +206,15 @@ def _publish_status(self, info_str=''): with self._status_pub_lock: path = self._path - #print str(structure_msg) - # Construct status message - #print self._container.get_active_states() + # Transform userdata to dictionary for pickling + keys = list(self._container.userdata.keys()) + data = {key: self._container.userdata[key] for key in keys} state_msg = SmachContainerStatus( header=Header(stamp = ROSClock().now().to_msg()), path=path, initial_states=self._container.get_initial_states(), active_states=self._container.get_active_states(), - local_data=base64.b64encode(pickle.dumps(self._container.userdata._data, 2)), + local_data=base64.b64encode(pickle.dumps(data, 2)), info=info_str) # Publish message self._status_pub.publish(state_msg)