diff --git a/rai_app/agents/tools.py b/rai_app/agents/tools.py index 7de1886..e5d5466 100644 --- a/rai_app/agents/tools.py +++ b/rai_app/agents/tools.py @@ -887,6 +887,10 @@ 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: diff --git a/rai_app/environment/scene_manager.py b/rai_app/environment/scene_manager.py index e7c5dda..f6771cc 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}")