下面代码中的碰撞检测的方式是否正确？

[Bziyue]

我在里程计的回调函数中添加了如下的碰撞检测代码，可是统计出的数据显示存在严重的与障碍物和其他无人机的碰撞。
由于无人机的半径为0.15，所以安全间隙应该大于等于0.3；地图分辨率为0.1，那么无人机距离点云的距离应该大于sqrt(3)/2 * 0.1 + 0.15 = 0.2366;(我看到在swarm-path.py代码中也是以相同逻辑生成的碰撞检测的查询距离)。
`void PPReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg)
{
odom_pos_(0) = msg->pose.pose.position.x;
odom_pos_(1) = msg->pose.pose.position.y;
odom_pos_(2) = msg->pose.pose.position.z;

odom_vel_(0) = msg->twist.twist.linear.x;
odom_vel_(1) = msg->twist.twist.linear.y;
odom_vel_(2) = msg->twist.twist.linear.z;

odom_q_.x() = msg->pose.pose.orientation.x;
odom_q_.y() = msg->pose.pose.orientation.y;
odom_q_.z() = msg->pose.pose.orientation.z;
odom_q_.w() = msg->pose.pose.orientation.w;

odom_x_dir_ = odom_q_.toRotationMatrix().col(0);
odom_yaw_ = atan2(odom_x_dir_(1), odom_x_dir_(0));
// std::cout << "odom_yaw_=" << odom_yaw_ <<std::endl;

have_odom_ = true;

// Check for real-time collisions with other drones and update minimum distance
double t_now = ros::Time::now().toSec();
const double SWARM_COLLISION_DIST = planner_manager_->swarm_clearence_; // 参数设定为0.3 无人机半径0.15 advanced_param.xml

// Reset current minimum distance to other drones for this update
double current_min_drone_dist = std::numeric_limits<double>::max();

for (size_t i = 0; i < planner_manager_->swarm_traj.size(); i++) {
  // Skip ourselves and invalid trajectories
  if ((int)i == planner_manager_->drone_id || planner_manager_->swarm_traj[i].drone_id < 0) {
    continue;
  }

  // Check if the other drone's trajectory is active
  double other_traj_start_time = planner_manager_->swarm_traj[i].start_time;
  double other_traj_duration = planner_manager_->swarm_traj[i].traj_duration;
  
  // If the trajectory is active and valid
  if (t_now >= other_traj_start_time && t_now <= other_traj_start_time + other_traj_duration) {
    // Calculate other drone's position at current time
    double t_relative = t_now - other_traj_start_time;
    int pos_idx = round(t_relative * 100); // 10ms resolution
    
    // Ensure index is within bounds
    if (pos_idx >= 0 && pos_idx < planner_manager_->swarm_traj[i].traj_pos.size()) {
      Eigen::Vector3d other_drone_pos = planner_manager_->swarm_traj[i].traj_pos[pos_idx];
      double dist = (odom_pos_ - other_drone_pos).norm();
      
      // Update minimum distance to other drones
      current_min_drone_dist = std::min(current_min_drone_dist, dist);
      
      // Check for collision
      if (dist < SWARM_COLLISION_DIST) { //agent_inflate_radius = math.sqrt(3) * voxelSize / 2.0 + 2 * drone_radius 详见swarm_path.py 
        if (have_log_files_) { // 实际算出来为0.3866，这里以无人机半径为0.15，那么安全距离为0.3
          collision_detected_ = true;
          collision_reason_ = "Real-time swarm collision with drone " + std::to_string(i);
          has_collision_with_drone_ = true;
          ROS_ERROR("Real-time collision detected with drone %zu! Distance: %.3f", i, dist);
          // Could trigger emergency stop or other safety measures here if needed
        }
        break;
      }
    }
  }
}

// Update global minimum drone distance if we found a valid distance in this update
if (current_min_drone_dist < std::numeric_limits<double>::max()) {
  min_dist_to_other_drone_ = std::min(min_dist_to_other_drone_, current_min_drone_dist);
}

// Track distance traveled for flight statistics
if (have_log_files_) {
    double segment_distance = (odom_pos_ - last_position_).norm();
    total_distance_ += segment_distance;
    last_position_ = odom_pos_;
}

static Eigen::Vector3d last_pos(0,0,0);
if ( (odom_pos_ - last_pos).norm() > 0.03 )
{
  std::vector<Eigen::Vector3d> pts;
  pts.push_back(odom_pos_);
  pts.push_back(global_goal_);
  visualization_->displayGlobalPathList(pts, 0.02, 0);

  last_pos = odom_pos_;
}

if (have_latest_safe_pt_)
{
  if ( (odom_pos_ - latest_safe_pt_).norm() > 0.1 )
  {
    if ( planner_manager_->has_cloud_ && planner_manager_->depthCloudStack_.size() > 0)
    {
      pcl::PointCloud<pcl::PointXYZ>::Ptr plannerCloud(new pcl::PointCloud<pcl::PointXYZ>());
      for (int i = 0; i < planner_manager_->depthCloudStackNum_; i++) {
        *plannerCloud += *(planner_manager_->depthCloudStack_[i]);
      }

      const double SAFE_DIST = 0.3232; // m
      bool safe = true;
      
      // Reset current minimum distance to obstacles for this update
      double current_min_obstacle_dist = std::numeric_limits<double>::max();
      
      for(int j = 0; j < plannerCloud->points.size(); j++)
      {
        Eigen::Vector3d pc(plannerCloud->points[j].x, plannerCloud->points[j].y, plannerCloud->points[j].z);
        double dist = (odom_pos_ - pc).norm();
        
        // Update current minimum distance to obstacles
        current_min_obstacle_dist = std::min(current_min_obstacle_dist, dist);
        
        if ( dist < SAFE_DIST )
        {
          safe = false;
          
          // Record obstacle collision for flight statistics
          //点云分辨率0.1 膨胀sqrt(3) * voxelSize / 2.0 + drone_radius=0.2366  voxelSize=0.1  drone_radius=0.15 详见swarm_path.py
          if (have_log_files_ && (dist - 0.2366 < 1e-6)) { 
            collision_detected_ = true;
            collision_reason_ = "Static obstacle collision";
            ROS_ERROR("Static obstacle collision detected! Distance: %.3f", dist);
            has_collision_with_obs_ = true;
          }
          
          break;
        }
      }
      
      // Update global minimum obstacle distance if we found a valid distance in this update
      if (current_min_obstacle_dist < std::numeric_limits<double>::max()) {
        min_dist_to_obstacle_ = std::min(min_dist_to_obstacle_, current_min_obstacle_dist);
      }

      if ( safe )
        latest_safe_pt_ = odom_pos_;

    }
  }
}
else
{
  latest_safe_pt_ = odom_pos_;
  have_latest_safe_pt_ = true;
}


if ( have_log_files_ )
{
  std::vector<double> data;
  data.push_back(odom_pos_(0));
  data.push_back(odom_pos_(1));
  data.push_back(odom_pos_(2));
  data.push_back(odom_vel_(0));
  data.push_back(odom_vel_(1));
  data.push_back(odom_vel_(2));
  std::string csv_data = odom_logger_.toCSV(ros::Time::now(), data);
  odom_logger_.appendData(csv_data);
} 

}`

