From 2f07dc9f7a014892023e439eb207ff2e28764559 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Tue, 30 Jun 2026 16:03:44 +0200 Subject: [PATCH 1/3] Catch moveit errors in housekeep tool MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/agents/tools.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rai_app/agents/tools.py b/rai_app/agents/tools.py index 7de1886..d379f1d 100644 --- a/rai_app/agents/tools.py +++ b/rai_app/agents/tools.py @@ -887,6 +887,8 @@ def housekeep(self, rack: str): pos, collection_name=rack, approach_distance=self.approach_distance ) except RuntimeError as e: + if "arm" in str(e): # don't count moveit related exceptions as "side not available" + raise e sides_not_available += 1 logging.warning(e) except Exception as e: From afff37b9e922d8ac8c35fea78d6ed92df7bd199f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Tue, 30 Jun 2026 16:04:20 +0200 Subject: [PATCH 2/3] Reset simulation on housekeep fail MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/environment/scene_manager.py | 9 +++++++++ tests/test_housekeep.py | 1 + 2 files changed, 10 insertions(+) diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index e7c5dda..ce87a28 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -317,6 +317,15 @@ def clear_scene(self): timeout_sec=3.0, ) + def reset_simulation(self): + wait_for_ros2_services(self.connector, ["/reset_simulation"]) + self.connector.call_service( + ROS2Message(payload={}), + target="/reset_simulation", + msg_type="simulation_interfaces/srv/ResetSimulation", + timeout_sec=10.0 + ) + def move_entity( self, entity_name, diff --git a/tests/test_housekeep.py b/tests/test_housekeep.py index 7a68782..016addf 100644 --- a/tests/test_housekeep.py +++ b/tests/test_housekeep.py @@ -79,4 +79,5 @@ def test_single_box_housekeeping( result = housekeep_tool._run(rack) assert "successfully" in result, f"HouseKeepTool failed: {result}" except Exception as e: + scene_manager.reset_simulation() pytest.fail(f"HouseKeepTool failed: {e}") From 5ac003163451b88c5b5c94d90249198078dadb02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Tue, 30 Jun 2026 16:12:09 +0200 Subject: [PATCH 3/3] pre-commit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- rai_app/agents/tools.py | 4 +++- rai_app/environment/scene_manager.py | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/rai_app/agents/tools.py b/rai_app/agents/tools.py index d379f1d..e5d5466 100644 --- a/rai_app/agents/tools.py +++ b/rai_app/agents/tools.py @@ -887,7 +887,9 @@ def housekeep(self, rack: str): pos, collection_name=rack, approach_distance=self.approach_distance ) except RuntimeError as e: - if "arm" in str(e): # don't count moveit related exceptions as "side not available" + if "arm" in str( + e + ): # don't count moveit related exceptions as "side not available" raise e sides_not_available += 1 logging.warning(e) diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index ce87a28..f6771cc 100644 --- a/rai_app/environment/scene_manager.py +++ b/rai_app/environment/scene_manager.py @@ -323,7 +323,7 @@ def reset_simulation(self): ROS2Message(payload={}), target="/reset_simulation", msg_type="simulation_interfaces/srv/ResetSimulation", - timeout_sec=10.0 + timeout_sec=10.0, ) def move_entity(