Merge branch 'development' into kd/batch_spawning

Author: knicked

pixi.toml

@@ -358,7 +358,7 @@ depends-on = ["sync"]
 description = "Run all linters and formatters"
 
 [feature.dev.tasks.test]
-cmd = "uv run pytest"
+cmd = "PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 uv run python -m pytest"
 depends-on = ["sync"]
 description = "Run test suite"
 

rai_app/agents/moveit2_agent.py

@@ -468,11 +468,16 @@ def move_arm(self, request: MoveArm.Request, response: MoveArm.Response):
                 )
             except Exception as e:
                 response.success = False
-                llm_response = self.llm.invoke(
-                    "The arm failed to move to the position. Check the logs and provide a short summary."
-                    + str(e)
-                )
-                response.report = llm_response.content
+                try:
+                    llm_response = self.llm.invoke(
+                        self.formulate_prompt(
+                            "The arm failed to move to the position. Check the logs and provide a short summary.",
+                            str(e),
+                        )
+                    )
+                    response.report = llm_response.content
+                except Exception:
+                    response.report = f"The arm failed to move to the position: {e}"
                 return response
 
         if request.gripper_state == 0:
@@ -486,10 +491,15 @@ def move_arm(self, request: MoveArm.Request, response: MoveArm.Response):
             except Exception as e:
                 self.logger.error(f"Failed to open gripper: {e}")
                 response.success = False
-                llm_response = self.llm.invoke(
-                    self.formulate_prompt("The arm failed to open the gripper.", str(e))
-                )
-                response.report = llm_response.content
+                try:
+                    llm_response = self.llm.invoke(
+                        self.formulate_prompt(
+                            "The arm failed to open the gripper.", str(e)
+                        )
+                    )
+                    response.report = llm_response.content
+                except Exception:
+                    response.report = f"The arm failed to open the gripper: {e}"
                 return response
         elif request.gripper_state == 2:
             try:
@@ -499,12 +509,15 @@ def move_arm(self, request: MoveArm.Request, response: MoveArm.Response):
             except Exception as e:
                 self.logger.error(f"Failed to close gripper: {e}")
                 response.success = False
-                llm_response = self.llm.invoke(
-                    self.formulate_prompt(
-                        "The arm failed to close the gripper.", str(e)
+                try:
+                    llm_response = self.llm.invoke(
+                        self.formulate_prompt(
+                            "The arm failed to close the gripper.", str(e)
+                        )
                     )
-                )
-                response.report = llm_response.content
+                    response.report = llm_response.content
+                except Exception:
+                    response.report = f"The arm failed to open the gripper: {e}"
                 return response
         else:
             response.success = False

rai_app/agents/nav2_agent.py

@@ -158,15 +158,20 @@ def navigate_to_pose(self, goal_handle: ServerGoalHandle) -> str:
         elif result == TaskResult.FAILED:
             reason = self.navigator.result_future.result().result.error_code
             enum_name = decode_error_code(reason, Nav2NavigateToPose)
-            response = self.llm.invoke(
-                self.formulate_prompt(
-                    f"Navigate to pose has failed with error code {enum_name}. Check the logs and provide a short summary.",
-                    str(feedback),
-                    request.pose,
-                )
-            )
             action_result.success = False
-            action_result.report = response.content
+            try:
+                response = self.llm.invoke(
+                    self.formulate_prompt(
+                        f"Navigate to pose has failed with error code {enum_name}. Check the logs and provide a short summary.",
+                        str(feedback),
+                        request.pose,
+                    )
+                )
+                action_result.report = response.content
+            except Exception:
+                action_result.report = (
+                    f"Navigate to pose has failed with error code {enum_name}"
+                )
             return action_result
         else:
             action_result.success = False
@@ -213,15 +218,20 @@ def drive_on_heading(self, goal_handle: ServerGoalHandle):
             goal_handle.abort()
             reason = self.navigator.result_future.result().result.error_code
             enum_name = decode_error_code(reason, Nav2DriveOnHeading)
-            response = self.llm.invoke(
-                self.formulate_prompt(
-                    f"Drive on heading has failed with error code {enum_name}. Check the logs and provide a short summary.",
-                    str(feedback),
-                    params,
-                )
-            )
             action_result.success = False
-            action_result.report = response.content
+            try:
+                response = self.llm.invoke(
+                    self.formulate_prompt(
+                        f"Drive on heading has failed with error code {enum_name}. Check the logs and provide a short summary.",
+                        str(feedback),
+                        params,
+                    )
+                )
+                action_result.report = response.content
+            except Exception:
+                action_result.report = (
+                    f"Drive on heading has failed with error code {enum_name}"
+                )
             return action_result
         else:
             action_result.success = False
@@ -273,15 +283,18 @@ def spin(self, goal_handle: ServerGoalHandle):
             enum_name = decode_error_code(reason, Nav2Spin)
 
             goal_handle.abort()
-            response = self.llm.invoke(
-                self.formulate_prompt(
-                    f"Turn (requested angle: {request.target_yaw} radians) has failed with error code {enum_name}. Check the logs and provide a short summary.",
-                    str(feedback),
-                    params,
-                )
-            )
             action_result.success = False
-            action_result.report = response.content
+            try:
+                response = self.llm.invoke(
+                    self.formulate_prompt(
+                        f"Turn (requested angle: {request.target_yaw} radians) has failed with error code {enum_name}. Check the logs and provide a short summary.",
+                        str(feedback),
+                        params,
+                    )
+                )
+                action_result.report = response.content
+            except Exception:
+                action_result.report = f"Turn (requested angle: {request.target_yaw} radians) has failed with error code {enum_name}."
             return action_result
         else:
             action_result.success = False
@@ -317,15 +330,20 @@ def follow_waypoints(self, goal_handle: ServerGoalHandle):
             enum_name = decode_error_code(reason, Nav2Spin)
 
             goal_handle.abort()
-            response = self.llm.invoke(
-                self.formulate_prompt(
-                    f"Followed waypoints has failed with error code {enum_name}. Check the logs and provide a short summary.",
-                    str(feedback),
-                    request.poses,
-                )
-            )
             action_result.success = False
-            action_result.report = response.content
+            try:
+                response = self.llm.invoke(
+                    self.formulate_prompt(
+                        f"Followed waypoints has failed with error code {enum_name}. Check the logs and provide a short summary.",
+                        str(feedback),
+                        request.poses,
+                    )
+                )
+                action_result.report = response.content
+            except Exception:
+                action_result.report = (
+                    f"Followed waypoints has failed with error code {enum_name}."
+                )
             return action_result
         else:
             action_result.success = False

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:

rai_app/environment/scene_manager.py

@@ -361,6 +361,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,

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}")