Timestamp,DroneID,FlightDuration,TotalDistance,AverageSpeed,FinalPositionX,FinalPositionY,FinalPositionZ,CollisionWithObs,CollisionWithAgent,MinDistToObstacle,MinDistToOtherDrone,NumberOfReplans,TotalReplanTime,AverageReplanTime,PercentTimeReplanning
1745380616.146654715,0,15.5689,14.8856,0.956111,-6.17185,0.276887,0.81781,true,false,0.220915,0.247071,73,0.021556,0.000295,0.138456
1745380613.950423949,1,13.3663,12.7325,0.952586,-5.70755,-1.83877,0.80868,true,true,0.225823,0.094077,63,0.021864,0.000347,0.163576
1745380616.296915344,2,15.703,14.8966,0.948646,-4.98336,-3.67668,0.94688,true,false,0.232566,1.041915,74,0.018256,0.000247,0.116257
1745380614.196555156,3,13.5909,12.9292,0.951309,-3.5864,-4.91439,0.85229,true,false,0.219765,0.313726,64,0.021294,0.000333,0.156680
1745380614.209381942,4,13.5925,12.929,0.951189,-1.77561,-5.64052,0.43973,false,true,0.250103,0.069275,64,0.022733,0.000355,0.167244
1745380614.152021054,5,13.5877,12.9166,0.950613,0.313955,-6.01438,0.36297,true,true,0.202603,0.095235,65,0.020659,0.000318,0.152045
1745380613.986574514,6,13.4318,12.7781,0.951331,1.58957,-5.81095,0.65904,false,true,0.270010,0.113409,64,0.021285,0.000333,0.158464
1745380615.221852274,7,14.6865,13.9667,0.950993,3.72535,-4.87926,0.90721,true,false,0.217234,0.355386,69,0.022571,0.000327,0.153689
1745380614.504146139,8,13.9907,13.3184,0.951948,4.73308,-3.84194,0.68804,false,false,0.255055,0.272330,66,0.019626,0.000297,0.140280
1745380613.922872179,9,13.3789,12.7316,0.951619,5.69802,-1.84078,0.79538,false,true,0.256967,0.141087,65,0.022198,0.000342,0.165916
1745380616.733904283,10,16.1093,13.9404,0.865367,6.02173,0.115453,0.77240,true,false,0.211462,0.400605,75,0.023768,0.000317,0.147541
1745380614.651165213,11,14.1256,13.4486,0.952072,5.69727,1.77085,0.72202,true,false,0.198985,0.369066,67,0.023013,0.000343,0.162918
1745380615.993114789,12,15.3602,14.6834,0.955942,4.87808,4.02729,0.79765,true,true,0.232798,0.083282,76,0.029909,0.000394,0.194717
1745380613.968579422,13,13.4648,12.7993,0.950578,3.43781,4.98221,0.78323,false,true,0.257582,0.159349,66,0.024971,0.000378,0.185452
1745380614.561960589,14,14.0691,13.4445,0.955607,1.65558,5.85778,0.7616,true,false,0.190220,0.243996,67,0.024887,0.000371,0.176890
1745380614.283461378,16,13.8015,13.1064,0.949636,-1.69045,5.88191,0.86730,false,true,0.251407,0.098511,65,0.021548,0.000332,0.156132
1745380615.85951929,17,14.6243,13.9356,0.952902,-3.34511,5.15312,0.72885,true,true,0.207883,0.165448,73,0.025463,0.000349,0.174116
1745380614.61760926,18,13.6104,12.9265,0.94975,-4.91995,3.37322,0.54671,true,false,0.210677,0.393769,65,0.017665,0.000272,0.129791
1745380619.536455399,19,19.0639,18.1762,0.953432,-6.01526,2.32054,0.82999,false,true,0.275303,0.085989,93,0.026092,0.000281,0.136865

