feat: 添加A/B任务点可视化标记

- 在Environment类添加task_marker_ids属性管理标记ID
- 实现create_task_markers()方法创建A点(绿色)和B点(红色)球体标记
- 实现_clear_task_markers()方法清理旧标记
- 在setup_environment()中自动创建标记
- 在cleanup()中清理标记避免重复创建问题
- 标记使用极小碰撞体积(0.005)避免干扰仿真

🤖 Generated with Claude Code

Co-Authored-By: Claude <noreply@anthropic.com>

src/gui/main_window.py

@@ -256,7 +256,14 @@ class MainWindow:
             # Clear current environment
             if self.environment:
                 self.environment.cleanup()
-            # Note: Robot cleanup is handled by ArmController internally
+            
+            # Clear current robot
+            if self.arm_controller and hasattr(self.arm_controller, 'robot_loader') and self.arm_controller.robot_loader:
+                if self.arm_controller.robot_loader.robot_id is not None:
+                    try:
+                        p.removeBody(self.arm_controller.robot_loader.robot_id, physicsClientId=self.physics_client)
+                    except:
+                        pass  # Robot might already be removed
             
             # Create new ConfigLoader with updated configuration
             self.config_loader = ConfigLoader()

src/robot/robot_loader.py

@@ -26,6 +26,14 @@ class RobotLoader:
         
     def load_robot(self) -> int:
         """Load robot URDF model into pybullet simulation"""
+        # Clear any existing robot
+        if self.robot_id is not None:
+            try:
+                p.removeBody(self.robot_id, physicsClientId=self.physics_client)
+            except:
+                pass  # Robot might already be removed
+            self.robot_id = None
+        
         model_path = self.robot_config['model_path']
         
         if not os.path.exists(model_path):

src/simulation/environment.py

@@ -20,6 +20,7 @@ class Environment:
         self.transport_object_id: Optional[int] = None
         self.ground_plane_id: Optional[int] = None
         self._wall_part_ids: List[int] = []  # 记录所有墙体部件ID
+        self.task_marker_ids: Dict[str, int] = {}  # 存储A/B点标记ID
         
     def setup_simulation(self) -> None:
         """Setup basic simulation environment"""
@@ -60,6 +61,15 @@ class Environment:
         self._wall_part_ids = []
         self.wall_id = None
     
+    def _clear_task_markers(self) -> None:
+        """Clear existing task markers"""
+        for marker_id in self.task_marker_ids.values():
+            try:
+                p.removeBody(marker_id, physicsClientId=self.physics_client)
+            except:
+                pass  # Marker might already be removed
+        self.task_marker_ids.clear()
+    
     def create_wall_with_hole(self) -> List[int]:
         """Create wall with hole using:
            - Top/Bottom: full wall width
@@ -258,10 +268,14 @@ class Environment:
         # Create transport object
         transport_obj_id = self.create_transport_object()
         
+        # Create task point markers
+        task_markers = self.create_task_markers()
+        
         return {
             'ground_plane_id': ground_id,
             'wall_parts': wall_parts,
-            'transport_object_id': transport_obj_id
+            'transport_object_id': transport_obj_id,
+            'task_markers': task_markers
         }
     
     def get_transport_object_pose(self) -> Tuple[List[float], List[float]]:
@@ -335,6 +349,60 @@ class Environment:
             'dimensions': self.wall_config['dimensions']
         }
     
+    def create_task_markers(self) -> Dict[str, int]:
+        """Create visual markers for task points A and B"""
+        # Clear existing markers
+        self._clear_task_markers()
+        
+        # Get task points from config
+        task_points = self.config_loader.get_task_points()
+        point_a_pos = task_points['point_A']['position']
+        point_b_pos = task_points['point_B']['position']
+        
+        # Create point A marker (green sphere)
+        a_visual = p.createVisualShape(
+            p.GEOM_SPHERE,
+            radius=0.05,
+            rgbaColor=[0.0, 1.0, 0.0, 0.8],  # Semi-transparent green
+            physicsClientId=self.physics_client
+        )
+        a_collision = p.createCollisionShape(
+            p.GEOM_SPHERE,
+            radius=0.005,  # Very small collision volume to avoid interference
+            physicsClientId=self.physics_client
+        )
+        a_id = p.createMultiBody(
+            baseMass=0,  # Static object
+            baseCollisionShapeIndex=a_collision,
+            baseVisualShapeIndex=a_visual,
+            basePosition=point_a_pos,
+            physicsClientId=self.physics_client
+        )
+        self.task_marker_ids['point_A'] = a_id
+        
+        # Create point B marker (red sphere)
+        b_visual = p.createVisualShape(
+            p.GEOM_SPHERE,
+            radius=0.05,
+            rgbaColor=[1.0, 0.0, 0.0, 0.8],  # Semi-transparent red
+            physicsClientId=self.physics_client
+        )
+        b_collision = p.createCollisionShape(
+            p.GEOM_SPHERE,
+            radius=0.005,
+            physicsClientId=self.physics_client
+        )
+        b_id = p.createMultiBody(
+            baseMass=0,
+            baseCollisionShapeIndex=b_collision,
+            baseVisualShapeIndex=b_visual,
+            basePosition=point_b_pos,
+            physicsClientId=self.physics_client
+        )
+        self.task_marker_ids['point_B'] = b_id
+        
+        return self.task_marker_ids
+    
     def reset_environment(self) -> None:
         """Reset environment to initial state"""
         if self.transport_object_id is not None:
@@ -344,7 +412,10 @@ class Environment:
     
     def cleanup(self) -> None:
         """Clean up environment objects"""
-        # Clean up wall parts first
+        # Clean up task markers first
+        self._clear_task_markers()
+        
+        # Clean up wall parts
         self._clear_wall()
         
         objects_to_remove = []