diff --git a/config/config.yaml b/config/config.yaml index af9c2d9..d409a81 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,2 +1,3 @@ method_mapping: dock: return_home + change_map: switch_map diff --git a/scripts/connect_openrmf_by_zenoh.py b/scripts/connect_openrmf_by_zenoh.py index a479dc0..38fc741 100644 --- a/scripts/connect_openrmf_by_zenoh.py +++ b/scripts/connect_openrmf_by_zenoh.py @@ -115,6 +115,17 @@ async def publish_map_name(self) -> None: map_name = next((item["name"] for item in map_list if item["id"] == search_id), "L1") self.map_name_pub.put(json.dumps(map_name).encode(), encoding=zenoh.Encoding.APP_JSON()) + async def switch_map(self, args: dict) -> None: + """Switch the robot to the specified map. + Args: + args (dict): The arguments for the switch_map method. + """ + map_list = await self.run_method("get_map_list") + map_id = next((item["id"] for item in map_list if item["name"] == args.get('map_name'))) + if map_id is not None: + payload = {"map_id": map_id, "pose": args.get("pose", {"x": 0.0, "y": 0.0, "theta": 0.0})} + await self.run_method("switch_map", payload) + async def publish_result(self) -> None: """Publish the last command is_completed to Zenoh.""" res = await self.run_method("get_command_state") @@ -166,6 +177,8 @@ def _command_callback(self, sample: Sample) -> None: if not hasattr(self.kachaka_client, method_name): raise AttributeError(f"Invalid method: {method_name}") print(f"Received command: {command}") + if method_name == "switch_map": + asyncio.run(self.switch_map(command['args'])) asyncio.run(self.run_method(method_name, command['args'])) except (json.JSONDecodeError, ValueError, AttributeError) as e: print(f"Invalid command: {str(e)}")