Node* start_node = new Node(agvs[i].current_x, agvs[i].current_y); Node* end_node = new Node(agvs[i].getStartX(), agvs[i].getStartY()); std::vector<Node*> path = astar.getPath(start_node, end_node); path.erase(path.begin()); paths[i] = path; }，添加代碼：遍歷所有paths[i]的第一個坐標，如果有其中兩個相同，輸出所有的i

好的，根据您的要求，可以在代码中添加以下部分来实现：
// 遍历所有路径的第一个坐标，查找是否有相同的
for (int i = 0; i < paths.size(); i++) {
for (int j = i + 1; j < paths.size(); j++) {
// 判断两个路径的第一个节点是否相同
if (paths[i][0]->x == paths[j][0]->x && paths[i][0]->y == paths[j][0]->y) {
std::cout << "i: " << i << " j: " << j << std::endl;
}
}
}

这段代码会遍历所有路径的第一个坐标，然后通过判断两个路径的第一个节点是否相同，来输出所有满足条件的 i 和 j 的值。您可以将这段代码添加到原有代码的末尾即可。

相关问题

Node* start_node = new Node(agvs[i].getCurrentX(), agvs[i].getCurrentY()); Node* end_node = new Node(agvs[i].getStartX(), agvs[i].getStartY()); Node* end_node1 = new Node(agvs[i].getEndX(), agvs[i].getEndY()); std::vector<Node*> path_to_start = astar.getPath(start_node, end_node); std::vector<Node*> path_to_end = astar.getPath(end_node, end_node1);,增加代碼，如果agv的current的x和y等於任務起點的x和y，則只將agv的起點和任務的終點放入path中

你可以在 if 语句中添加条件，判断 agv 的当前坐标是否与任务起点坐标相同，如果是则直接将 agv 的起点和任务的终点放入路径中。代码如下：
Node* start_node = new Node(agvs[i].getCurrentX(), agvs[i].getCurrentY());
Node* end_node = new Node(agvs[i].getStartX(), agvs[i].getStartY());
Node* end_node1 = new Node(agvs[i].getEndX(), agvs[i].getEndY());

if (agvs[i].getCurrentX() == agvs[i].getStartX() && agvs[i].getCurrentY() == agvs[i].getStartY()) {
std::vector<Node*> path;
path.push_back(start_node);
path.push_back(end_node1);
// do something with path
}
else {
std::vector<Node*> path_to_start = astar.getPath(start_node, end_node);
std::vector<Node*> path_to_end = astar.getPath(end_node, end_node1);
// do something with path_to_start and path_to_end
}

