for (int i = 0; i < agvs.size(); i++) { if (agvs[i].getLoad() == true) { // 如果是负载的状态 if (agvs[i].getCurrentX() == agvs[i].getEndX() && agvs[i].getCurrentY() == agvs[i].getEndY()) { // 如果到达终点 agvs[i].setLoad(false); // 设置为空载状态 agvs[i].setState(true); std :: cout << "agv__id :" << agvs[i].getid() << " ,agv_get_task_id :" << agvs[i].get_task_id() << endl; for (int j = 0; j < tasks.size(); j++) { if (tasks[j].id == agvs[i].get_task_id()) { completed_task_index = j; break; } } if (completed_task_index != -1) { tasks[completed_task_index].completed = 2; } task_to_agv(); // 更新任务分配 update(); // 更新AGV状态 } else { // 否则行驶到终点 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 { // 如果是空载的状态 if (agvs[i].getCurrentX() == agvs[i].getStartX() && agvs[i].getCurrentY() == agvs[i].getStartY()) { // 如果到达起点 agvs[i].setLoad(true); // 设置为负载状态 } else { // 否则行驶到起点 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; } } }，添加代碼：繪製直綫agv從path獲取的行駛路綫

你可以使用图形库（如OpenCV）来绘制直线路径。你可以按照以下步骤进行操作：

1. 在循环中，使用当前AGV的路径（paths[i]）来绘制直线路径。

2. 在绘制前，需要将路径中的节点坐标转换为像素坐标。可以使用以下公式将节点坐标转换为像素坐标：

pixel_x = node_x * pixel_per_meter;

pixel_y = node_y * pixel_per_meter;

其中，pixel_per_meter是每米对应的像素数，可以根据地图的比例尺计算得到。

3. 绘制直线路径。可以使用cv::line函数来绘制直线，参数为起点坐标和终点坐标。

下面是一个示例代码片段，可以帮助你完成绘制直线路径的操作：

// 定义像素坐标系与地图坐标系的比例尺
double pixel_per_meter = 10.0;

for (int i = 0; i < agvs.size(); i++) {
    // 省略其他代码
    if (agvs[i].getLoad() == true) {
        // 省略其他代码
        else {
            // 否则行驶到终点
            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;
            
            // 绘制直线路径
            cv::Mat map_image; // 地图图像
            // 省略其他代码，加载地图图像
            cv::Scalar path_color(0, 0, 255); // 红色
            for (int j = 1; j < path.size(); j++) {
                int start_x = path[j-1]->x * pixel_per_meter;
                int start_y = path[j-1]->y * pixel_per_meter;
                int end_x = path[j]->x * pixel_per_meter;
                int end_y = path[j]->y * pixel_per_meter;
                cv::Point start_point(start_x, start_y);
                cv::Point end_point(end_x, end_y);
                cv::line(map_image, start_point, end_point, path_color, 2);
            }
        }
    }
    else {
        // 省略其他代码
    }
}