diff --git a/cabot_ui/cabot_ui/menu.py b/cabot_ui/cabot_ui/menu.py index bab61717..e673a8c4 100644 --- a/cabot_ui/cabot_ui/menu.py +++ b/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,16 +271,16 @@ 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") @@ -282,7 +288,7 @@ def create_menu(menu_obj, config, identifier=None, title=None, usage=None, paren 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): diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 431e4062..3aaaf613 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -1307,6 +1307,10 @@ 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 @@ -1314,9 +1318,10 @@ def __init__(self, nav): 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) diff --git a/cabot_ui/scripts/cabot_ui_manager.py b/cabot_ui/scripts/cabot_ui_manager.py index 93aad471..b7920e5b 100755 --- a/cabot_ui/scripts/cabot_ui_manager.py +++ b/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)