use single-threaded executors for cabot_ui_manager.py (#100)

Signed-off-by: Daisuke Sato <[email protected]>

cabot_ui/cabot_ui/menu.py

@@ -217,7 +217,7 @@ class Menu(object):
     def _get_path(self, name):
         return ".".join(["persistent", name])
 
-    def _get_saved_config(self, name, default=None):
+    def _get_saved_config(self, name, callback, default=None):
         try:
             self.get_client.wait_for_service(timeout_sec=3.0)
             path = self._get_path(name)
@@ -226,15 +226,21 @@ def _get_saved_config(self, name, default=None):
             req.names.append(path)
 
             CaBotRclpyUtil.info(f"getting the param {path}")
-            result = self.get_client.call(req)
-            CaBotRclpyUtil.info(f"got the result {result}")
-            if len(result.values) == 1:
-                return rclpy.parameter.parameter_value_to_python(result.values[0])
-            CaBotRclpyUtil.error("cannot get the parameter")
+            future = self.get_client.call_async(req)
+
+            def done_callback(future):
+                result = future.result()
+                CaBotRclpyUtil.info(f"got the result {result}")
+                if len(result.values) == 1:
+                    callback(rclpy.parameter.parameter_value_to_python(result.values[0]))
+                else:
+                    CaBotRclpyUtil.error("cannot get the parameter")
+                    callback(default)
+            future.add_done_callback(done_callback)
         except:  # noqa: #722
             if default is not None:
                 self._save_config(name, default)
-            return default
+            callback(default)
 
     def _save_config(self, name, value):
         CaBotRclpyUtil.info("save config")
@@ -246,7 +252,7 @@ def _save_config(self, name, value):
             req = SetParameters.Request()
             param = rclpy.parameter.Parameter(path, value=value).to_parameter_msg()
             req.parameters.append(param)
-            result = self.set_client.call(req)
+            result = self.set_client.call_async(req)
             CaBotRclpyUtil.info(f"{result}")
         except:  # noqa: #722
             CaBotRclpyUtil.error(traceback.format_exc())
@@ -265,24 +271,24 @@ def get_menu_config(config, name, default=None, error=False):
         return default
 
     @staticmethod
-    def create_menu(menu_obj, config, identifier=None, title=None, usage=None, parent=None):
+    def create_menu(menu_obj, config, callback=None, identifier=None, title=None, usage=None, parent=None):
+        CaBotRclpyUtil.info(F"create_menu {config=}")
         if not config:
             return None
-
         """Create menu from config"""
         # refer menu
         menu = config["menu"] if "menu" in config else None
         if menu is not None:
             config2 = menu_obj[menu] if menu in menu_obj else []
-            return Menu.create_menu(menu_obj, config2, identifier=menu, title=title, usage=usage, parent=parent)
+            return Menu.create_menu(menu_obj, config2, callback=callback, identifier=menu, title=title, usage=usage, parent=parent)
 
         # otherwise
         _type = Menu.get_menu_config(config, "type", "item")
 
         if _type == "list":
             return MenuList(menu_obj, config, identifier=identifier, parent=parent)
         elif _type == "adjust":
-            return MenuAdjust(config, identifier=identifier, parent=parent)
+            return MenuAdjust(config, identifier=identifier, parent=parent, callback=callback)
         elif _type == "item":
             return MenuItem(config, identifier=identifier, parent=parent)
 
@@ -452,7 +458,7 @@ def reset(self):
 
 class MenuAdjust(Menu):
     """Adjustable menu"""
-    def __init__(self, config=None, identifier=None, parent=None):
+    def __init__(self, config=None, identifier=None, parent=None, callback=None):
         super(MenuAdjust, self).__init__(config=config, identifier=identifier, parent=parent)
         self._type = Menu.Adjust
         self._max = Menu.get_menu_config(config, "max", error=True)
@@ -472,9 +478,14 @@ def __init__(self, config=None, identifier=None, parent=None):
 
         self._step = Menu.get_menu_config(config, "step", 1)
         self._name = Menu.get_menu_config(config, "name", error=True)
-        self._current = self._get_saved_current()
+
+        def current_value_callback(value):
+            self._current = value
+            self._check_action_once()
+            if callback:
+                callback()
+        self._get_saved_current(current_value_callback)
         self._actions = Actions.create_actions(config, self)
-        self._check_action_once()
 
     def _check_action(self):
         if self._actions is None:
@@ -488,11 +499,13 @@ def _check_action(self):
     def _check_action_once(self):
         self._check_action()
 
-    def _get_saved_current(self):
-        temp = self._get_saved_config(self._name, default=self._default)
-        if hasattr(self, "_values") and self._values is not None and isinstance(temp, str):
-            temp = self._values.index(temp)
-        return temp if temp is not None else self._default
+    def _get_saved_current(self, callback):
+        def config_callback(config):
+            temp = config
+            if hasattr(self, "_values") and self._values is not None and isinstance(temp, str):
+                temp = self._values.index(temp)
+            callback(temp if temp is not None else self._default)
+        self._get_saved_config(self._name, config_callback, default=self._default)
 
     def _save_current(self):
         if self._actions is not None:
@@ -548,7 +561,9 @@ def description(self):
         # " " + i18n.localized_string(self._title))
 
     def reset(self):
-        self._current = self._get_saved_current()
+        def current_value_callback(value):
+            self._current = value
+        self._get_saved_current(current_value_callback)
 
 
 class MenuItem(Menu):

cabot_ui/cabot_ui/navigation.py

@@ -1307,16 +1307,21 @@ def _goto_floor(self, floor, gh_callback, callback):
         self._logger.info(F"go to floor {floor}")
         self.delegate.activity_log("cabot/navigation", "go_to_floor", str(floor))
 
+        class GotoFloorTaskResult():
+            def __init__(self):
+                pass
+
         class GotoFloorTask():
             def __init__(self, nav):
                 self._nav = nav
                 self._node = nav._node
                 self._logger = nav._logger
                 self.buffer = nav.buffer
                 self.delegate = nav.delegate
-                self.future = self._node.executor.create_task(self.handler)
-                self.future.add_done_callback(lambda x: callback(True))
+                self.future = rclpy.task.Future()
                 self.rate = self._nav._node.create_rate(2)
+                self.thread = threading.Thread(target=self.run)
+                self.thread.start()
                 self.cancelled = False
 
             def cancel_goal_async(self):
@@ -1329,16 +1334,10 @@ def dummy():
                 self.cancelled = True
                 return self.cancel_future
 
-            def handler(self):
-                try:
-                    self._handle()
-                except:  # noqa: #722
-                    self._logger.error(traceback.format_exc())
-
-            def _handle(self):
+            def run(self):
                 first = True
                 while rclpy.ok():
-                    self.rate.sleep()
+                    time.sleep(0.5)
                     self._logger.info(F"GotoFloorTask loop cancelled={self.cancelled}")
                     if self.cancelled:
                         self._logger.info("GotoFloorTask cancelled")
@@ -1356,7 +1355,8 @@ def _handle(self):
                     except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                             tf2_ros.ExtrapolationException):
                         self._logger.warn(F"Could not find tf from map to {self._nav.current_frame}")