我使用一个了另一个节点订阅所有无人机的里程计，单独进行无人机间的碰撞检测
` # 为每个无人机创建订阅者
for drone_id in range(self.num_drones):
topic = self.odom_topic_format.format(drone_id)
sub = rospy.Subscriber(
topic,
Odometry,
self.odom_callback,
callback_args=drone_id
)
self.subscribers.append(sub)
rospy.loginfo(f"已订阅 {topic}")

    # 创建一个定时器，定期检查无人机之间的距离
    self.check_timer = rospy.Timer(rospy.Duration(0.02), self.check_distances)  # 10Hz

def odom_callback(self, msg, drone_id):
# 提取位置
pos = msg.pose.pose.position
position = np.array([pos.x, pos.y, pos.z])

    # 提取速度
    vel = msg.twist.twist.linear
    velocity = np.array([vel.x, vel.y, vel.z])
    
    # 存储数据
    self.drone_positions[drone_id] = position
    self.drone_velocities[drone_id] = velocity
    self.timestamps[drone_id] = msg.header.stamp
    
    # 如果需要记录里程计数据，则写入CSV文件
    if self.record_odom and drone_id in self.odom_csv_writers:
        timestamp = msg.header.stamp.to_sec()
        
        # 记录到CSV文件
        self.odom_csv_writers[drone_id].writerow([
            timestamp,
            pos.x, pos.y, pos.z,
            vel.x, vel.y, vel.z
        ])
        
        # 每10条数据刷新一次文件缓冲区，避免频繁IO操作
        if int(timestamp * 10) % 10 == 0:  # 大约每秒刷新一次
            self.odom_csv_files[drone_id].flush()

def check_distances(self, event):
    """检查所有无人机之间的距离，如果小于安全距离则记录"""
    if len(self.drone_positions) < 2:
        return  # 需要至少两架无人机的数据
    
    # 为每个无人机计算最小距离
    for drone_id in self.drone_positions:
        min_distance = float('inf')
        closest_drone = None
        
        for other_id in self.drone_positions:
            if drone_id == other_id:
                continue  # 跳过自身
            
            # 计算两架无人机之间的距离
            pos1 = self.drone_positions[drone_id]
            pos2 = self.drone_positions[other_id]
            distance = np.linalg.norm(pos1 - pos2)
            
            # 更新最小距离
            if distance < min_distance:
                min_distance = distance
                closest_drone = other_id
            
            # 如果距离小于安全距离，记录到CSV
            if distance < self.safety_distance:
                # 获取当前时间戳
                timestamp = self.timestamps.get(drone_id, rospy.Time.now())
                
                # 记录到CSV文件
                self.csv_writers[drone_id].writerow([
                    timestamp.to_sec(),
                    drone_id,
                    other_id,
                    pos1[0], pos1[1], pos1[2],
                    pos2[0], pos2[1], pos2[2],
                    distance
                ])
                
                # 立即刷新文件缓冲区
                self.csv_files[drone_id].flush()
                
                rospy.logwarn(f"潜在碰撞风险! 无人机 {drone_id} 和无人机 {other_id} 之间的距离: {distance:.3f}m")
        
        # 更新该无人机的最小距离信息
        if closest_drone is not None:
            # 如果是第一次记录或者距离更小，则更新
            if drone_id not in self.drone_min_distances or min_distance < self.drone_min_distances[drone_id]['min_distance']:
                self.drone_min_distances[drone_id] = {
                    'min_distance': min_distance,
                    'closest_drone': closest_drone,
                    'timestamp': rospy.Time.now()
                }

`
统计数据中可以看到某些无人机间的最小距离小于0.3：
timestamp,drone_id,closest_drone_id,min_distance
1745379913.4987183,18,16,0.5117502627686759
1745379914.360363,19,0,0.11631997586149437
1745379915.7220552,17,9,0.0357553423146258
1745379915.6977174,16,0,0.3027294550643361
1745379915.4360116,15,6,0.09747583357337575
1745379916.350948,14,17,0.22051316351402608
1745379915.6322777,13,8,0.07165828232617993
1745379911.2592359,12,11,1.0739991556525492
1745379916.1118956,11,8,0.0902747808537505
1745379915.730172,9,17,0.046552766253204064
1745379914.4840295,10,11,0.25816349159989216
1745379915.6385045,8,13,0.06990080877465157
1745379924.0886233,7,9,0.47248885405434904
1745379915.4241142,6,13,0.07814394656446005
1745379914.9096737,5,2,0.09975575230557378
1745379915.5573263,4,2,0.1831445981049852
1745379911.9795296,3,4,0.32876700575933376
1745379914.9117634,2,5,0.09975575230557378
1745379913.4017692,1,0,0.20309488727719777
1745379914.3673394,0,19,0.11631997586149437

