add position tracking in wait_until_idle; update tests for new position handling

robot-control/src/api/grbl_handler.py

@@ -67,24 +67,48 @@ class GRBLHandler:
         finally:
             self.controller_active.clear()
 
-    async def wait_until_idle(self, timeout_s):
-        """Wait until GRBL reports idle status"""
+    async def get_status(self):
+        """Get current GRBL status
+        
+        Returns:
+            str: Status response from GRBL, or None if in debug mode
+        """
+        if self.debug:
+            return None
+            
+        if self.writer:
+            self.writer.write(b"?\n")
+            await self.writer.drain()
+            # '?' command returns status report and 'ok'
+            response = await self._process_response()
+            response = response + await self._process_response()
+            return response
+        return None
+
+    async def wait_until_idle(self, timeout_s, position: list[float] = None):
+        """Wait until GRBL reports idle status
+        
+        Args:
+            timeout_s: Timeout in seconds
+            position: Optional list of 3 floats that will be updated with current position
+        """
         if self.debug:
             await asyncio.sleep(1)
             return
             
         start = datetime.datetime.now()
         while True:
-            if self.writer:
-                self.writer.write(b"?\n")
-                await self.writer.drain()
-                # '?' command returns status report and 'ok'
-                response = await self._process_response()
-                response = response + await self._process_response()
-                
-                if response and "Idle" in response:
-                    logger.debug("Movement complete.\nContinuing...")
-                    break
+            response = await self.get_status()
+            if response and 'MPos:' in response:
+                # Parse position from status reports (<Idle|MPos:0.000,0.000,0.000|...>)
+                pos_str = response.split('MPos:')[1].split('|')[0]
+                if position is not None:
+                    x, y, z = map(float, pos_str.split(','))
+                    position = (x,y,z)
+
+            if response and "Idle" in response:
+                logger.debug("Movement complete.\nContinuing...")
+                break
                     
             now = datetime.datetime.now()
             if (now - start).total_seconds() > timeout_s:
@@ -93,11 +117,11 @@ class GRBLHandler:
                 
             await asyncio.sleep(0.2)  # Async delay to prevent flooding
 
-    async def send_and_wait_gcode(self, commands: List[str], timeout_s=60):
+    async def send_and_wait_gcode(self, commands: List[str], timeout_s=60, position: list[float] = None):
         """Send GCODE commands and wait until machine is idle"""
         await self.send_gcode(commands)
         await asyncio.sleep(0.2)  # Delay to allow GRBL to process commands
-        await self.wait_until_idle(timeout_s)
+        await self.wait_until_idle(timeout_s, position)
 
     async def _process_response(self):
         """Process GRBL responses"""

robot-control/src/robot/movement.py

@@ -35,14 +35,17 @@ class RobotMovement:
             self.is_moving = True
             logger.debug(f"Moving to position X:{x:.3f} Y:{y:.3f} Z:{z:.3f}")
             # Send G-code move command
-            await self.grbl.send_and_wait_gcode([
-                f'G1 X{x:.3f} Y{y:.3f} Z{z:.3f} F{self.speed * 60}'
-            ])
-            self.is_moving = False
+            await self.grbl.send_and_wait_gcode(
+                [f'G1 X{x:.3f} Y{y:.3f} Z{z:.3f} F{self.speed * 60}'],
+                position=self.current_position
+            )
+            if self.current_position != (x, y, z):
+                raise RuntimeError(f"Movement failed: Not arrived at the target position {x}, {y}, {z}!")
             return True
-        except Exception as e:
-            self.is_moving = False
+        except TimeoutError as e:
             raise RuntimeError(f"Movement failed: {str(e)}")
+        finally:
+            self.is_moving = False
 
     async def move_relative(self, dx: float, dy: float, dz: float) -> bool:
         logger.debug(f"Moving relative by dX:{dx:.3f} dY:{dy:.3f} dZ:{dz:.3f}")
@@ -55,7 +58,7 @@ class RobotMovement:
         self.is_moving = True
         try:
             logger.info("Homing machine...")
-            await self.grbl.send_and_wait_gcode(['$H'])  # GRBL home command
+            await self.grbl.send_and_wait_gcode(['$H'], position=self.current_position)
             self.is_moving = False
             return True
         except Exception as e:

robot-control/tests/test_robot.py

@@ -27,20 +27,24 @@ class TestRobotMovement:
 
     @pytest.mark.asyncio
     async def test_set_speed(self, robot_movement: RobotMovement):
+        await robot_movement.grbl.connect()
         await robot_movement.set_speed(200.0)
         assert robot_movement.speed == 200.0
 
         with pytest.raises(ValueError):
             await robot_movement.set_speed(-100.0)
 
+    # TODO [SG]: Getting current position currently does not work in debug mode
     @pytest.mark.asyncio
     async def test_move_to_position(self, robot_movement: RobotMovement):
-        result = await robot_movement.move_to_position(100.0, 200.0, 300.0)
+        await robot_movement.grbl.connect()
+        result = await robot_movement.move_to_position(10.0, 20.0, 30.0)
         assert result is True
-        assert robot_movement.get_position() == (100.0, 200.0, 300.0)
+        assert robot_movement.get_position() == (10.0, 20.0, 30.0)
 
     @pytest.mark.asyncio
     async def test_move_relative(self, robot_movement: RobotMovement):
+        await robot_movement.grbl.connect()
         await robot_movement.move_to_position(100.0, 100.0, 100.0)
         assert await robot_movement.move_relative(50.0, -50.0, 25.0)
         assert robot_movement.get_position() == (150.0, 50.0, 125.0)