-                self.future.done()
+                self.future.set_result(GotoFloorTaskResult())
+                callback(self.future)
 
         task = GotoFloorTask(self)
         gh_callback(task)

cabot_ui/scripts/cabot_ui_manager.py

@@ -45,7 +45,6 @@
 import rclpy.client
 from rclpy.node import Node
 from rclpy.executors import SingleThreadedExecutor
-from rclpy.executors import MultiThreadedExecutor
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 import std_msgs.msg
 import std_srvs.srv
@@ -115,24 +114,13 @@ def manager_status(stat):
                 stat.summary(DiagnosticStatus.ERROR, "Not ready")
             return stat
         self.updater.add(FunctionDiagnosticTask("UI Manager", manager_status))
-
+        self._logger.info("set timer for create_menu")
         self.create_menu_timer = self._node.create_timer(1.0, self.create_menu, callback_group=MutuallyExclusiveCallbackGroup())
 
     def create_menu(self):
+        self._logger.info("create_menu is called")
         try:
-            self.create_menu_timer.cancel()
-            menu_file = node.declare_parameter('menu_file', '').value
-            with open(menu_file, 'r') as stream:
-                menu_obj = yaml.safe_load(stream)
-                self.main_menu = Menu.create_menu(menu_obj, {"menu": "main_menu"})
-            self.speed_menu = None
-            if self.main_menu:
-                self.main_menu.delegate = self
-                self.speed_menu = self.main_menu.get_menu_by_identifier("max_velocity_menu")
-            else:
-                self._logger.error("menu is not initialized")
-
-            if self.speed_menu:
+            def done_callback():
                 init_speed = self.speed_menu.value
                 try:
                     desc = ParameterDescriptor()
@@ -147,8 +135,14 @@ def create_menu(self):
                 self._logger.info(f"Initial Speed = {init_speed}")
                 self.speed_menu.set_value(init_speed)
 
-            self.menu_stack = []
-            self._logger.info("create_menu completed")
+                self.menu_stack = []
+                self._logger.info("create_menu completed")
+
+            self.create_menu_timer.cancel()
+            menu_file = node.declare_parameter('menu_file', '').value
+            with open(menu_file, 'r') as stream:
+                menu_obj = yaml.safe_load(stream)
+                self.speed_menu = Menu.create_menu(menu_obj, {"menu": "max_velocity_menu"}, done_callback)
         except:  # noqa: #722
             self._logger.error(traceback.format_exc())
 
@@ -625,11 +619,11 @@ def receiveSignal(signal_num, frame):
     act_node = Node("cabot_ui_manager_navigation_actions", start_parameter_services=False)
     soc_node = Node("cabot_ui_manager_navigation_social", start_parameter_services=False)
     nodes = [node, nav_node, tf_node, srv_node, act_node, soc_node]
-    executors = [MultiThreadedExecutor(),
-                 MultiThreadedExecutor(),
+    executors = [SingleThreadedExecutor(),
+                 SingleThreadedExecutor(),
+                 SingleThreadedExecutor(),
                  SingleThreadedExecutor(),
                  SingleThreadedExecutor(),
-                 MultiThreadedExecutor(),
                  SingleThreadedExecutor()]
     names = ["node", "tf", "nav", "srv", "act", "soc"]
     manager = CabotUIManager(node, nav_node, tf_node, srv_node, act_node, soc_node)