我将advanced_param.xml中的安全间隙设置为原始代码中的0.24 “”进行测试，问题依然存在：
Timestamp,DroneID,FlightDuration,TotalDistance,AverageSpeed,FinalPositionX,FinalPositionY,FinalPositionZ,CollisionWithObs,CollisionWithAgent,MinDistToObstacle,MinDistToOtherDrone,NumberOfReplans,TotalReplanTime,AverageReplanTime,PercentTimeReplanning
1745380616.146654715,0,15.5689,14.8856,0.956111,-6.17185,0.276887,0.81781,true,false,0.220915,0.247071,73,0.021556,0.000295,0.138456
1745380613.950423949,1,13.3663,12.7325,0.952586,-5.70755,-1.83877,0.80868,true,true,0.225823,0.094077,63,0.021864,0.000347,0.163576
1745380616.296915344,2,15.703,14.8966,0.948646,-4.98336,-3.67668,0.94688,true,false,0.232566,1.041915,74,0.018256,0.000247,0.116257
1745380614.196555156,3,13.5909,12.9292,0.951309,-3.5864,-4.91439,0.85229,true,false,0.219765,0.313726,64,0.021294,0.000333,0.156680
1745380614.209381942,4,13.5925,12.929,0.951189,-1.77561,-5.64052,0.43973,false,true,0.250103,0.069275,64,0.022733,0.000355,0.167244
1745380614.152021054,5,13.5877,12.9166,0.950613,0.313955,-6.01438,0.36297,true,true,0.202603,0.095235,65,0.020659,0.000318,0.152045
1745380613.986574514,6,13.4318,12.7781,0.951331,1.58957,-5.81095,0.65904,false,true,0.270010,0.113409,64,0.021285,0.000333,0.158464
1745380615.221852274,7,14.6865,13.9667,0.950993,3.72535,-4.87926,0.90721,true,false,0.217234,0.355386,69,0.022571,0.000327,0.153689
1745380614.504146139,8,13.9907,13.3184,0.951948,4.73308,-3.84194,0.68804,false,false,0.255055,0.272330,66,0.019626,0.000297,0.140280
1745380613.922872179,9,13.3789,12.7316,0.951619,5.69802,-1.84078,0.79538,false,true,0.256967,0.141087,65,0.022198,0.000342,0.165916
1745380616.733904283,10,16.1093,13.9404,0.865367,6.02173,0.115453,0.77240,true,false,0.211462,0.400605,75,0.023768,0.000317,0.147541
1745380614.651165213,11,14.1256,13.4486,0.952072,5.69727,1.77085,0.72202,true,false,0.198985,0.369066,67,0.023013,0.000343,0.162918
1745380615.993114789,12,15.3602,14.6834,0.955942,4.87808,4.02729,0.79765,true,true,0.232798,0.083282,76,0.029909,0.000394,0.194717
1745380613.968579422,13,13.4648,12.7993,0.950578,3.43781,4.98221,0.78323,false,true,0.257582,0.159349,66,0.024971,0.000378,0.185452
1745380614.561960589,14,14.0691,13.4445,0.955607,1.65558,5.85778,0.7616,true,false,0.190220,0.243996,67,0.024887,0.000371,0.176890
1745380614.283461378,16,13.8015,13.1064,0.949636,-1.69045,5.88191,0.86730,false,true,0.251407,0.098511,65,0.021548,0.000332,0.156132
1745380615.85951929,17,14.6243,13.9356,0.952902,-3.34511,5.15312,0.72885,true,true,0.207883,0.165448,73,0.025463,0.000349,0.174116
1745380614.61760926,18,13.6104,12.9265,0.94975,-4.91995,3.37322,0.54671,true,false,0.210677,0.393769,65,0.017665,0.000272,0.129791
1745380619.536455399,19,19.0639,18.1762,0.953432,-6.01526,2.32054,0.82999,false,true,0.275303,0.085989,93,0.026092,0.000281,0.136865
独立节点检测数据：
timestamp,drone_id,closest_drone_id,min_distance
1745380608.4696946,0,14,0.24576234499512312
1745380603.6296566,3,4,0.33805389055472806
1745380608.0323174,5,4,0.09351045806467694
1745380608.0342665,4,5,0.09351045806467694
1745380607.4144013,6,12,0.0995926878119121
1745380607.1320908,8,15,0.26794368034680266
1745380607.396458,12,6,0.10020075713553488
1745380607.342138,15,6,0.1105586069139684
1745380606.9168706,17,1,0.17676436139688642
1745380605.9925964,19,1,0.08819335703894987
1745380604.3100505,18,17,0.3843771313224565
1745380607.9597821,16,19,0.13536492662790264
1745380608.4750416,14,0,0.24576234499512312
1745380607.2606046,13,9,0.16994714786779896
1745380608.259641,11,7,0.3633634048548058
1745380606.3303242,10,12,0.38845301575951524
1745380607.5752864,9,1,0.14029114254227815
1745380608.2598448,7,11,0.3633634048548058
1745380604.1907122,2,3,1.0551614904826074
1745380605.9948297,1,19,0.08819335703894987

[Bziyue]