C++ (Cpp) Pose::queryY示例

QPainterPath getTrajectoryPath(FType func, Pose s, int vl_s, int vr_s, Pose e, int vl_e, int vr_e,
                                          double timespanMs, double timeLCMs) {
    // using some code from simulate() in dialog.cpp
    QPainterPath p;
    p.moveTo(s.queryX(), s.queryY());
    double final_vel = max(fabs(vl_e), fabs(vr_e));
    ControllerWrapper dc(func, vl_s, vr_s, 0);  // not using any delay for trajectory drawing.
    Pose curPose = s;
    for (int i = 0; i*timeLCMs < timespanMs; i++) {
        int vl, vr;
        dc.genControls(curPose, e, vl, vr, final_vel);
        // patch up code;
//        insertArcInPath(p, curPose.theta(), vl, vr, timeLCMs*0.001);
        curPose.update(vl, vr, timeLCMs*0.001);
        p.lineTo(curPose.queryX(), curPose.queryY());
    }
    return p;
}