void MainWindow::moveAgvs_(){ timer =new QTimer(this); timer->start(100); connect(timer, &QTimer::timeout, this, &MainWindow::moveAgvs);} void MainWindow::moveAgvs() { Astar astar; std::vector<std::vector<Node*>> paths(agvs.size()); // 得到agv的路綫 for (int i = 0; i < agvs.size(); i++) { if (agvs[i].getLoad()){//如果是負載的狀態，則任務的起點到任務的終點 if (agvs[i].getCurrentX() == agvs[i].getEndX() && agvs[i].getCurrentY() == agvs[i].getEndY()) { agvs[i].setState(true); } Node* start_node = new Node(agvs[i].getCurrentX(), agvs[i].getCurrentY()); Node* end_node1 = new Node(agvs[i].getEndX(), agvs[i].getEndY()); std::vector<Node*> path_to_end = astar.getPath(start_node, end_node1); path_to_end.erase(path_to_end.begin()); std::vector<Node*> path; path.insert(path.end(), path_to_end.begin(), path_to_end.end()); paths[i] = path;} else { //如果是空載的狀態，則行駛到任務的起點 //如果agv已經到達任務起點，變爲負載狀態 if (agvs[i].getCurrentX() == agvs[i].getStartX() && agvs[i].getCurrentY() == agvs[i].getStartY()) { agvs[i].setLoad(true); } Node* start_node = new Node(agvs[i].getCurrentX(), agvs[i].getCurrentY()); Node* end_node = new Node(agvs[i].getStartX(), agvs[i].getStartY()); std::vector<Node*> path_to_start = astar.getPath(start_node, end_node); std::vector<Node*> path; path.insert(path.end(), path_to_start.begin() + 1, path_to_start.end()); paths[i] = path;} } //模擬小車行駛 for (int i = 0; i < agvs.size(); i++) { if (! paths[i].empty()) { Node* next_node = paths[i][0]; float speed = agvs[i].getSpeed(); float distance = sqrt(pow(next_node->x - agvs[i].getCurrentX(), 2) + pow(next_node->y - agvs[i].getCurrentY(), 2)); float time = distance / speed * 1000; QTimer::singleShot(time, this, &, i, next_node { agvs[i].setCurrentX(next_node->x); agvs[i].setCurrentY(next_node->y); std::cout << "AGV " << agvs[i].getid() << " current_x: " << agvs[i].getCurrentX() << " current_y: " << agvs[i].getCurrentY() <<std::endl; this->update(); if (next_node->x == agvs[i].getEndX() && next_node->y == agvs[i].getEndY()) { task_to_agv(); } }); } } }，增加函數，畫出小車形式路徑

可以在 MainWindow 类中增加一个函数来画出小车行驶的路径。具体实现如下：
void MainWindow::drawPaths() {
QPainter painter(this);
for (int i = 0; i < agvs.size(); i++) {
std::vector<Node*> path = paths[i];
if (!path.empty()) {
painter.setPen(QPen(Qt::green, 3));
for (int j = 0; j < path.size() - 1; j++) {
painter.drawLine(QPointF(path[j]->x, path[j]->y), QPointF(path[j+1]->x, path[j+1]->y));
}
}
}
}

在 moveAgvs() 函数中，调用 drawPaths() 函数来画出路径。同时，为了保证路径和小车的移动同步，需要在 QTimer 计时器的回调函数中调用 drawPaths() 函数。具体实现如下：
void MainWindow::moveAgvs() {
Astar astar;
std::vector<std::vector<Node*>> paths(agvs.size());

// 得到agv的路线
for (int i = 0; i < agvs.size(); i++) {
if (agvs[i].getLoad()) { // 如果是负载的状态，则任务的起点到任务的终点
if (agvs[i].getCurrentX() == agvs[i].getEndX() && agvs[i].getCurrentY() == agvs[i].getEndY()) {
agvs[i].setState(true);
}
Node* start_node = new Node(agvs[i].getCurrentX(), agvs[i].getCurrentY());
Node* end_node1 = new Node(agvs[i].getEndX(), agvs[i].getEndY());
std::vector<Node*> path_to_end = astar.getPath(start_node, end_node1);
path_to_end.erase(path_to_end.begin());
std::vector<Node*> path;
path.insert(path.end(), path_to_end.begin(), path_to_end.end());
paths[i] = path;
} else { // 如果是空载的状态，则行驶到任务的起点
// 如果agv已经到达任务起点，变为负载状态
if (agvs[i].getCurrentX() == agvs[i].getStartX() && agvs[i].getCurrentY() == agvs[i].getStartY()) {
agvs[i].setLoad(true);
}
Node* start_node = new Node(agvs[i].getCurrentX(), agvs[i].getCurrentY());
Node* end_node = new Node(agvs[i].getStartX(), agvs[i].getStartY());
std::vector<Node*> path_to_start = astar.getPath(start_node, end_node);
std::vector<Node*> path;
path.insert(path.end(), path_to_start.begin() + 1, path_to_start.end());
paths[i] = path;
}
}

// 模拟小车行驶
for (int i = 0; i < agvs.size(); i++) {
if (!paths[i].empty()) {
Node* next_node = paths[i][0];
float speed = agvs[i].getSpeed();
float distance = sqrt(pow(next_node->x - agvs[i].getCurrentX(), 2) + pow(next_node->y - agvs[i].getCurrentY(), 2));
float time = distance / speed * 1000;
QTimer::singleShot(time, this, [=, &paths] {
agvs[i].setCurrentX(next_node->x);
agvs[i].setCurrentY(next_node->y);
std::cout << "AGV " << agvs[i].getid() << " current_x: " << agvs[i].getCurrentX() << " current_y: " << agvs[i].getCurrentY() << std::endl;
paths[i].erase(paths[i].begin());
this->update();
if (next_node->x == agvs[i].getEndX() && next_node->y == agvs[i].getEndY()) {
task_to_agv();
}
drawPaths(); // 每次移动后都重新画出路径
});
}
}
}