写一个基于Qt的机器人A星算法路径规划

好的，以下是一个基于Qt的机器人A星算法路径规划的示例代码：

#include <QtWidgets/QApplication>
#include <QtWidgets/QWidget>
#include <QtWidgets/QPushButton>
#include <QtWidgets/QHBoxLayout>
#include <QtGui/QPainter>
#include <QtCore/QDebug>
#include <QtCore/QPointF>
#include <QtCore/QRectF>
#include <QtCore/QList>
#include <QtCore/QMap>
#include <QtCore/QSet>
#include <QtCore/QPair>
#include <QtCore/QTimer>

class Robot : public QWidget {
    Q_OBJECT

public:
    Robot(QWidget *parent = nullptr);

protected:
    void paintEvent(QPaintEvent *event) override;
    void mousePressEvent(QMouseEvent *event) override;
    void mouseReleaseEvent(QMouseEvent *event) override;

private:
    void drawMap(QPainter &painter);
    void drawPath(QPainter &painter);
    void updatePath();
    void findPath();

    QPoint mapToGrid(const QPointF &point) const;
    QPointF gridToMap(const QPoint &point) const;
    bool isObstacle(const QPoint &point) const;
    bool isValid(const QPoint &point) const;
    int distance(const QPoint &a, const QPoint &b) const;
    QList<QPoint> neighbors(const QPoint &point) const;
    int heuristic(const QPoint &point) const;

    QPointF m_start;
    QPointF m_end;
    QList<QPointF> m_path;
    QMap<QPoint, bool> m_obstacles;
};

Robot::Robot(QWidget *parent)
    : QWidget(parent)
{
    setMinimumSize(400, 400);

    QPushButton *resetButton = new QPushButton(tr("Reset"), this);
    QPushButton *findPathButton = new QPushButton(tr("Find Path"), this);
    QHBoxLayout *layout = new QHBoxLayout(this);
    layout->addWidget(resetButton);
    layout->addWidget(findPathButton);

    connect(resetButton, &QPushButton::clicked, [this]() {
        m_start = QPointF();
        m_end = QPointF();
        m_path.clear();
        m_obstacles.clear();
        update();
    });

    connect(findPathButton, &QPushButton::clicked, this, &Robot::findPath);
}

void Robot::paintEvent(QPaintEvent *event)
{
    Q_UNUSED(event);

    QPainter painter(this);
    drawMap(painter);
    drawPath(painter);
}

void Robot::mousePressEvent(QMouseEvent *event)
{
    if (event->button() == Qt::LeftButton) {
        m_start = event->pos();
        m_path.clear();
        update();
    } else if (event->button() == Qt::RightButton) {
        m_end = event->pos();
        m_path.clear();
        update();
    }
}

void Robot::mouseReleaseEvent(QMouseEvent *event)
{
    if (event->button() == Qt::LeftButton) {
        m_end = event->pos();
        updatePath();
    } else if (event->button() == Qt::RightButton) {
        m_start = event->pos();
        updatePath();
    }
}

void Robot::drawMap(QPainter &painter)
{
    painter.setPen(Qt::black);
    painter.setBrush(Qt::white);
    painter.drawRect(rect());

    painter.setPen(Qt::NoPen);
    painter.setBrush(Qt::gray);
    for (QMap<QPoint, bool>::const_iterator it = m_obstacles.constBegin(); it != m_obstacles.constEnd(); ++it) {
        if (it.value())
            painter.drawRect(QRectF(gridToMap(it.key()), QSizeF(1, 1)));
    }

    painter.setBrush(Qt::green);
    painter.drawEllipse(QRectF(m_start - QPointF(0.5, 0.5), QSizeF(1, 1)));

    painter.setBrush(Qt::red);
    painter.drawEllipse(QRectF(m_end - QPointF(0.5, 0.5), QSizeF(1, 1)));
}

