| 55 | int drone_num_; |
| 56 | |
| 57 | double calcPathLength(const vector<Eigen::Vector3d>& path) { |
| 58 | if (path.empty()) return 0; |
| 59 | double len = 0.0; |
| 60 | for (int i = 0; i < path.size() - 1; ++i) { |
| 61 | len += (path[i + 1] - path[i]).norm(); |
| 62 | } |
| 63 | return len; |
| 64 | } |
| 65 | |
| 66 | void displayTrajWithColor( |
| 67 | vector<Eigen::Vector3d> path, double resolution, Eigen::Vector4d color, int id) { |