优化代码

src/Safe_navigation/run_main.py

@@ -15,6 +15,7 @@
 from collections import deque
 
 
+
 class AirSimNHCarSimulator:
     """AirSim无人车仿真主类"""
 
@@ -37,6 +38,7 @@ def __init__(self, ip="127.0.0.1", port=41451, vehicle_name="PhysXCar"):
         self.data_log = []
         self.data_file = None
 
+
         # 传感器数据缓存
         self.sensor_data = {
             "camera": deque(maxlen=100),
@@ -72,6 +74,7 @@ def connect(self):
 
             self.is_connected = True
             print("✓ 成功连接到AirSim仿真器！")
+
             return True
 
         except Exception as e:
@@ -82,6 +85,7 @@ def connect(self):
             print("3. 网络连接正常")
             return False
 
+
     def enable_api_control(self, enable=True):
         """启用/禁用API控制"""
         try:
@@ -114,15 +118,12 @@ def get_vehicle_state(self):
             # 获取车辆物理信息
             kinematics = self.client.simGetVehiclePose(vehicle_name=self.vehicle_name)
 
+
             state_info = {
                 "timestamp": time.time(),
                 "speed_kmh": state.speed,
                 "speed_ms": state.speed / 3.6,
-                "position": {
-                    "x": kinematics.position.x_val,
-                    "y": kinematics.position.y_val,
-                    "z": kinematics.position.z_val
-                },
+
                 "orientation": {
                     "w": kinematics.orientation.w_val,
                     "x": kinematics.orientation.x_val,
@@ -240,9 +241,7 @@ def get_gps_data(self):
             print(f"获取GPS数据失败: {e}")
             return None
 
-    def manual_control_demo(self, duration=10):
-        """
-        手动控制演示：前进、转向、停止
+
 
         参数:
             duration: 演示总时长（秒）
@@ -252,68 +251,16 @@ def manual_control_demo(self, duration=10):
             return False
 
         print(f"\n开始手动控制演示 ({duration}秒)...")
-        print("操作序列: 加速 → 左转 → 右转 → 刹车停止")
 
         start_time = time.time()
         sequence = 0
 
+
         try:
             while time.time() - start_time < duration:
                 elapsed = time.time() - start_time
 
-                # 根据时间执行不同控制序列
-                if elapsed < duration * 0.25:  # 第一阶段：直线加速
-                    controls = airsim.CarControls()
-                    controls.throttle = 0.7
-                    controls.steering = 0.0
-                    self.client.setCarControls(controls, vehicle_name=self.vehicle_name)
-                    if sequence < 1:
-                        print("  阶段1: 直线加速")
-                        sequence = 1
-
-                elif elapsed < duration * 0.5:  # 第二阶段：左转
-                    controls = airsim.CarControls()
-                    controls.throttle = 0.5
-                    controls.steering = 0.3  # 左转
-                    self.client.setCarControls(controls, vehicle_name=self.vehicle_name)
-                    if sequence < 2:
-                        print("  阶段2: 左转")
-                        sequence = 2
-
-                elif elapsed < duration * 0.75:  # 第三阶段：右转
-                    controls = airsim.CarControls()
-                    controls.throttle = 0.5
-                    controls.steering = -0.3  # 右转
-                    self.client.setCarControls(controls, vehicle_name=self.vehicle_name)
-                    if sequence < 3:
-                        print("  阶段3: 右转")
-                        sequence = 3
-
-                else:  # 第四阶段：减速停止
-                    controls = airsim.CarControls()
-                    controls.throttle = 0.0
-                    controls.brake = 1.0
-                    controls.steering = 0.0
-                    self.client.setCarControls(controls, vehicle_name=self.vehicle_name)
-                    if sequence < 4:
-                        print("  阶段4: 刹车停止")
-                        sequence = 4
-
-                # 实时显示车辆状态
-                state = self.get_vehicle_state()
-                if state:
-                    print(f"\r速度: {state['speed_kmh']:.1f} km/h | "
-                          f"位置: ({state['position']['x']:.1f}, "
-                          f"{state['position']['y']:.1f})", end="")
-
-                # 采集传感器数据
-                self.capture_camera_images(["front"])  # 只采集前摄像头
-                self.get_imu_data()
-                self.get_gps_data()
-
-                time.sleep(0.1)  # 控制频率 10Hz
-
-            print("\n✓ 手动控制演示完成")
+
             return True
 
         except Exception as e:
@@ -382,13 +329,14 @@ def save_simulation_data(self):
                 "camera_frames": len(self.sensor_data["camera"]),
                 "imu_samples": len(self.sensor_data["imu"]),
                 "gps_samples": len(self.sensor_data["gps"]),
-                "total_log_entries": len(self.data_log)
+
             }
 
             stats_file = f"{self.data_dir}/simulation_stats.json"
             with open(stats_file, 'w') as f:
                 json.dump(stats, f, indent=2)
 
+
             # 生成数据报告
             report_file = f"{self.data_dir}/report.txt"
             with open(report_file, 'w') as f:
@@ -400,7 +348,7 @@ def save_simulation_data(self):
                 f.write(f"摄像头帧数: {stats['camera_frames']}\n")
                 f.write(f"IMU采样数: {stats['imu_samples']}\n")
                 f.write(f"GPS采样数: {stats['gps_samples']}\n")
-                f.write(f"日志条目: {stats['total_log_entries']}\n\n")
+
                 f.write("数据文件:\n")
                 for file in os.listdir(self.data_dir):
                     f.write(f"  - {file}\n")
@@ -410,13 +358,14 @@ def save_simulation_data(self):
             print(f"  统计数据: {stats_file}")
             print(f"  报告文件: {report_file}")
 
+
             return True
 
         except Exception as e:
             print(f"✗ 保存数据失败: {e}")
             return False
 
-    def run_full_demo(self, control_duration=15, data_duration=8):
+
         """运行完整演示"""
         print("=" * 60)
         print("AirSimNH 无人车完整仿真演示")
@@ -431,6 +380,7 @@ def run_full_demo(self, control_duration=15, data_duration=8):
             if not self.enable_api_control(True):
                 return False
 
+
             # 步骤3: 手动控制演示
             if not self.manual_control_demo(control_duration):
                 print("手动控制演示失败，继续其他演示...")
@@ -458,10 +408,7 @@ def cleanup(self):
         # 停止车辆
         if self.is_api_control_enabled:
             controls = airsim.CarControls()
-            controls.brake = 1.0
-            controls.handbrake = True
-            try:
-                self.client.setCarControls(controls, vehicle_name=self.vehicle_name)
+
             except:
                 pass
 
@@ -486,7 +433,7 @@ def main():
     # 运行完整演示
     try:
         simulator.run_full_demo(
-            control_duration=20,  # 控制演示时长（秒）
+
             data_duration=10  # 数据采集时长（秒）
         )