void Robot::drawPath(QPainter &painter)
{
    painter.setPen(Qt::blue);
    painter.setBrush(Qt::NoBrush);
    for (int i = 0; i < m_path.count() - 1; ++i) {
        painter.drawLine(gridToMap(m_path.at(i)), gridToMap(m_path.at(i + 1)));
    }
}

void Robot::updatePath()
{
    if (m_start.isNull() || m_end.isNull())
        return;

    findPath();
    update();
}

void Robot::findPath()
{
    QList<QPoint> openList;
    QList<QPoint> closedList;
    QMap<QPoint, QPoint> cameFrom;
    QMap<QPoint, int> gScore;
    QMap<QPoint, int> fScore;

    QPoint start = mapToGrid(m_start);
    QPoint end = mapToGrid(m_end);

    openList.append(start);
    gScore[start] = 0;
    fScore[start] = gScore[start] + heuristic(start);

    while (!openList.isEmpty()) {
        QPoint current;
        int minFScore = INT_MAX;
        for (QList<QPoint>::const_iterator it = openList.constBegin(); it != openList.constEnd(); ++it) {
            if (fScore[*it] < minFScore) {
                current = *it;
                minFScore = fScore[*it];
            }
        }

        if (current == end) {
            QList<QPointF> path;
            path.append(m_end);
            QPoint point = end;
            while (cameFrom.contains(point)) {
                point = cameFrom[point];
                path.prepend(gridToMap(point) + QPointF(0.5, 0.5));
            }
            path.prepend(m_start);
            m_path = path;
            return;
        }

        openList.removeOne(current);
        closedList.append(current);

        for (QList<QPoint>::const_iterator it = neighbors(current).constBegin(); it != neighbors(current).constEnd(); ++it) {
            if (closedList.contains(*it))
                continue;

            int tentativeGScore = gScore[current] + distance(current, *it);
            if (!openList.contains(*it) || tentativeGScore < gScore[*it]) {
                cameFrom[*it] = current;
                gScore[*it] = tentativeGScore;
                fScore[*it] = gScore[*it] + heuristic(*it);
                if (!openList.contains(*it))
                    openList.append(*it);
            }
        }
    }
}

QPoint Robot::mapToGrid(const QPointF &point) const
{
    return QPoint(qFloor(point.x()), qFloor(point.y()));
}

QPointF Robot::gridToMap(const QPoint &point) const
{
    return QPointF(point.x(), point.y());
}

bool Robot::isObstacle(const QPoint &point) const
{
    if (m_obstacles.contains(point))
        return m_obstacles[point];
    return false;
}

bool Robot::isValid(const QPoint &point) const
{
    return point.x() >= 0 && point.x() < width() && point.y() >= 0 && point.y() < height();
}

int Robot::distance(const QPoint &a, const QPoint &b) const
{
    int dx = qAbs(a.x() - b.x());
    int dy = qAbs(a.y() - b.y());
    return dx + dy;
}

QList<QPoint> Robot::neighbors(const QPoint &point) const
{
    QList<QPoint> result;
    for (int dx = -1; dx <= 1; ++dx) {
        for (int dy = -1; dy <= 1; ++dy) {
            if (dx == 0 && dy == 0)
                continue;
            QPoint neighbor(point.x() + dx, point.y() + dy);
            if (isValid(neighbor) && !isObstacle(neighbor))
                result.append(neighbor);
        }
    }
    return result;
}

int Robot::heuristic(const QPoint &point) const
{
    return distance(point, mapToGrid(m_end));
}

int main(int argc, char *argv[])
{
    QApplication app(argc, argv);

    Robot robot;
    robot.show();

    return app.exec();
}

#include "main.moc"

这个示例代码实现了一个简单的机器人路径规划程序，使用A星算法来寻找从起点到终点的最短路径。程序中使用了Qt的绘图功能来显示地图和路径，并且通过鼠标点击来设置起点和终点。地图上的障碍物可以通过鼠标点击来设置或取消。在寻找路径时，程序会计算每个格子的G值和F值，并根据它们来选择下一个要探索的格子。最后，程序会通过反向追溯来计算路径，并将其显示在屏幕上。