From a74fc57389bee4e25947c94ee8a2c740da6e9753 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tom=C3=A1s=20Badenes?= Date: Tue, 26 Nov 2024 11:27:20 -0300 Subject: [PATCH 1/4] Prevent crashing and log errors --- .../inorbit_mir_connector/src/connector.py | 183 ++++++++++-------- 1 file changed, 101 insertions(+), 82 deletions(-) diff --git a/mir_connector/inorbit_mir_connector/src/connector.py b/mir_connector/inorbit_mir_connector/src/connector.py index 545a21e..ebacaab 100644 --- a/mir_connector/inorbit_mir_connector/src/connector.py +++ b/mir_connector/inorbit_mir_connector/src/connector.py @@ -113,92 +113,111 @@ def _inorbit_command_handler(self, command_name, args, options): - `metadata` is reserved for the future and will contains additional information about the received command request. """ - if command_name == COMMAND_CUSTOM_COMMAND: - self._logger.info(f"Received '{command_name}'!. {args}") - script_name = args[0] - script_args = args[1] - # TODO (Elvio): Needs to be re designed. - # 1. script_name is not standarized at all - # 2. Consider implementing a callback for handling mission specific commands - # 3. Needs an interface for supporting mission related actions - if script_name == "queue_mission" and script_args[0] == "--mission_id": - self.mission_tracking.mir_mission_tracking_enabled = ( - self._robot_session.missions_module.executor.wait_until_idle(0) - ) - self.mir_api.queue_mission(script_args[1]) - elif script_name == "run_mission_now" and script_args[0] == "--mission_id": - self.mission_tracking.mir_mission_tracking_enabled = ( - self._robot_session.missions_module.executor.wait_until_idle(0) - ) - self.mir_api.abort_all_missions() - self.mir_api.queue_mission(script_args[1]) - elif script_name == "abort_missions": - self._robot_session.missions_module.executor.cancel_mission("*") - self.mir_api.abort_all_missions() - elif script_name == "set_state": - if script_args[0] == "--state_id": - state_id = script_args[1] - if not state_id.isdigit() or int(state_id) not in MIR_STATE.keys(): - self._logger.error(f"Invalid `state_id` ({state_id})") - options["result_function"]("1") - return - state_id = int(state_id) - self._logger.info( - f"Setting robot state to state {state_id}: {MIR_STATE[state_id]}" + try: + if command_name == COMMAND_CUSTOM_COMMAND: + self._logger.info(f"Received '{command_name}'!. {args}") + if len(args) < 2: + self._logger.error("Invalid number of arguments: ", args) + options["result_function"]( + "1", execution_status_details="Invalid number of arguments" + ) + return + script_name = args[0] + script_args = args[1] + # TODO (Elvio): Needs to be re designed. + # 1. script_name is not standarized at all + # 2. Consider implementing a callback for handling mission specific commands + # 3. Needs an interface for supporting mission related actions + if script_name == "queue_mission" and script_args[0] == "--mission_id": + self.mission_tracking.mir_mission_tracking_enabled = ( + self._robot_session.missions_module.executor.wait_until_idle(0) ) - self.mir_api.set_state(state_id) - if script_args[0] == "--clear_error": - self._logger.info("Clearing error state") - self.mir_api.clear_error() - elif script_name == "set_waiting_for" and script_args[0] == "--text": - self._logger.info(f"Setting 'waiting for' value to {script_args[1]}") - self.mission_tracking.waiting_for_text = script_args[1] - elif script_name == "localize": - # The localize command sets the robot's position and current map - # The expected arguments are "x" and "y" in meters and "orientation" in degrees, as - # in MiR Fleet, and "map_id" as the target map in MiR Fleet, which matches the - # uploaded "frame_id" in InOrbit - if ( - len(script_args) == 8 - and script_args[0] == "--x" - and script_args[2] == "--y" - and script_args[4] == "--orientation" - and script_args[6] == "--map_id" - ): - status = { - "position": { - "x": float(script_args[1]), - "y": float(script_args[3]), - "orientation": float(script_args[5]), - }, - "map_id": script_args[7], - } - self._logger.info(f"Changing map to {script_args[7]}") - self.mir_api.set_status(status) + self.mir_api.queue_mission(script_args[1]) + elif script_name == "run_mission_now" and script_args[0] == "--mission_id": + self.mission_tracking.mir_mission_tracking_enabled = ( + self._robot_session.missions_module.executor.wait_until_idle(0) + ) + self.mir_api.abort_all_missions() + self.mir_api.queue_mission(script_args[1]) + elif script_name == "abort_missions": + self._robot_session.missions_module.executor.cancel_mission("*") + self.mir_api.abort_all_missions() + elif script_name == "set_state": + if script_args[0] == "--state_id": + state_id = script_args[1] + if not state_id.isdigit() or int(state_id) not in MIR_STATE.keys(): + error = f"Invalid `state_id` ({state_id})" + self._logger.error(error) + options["result_function"]("1", execution_status_details=error) + return + state_id = int(state_id) + self._logger.info( + f"Setting robot state to state {state_id}: {MIR_STATE[state_id]}" + ) + self.mir_api.set_state(state_id) + if script_args[0] == "--clear_error": + self._logger.info("Clearing error state") + self.mir_api.clear_error() + elif script_name == "set_waiting_for" and script_args[0] == "--text": + self._logger.info(f"Setting 'waiting for' value to {script_args[1]}") + self.mission_tracking.waiting_for_text = script_args[1] + elif script_name == "localize": + # The localize command sets the robot's position and current map + # The expected arguments are "x" and "y" in meters and "orientation" in degrees, + # as in MiR Fleet, and "map_id" as the target map in MiR Fleet, which matches + # the uploaded "frame_id" in InOrbit + if ( + len(script_args) == 8 + and script_args[0] == "--x" + and script_args[2] == "--y" + and script_args[4] == "--orientation" + and script_args[6] == "--map_id" + ): + status = { + "position": { + "x": float(script_args[1]), + "y": float(script_args[3]), + "orientation": float(script_args[5]), + }, + "map_id": script_args[7], + } + self._logger.info(f"Changing map to {script_args[7]}") + self.mir_api.set_status(status) + else: + self._logger.error("Invalid arguments for 'localize' command") + options["result_function"]( + "1", execution_status_details="Invalid arguments" + ) + return else: - self._logger.error("Invalid arguments for 'localize' command") - options["result_function"]("1", execution_status_details="Invalid arguments") + # Other kind if custom commands may be handled by the edge-sdk + # (e.g.user_scripts) and not by the connector code itself + # Do not return any result and leave it to the edge-sdk to handle it return + # Return '0' for success + options["result_function"]("0") + elif command_name == COMMAND_NAV_GOAL: + self._logger.info(f"Received '{command_name}'!. {args}") + pose = args[0] + self.send_waypoint_over_missions(pose) + elif command_name == COMMAND_MESSAGE: + msg = args[0] + if msg == "inorbit_pause": + self.mir_api.set_state(4) + elif msg == "inorbit_resume": + self.mir_api.set_state(3) else: - # Other kind if custom commands may be handled by the edge-sdk (e.g. user_scripts) - # and not by the connector code itself - # Do not return any result and leave it to the edge-sdk to handle it - return - # Return '0' for success - options["result_function"]("0") - elif command_name == COMMAND_NAV_GOAL: - self._logger.info(f"Received '{command_name}'!. {args}") - pose = args[0] - self.send_waypoint_over_missions(pose) - elif command_name == COMMAND_MESSAGE: - msg = args[0] - if msg == "inorbit_pause": - self.mir_api.set_state(4) - elif msg == "inorbit_resume": - self.mir_api.set_state(3) - - else: - self._logger.info(f"Received '{command_name}'!. {args}") + self._logger.info(f"Received unknown command '{command_name}'!. {args}") + except Exception as ex: + self._logger.error( + f"Failed to execute command '{command_name}' with args {args}. Exception:\n{ex}" + ) + options["result_function"]( + "1", + execution_status_details="An error occured executing custom command", + stderr=str(ex), + ) + return def _connect(self) -> None: """Connect to the robot services and to InOrbit""" From 04ed59555fc5192e964e4f3ca6e8720978eea67c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tom=C3=A1s=20Badenes?= Date: Tue, 26 Nov 2024 11:51:09 -0300 Subject: [PATCH 2/4] Fix tests --- mir_connector/inorbit_mir_connector/src/connector.py | 2 +- .../inorbit_mir_connector/tests/test_connector.py | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/mir_connector/inorbit_mir_connector/src/connector.py b/mir_connector/inorbit_mir_connector/src/connector.py index ebacaab..068f6bc 100644 --- a/mir_connector/inorbit_mir_connector/src/connector.py +++ b/mir_connector/inorbit_mir_connector/src/connector.py @@ -146,7 +146,7 @@ def _inorbit_command_handler(self, command_name, args, options): if script_args[0] == "--state_id": state_id = script_args[1] if not state_id.isdigit() or int(state_id) not in MIR_STATE.keys(): - error = f"Invalid `state_id` ({state_id})" + error = f"Invalid `state_id` '{state_id}'" self._logger.error(error) options["result_function"]("1", execution_status_details=error) return diff --git a/mir_connector/inorbit_mir_connector/tests/test_connector.py b/mir_connector/inorbit_mir_connector/tests/test_connector.py index 92edf91..74dca9d 100644 --- a/mir_connector/inorbit_mir_connector/tests/test_connector.py +++ b/mir_connector/inorbit_mir_connector/tests/test_connector.py @@ -189,12 +189,16 @@ def test_command_callback_state(connector, callback_kwargs): callback_kwargs["args"] = ["set_state", ["--state_id", "123"]] connector._inorbit_command_handler(**callback_kwargs) - callback_kwargs["options"]["result_function"].assert_called_with("1") + callback_kwargs["options"]["result_function"].assert_called_with( + "1", execution_status_details="Invalid `state_id` '123'" + ) assert not connector.mir_api.set_state.called callback_kwargs["args"] = ["set_state", ["--state_id", "abc"]] connector._inorbit_command_handler(**callback_kwargs) - callback_kwargs["options"]["result_function"].assert_called_with("1") + callback_kwargs["options"]["result_function"].assert_called_with( + "1", execution_status_details="Invalid `state_id` 'abc'" + ) assert not connector.mir_api.set_state.called From da253d19e6ee707d9e0d3cd5cbe6ab7129dffdee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tom=C3=A1s=20Badenes?= Date: Tue, 26 Nov 2024 13:34:21 -0300 Subject: [PATCH 3/4] Rely on the wrapper in the connector package --- .../inorbit_mir_connector/src/connector.py | 191 +++++++++--------- 1 file changed, 90 insertions(+), 101 deletions(-) diff --git a/mir_connector/inorbit_mir_connector/src/connector.py b/mir_connector/inorbit_mir_connector/src/connector.py index 068f6bc..aa1892b 100644 --- a/mir_connector/inorbit_mir_connector/src/connector.py +++ b/mir_connector/inorbit_mir_connector/src/connector.py @@ -113,111 +113,100 @@ def _inorbit_command_handler(self, command_name, args, options): - `metadata` is reserved for the future and will contains additional information about the received command request. """ - try: - if command_name == COMMAND_CUSTOM_COMMAND: - self._logger.info(f"Received '{command_name}'!. {args}") - if len(args) < 2: - self._logger.error("Invalid number of arguments: ", args) - options["result_function"]( - "1", execution_status_details="Invalid number of arguments" - ) - return - script_name = args[0] - script_args = args[1] - # TODO (Elvio): Needs to be re designed. - # 1. script_name is not standarized at all - # 2. Consider implementing a callback for handling mission specific commands - # 3. Needs an interface for supporting mission related actions - if script_name == "queue_mission" and script_args[0] == "--mission_id": - self.mission_tracking.mir_mission_tracking_enabled = ( - self._robot_session.missions_module.executor.wait_until_idle(0) - ) - self.mir_api.queue_mission(script_args[1]) - elif script_name == "run_mission_now" and script_args[0] == "--mission_id": - self.mission_tracking.mir_mission_tracking_enabled = ( - self._robot_session.missions_module.executor.wait_until_idle(0) - ) - self.mir_api.abort_all_missions() - self.mir_api.queue_mission(script_args[1]) - elif script_name == "abort_missions": - self._robot_session.missions_module.executor.cancel_mission("*") - self.mir_api.abort_all_missions() - elif script_name == "set_state": - if script_args[0] == "--state_id": - state_id = script_args[1] - if not state_id.isdigit() or int(state_id) not in MIR_STATE.keys(): - error = f"Invalid `state_id` '{state_id}'" - self._logger.error(error) - options["result_function"]("1", execution_status_details=error) - return - state_id = int(state_id) - self._logger.info( - f"Setting robot state to state {state_id}: {MIR_STATE[state_id]}" - ) - self.mir_api.set_state(state_id) - if script_args[0] == "--clear_error": - self._logger.info("Clearing error state") - self.mir_api.clear_error() - elif script_name == "set_waiting_for" and script_args[0] == "--text": - self._logger.info(f"Setting 'waiting for' value to {script_args[1]}") - self.mission_tracking.waiting_for_text = script_args[1] - elif script_name == "localize": - # The localize command sets the robot's position and current map - # The expected arguments are "x" and "y" in meters and "orientation" in degrees, - # as in MiR Fleet, and "map_id" as the target map in MiR Fleet, which matches - # the uploaded "frame_id" in InOrbit - if ( - len(script_args) == 8 - and script_args[0] == "--x" - and script_args[2] == "--y" - and script_args[4] == "--orientation" - and script_args[6] == "--map_id" - ): - status = { - "position": { - "x": float(script_args[1]), - "y": float(script_args[3]), - "orientation": float(script_args[5]), - }, - "map_id": script_args[7], - } - self._logger.info(f"Changing map to {script_args[7]}") - self.mir_api.set_status(status) - else: - self._logger.error("Invalid arguments for 'localize' command") - options["result_function"]( - "1", execution_status_details="Invalid arguments" - ) + if command_name == COMMAND_CUSTOM_COMMAND: + self._logger.info(f"Received '{command_name}'!. {args}") + if len(args) < 2: + self._logger.error("Invalid number of arguments: ", args) + options["result_function"]( + "1", execution_status_details="Invalid number of arguments" + ) + return + script_name = args[0] + script_args = args[1] + # TODO (Elvio): Needs to be re designed. + # 1. script_name is not standarized at all + # 2. Consider implementing a callback for handling mission specific commands + # 3. Needs an interface for supporting mission related actions + if script_name == "queue_mission" and script_args[0] == "--mission_id": + self.mission_tracking.mir_mission_tracking_enabled = ( + self._robot_session.missions_module.executor.wait_until_idle(0) + ) + self.mir_api.queue_mission(script_args[1]) + elif script_name == "run_mission_now" and script_args[0] == "--mission_id": + self.mission_tracking.mir_mission_tracking_enabled = ( + self._robot_session.missions_module.executor.wait_until_idle(0) + ) + self.mir_api.abort_all_missions() + self.mir_api.queue_mission(script_args[1]) + elif script_name == "abort_missions": + self._robot_session.missions_module.executor.cancel_mission("*") + self.mir_api.abort_all_missions() + elif script_name == "set_state": + if script_args[0] == "--state_id": + state_id = script_args[1] + if not state_id.isdigit() or int(state_id) not in MIR_STATE.keys(): + error = f"Invalid `state_id` '{state_id}'" + self._logger.error(error) + options["result_function"]("1", execution_status_details=error) return + state_id = int(state_id) + self._logger.info( + f"Setting robot state to state {state_id}: {MIR_STATE[state_id]}" + ) + self.mir_api.set_state(state_id) + if script_args[0] == "--clear_error": + self._logger.info("Clearing error state") + self.mir_api.clear_error() + elif script_name == "set_waiting_for" and script_args[0] == "--text": + self._logger.info(f"Setting 'waiting for' value to {script_args[1]}") + self.mission_tracking.waiting_for_text = script_args[1] + elif script_name == "localize": + # The localize command sets the robot's position and current map + # The expected arguments are "x" and "y" in meters and "orientation" in degrees, + # as in MiR Fleet, and "map_id" as the target map in MiR Fleet, which matches + # the uploaded "frame_id" in InOrbit + if ( + len(script_args) == 8 + and script_args[0] == "--x" + and script_args[2] == "--y" + and script_args[4] == "--orientation" + and script_args[6] == "--map_id" + ): + status = { + "position": { + "x": float(script_args[1]), + "y": float(script_args[3]), + "orientation": float(script_args[5]), + }, + "map_id": script_args[7], + } + self._logger.info(f"Changing map to {script_args[7]}") + self.mir_api.set_status(status) else: - # Other kind if custom commands may be handled by the edge-sdk - # (e.g.user_scripts) and not by the connector code itself - # Do not return any result and leave it to the edge-sdk to handle it + self._logger.error("Invalid arguments for 'localize' command") + options["result_function"]( + "1", execution_status_details="Invalid arguments" + ) return - # Return '0' for success - options["result_function"]("0") - elif command_name == COMMAND_NAV_GOAL: - self._logger.info(f"Received '{command_name}'!. {args}") - pose = args[0] - self.send_waypoint_over_missions(pose) - elif command_name == COMMAND_MESSAGE: - msg = args[0] - if msg == "inorbit_pause": - self.mir_api.set_state(4) - elif msg == "inorbit_resume": - self.mir_api.set_state(3) else: - self._logger.info(f"Received unknown command '{command_name}'!. {args}") - except Exception as ex: - self._logger.error( - f"Failed to execute command '{command_name}' with args {args}. Exception:\n{ex}" - ) - options["result_function"]( - "1", - execution_status_details="An error occured executing custom command", - stderr=str(ex), - ) - return + # Other kind if custom commands may be handled by the edge-sdk + # (e.g.user_scripts) and not by the connector code itself + # Do not return any result and leave it to the edge-sdk to handle it + return + # Return '0' for success + options["result_function"]("0") + elif command_name == COMMAND_NAV_GOAL: + self._logger.info(f"Received '{command_name}'!. {args}") + pose = args[0] + self.send_waypoint_over_missions(pose) + elif command_name == COMMAND_MESSAGE: + msg = args[0] + if msg == "inorbit_pause": + self.mir_api.set_state(4) + elif msg == "inorbit_resume": + self.mir_api.set_state(3) + else: + self._logger.info(f"Received unknown command '{command_name}'!. {args}") def _connect(self) -> None: """Connect to the robot services and to InOrbit""" From cb4fbc1c727a2cfd65b3f7070433323d674b571e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tom=C3=A1s=20Badenes?= Date: Tue, 26 Nov 2024 13:42:16 -0300 Subject: [PATCH 4/4] Commets --- .../inorbit_mir_connector/src/connector.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/mir_connector/inorbit_mir_connector/src/connector.py b/mir_connector/inorbit_mir_connector/src/connector.py index aa1892b..8e91a31 100644 --- a/mir_connector/inorbit_mir_connector/src/connector.py +++ b/mir_connector/inorbit_mir_connector/src/connector.py @@ -162,9 +162,9 @@ def _inorbit_command_handler(self, command_name, args, options): self.mission_tracking.waiting_for_text = script_args[1] elif script_name == "localize": # The localize command sets the robot's position and current map - # The expected arguments are "x" and "y" in meters and "orientation" in degrees, - # as in MiR Fleet, and "map_id" as the target map in MiR Fleet, which matches - # the uploaded "frame_id" in InOrbit + # The expected arguments are "x" and "y" in meters and "orientation" in degrees, as + # in MiR Fleet, and "map_id" as the target map in MiR Fleet, which matches the + # uploaded "frame_id" in InOrbit if ( len(script_args) == 8 and script_args[0] == "--x" @@ -184,13 +184,11 @@ def _inorbit_command_handler(self, command_name, args, options): self.mir_api.set_status(status) else: self._logger.error("Invalid arguments for 'localize' command") - options["result_function"]( - "1", execution_status_details="Invalid arguments" - ) + options["result_function"]("1", execution_status_details="Invalid arguments") return else: - # Other kind if custom commands may be handled by the edge-sdk - # (e.g.user_scripts) and not by the connector code itself + # Other kind if custom commands may be handled by the edge-sdk (e.g. user_scripts) + # and not by the connector code itself # Do not return any result and leave it to the edge-sdk to handle it return # Return '0' for success