bool new_x = false;
    // 这里的{}用于限定互斥锁的作用范围，以确保互斥锁仅在需要时锁住。
    {
        //创建一个名为 lock 的 std::lock_guard 对象，它使用 名为_x_feedback_mutex的 互斥锁。
        // 这个锁的作用是确保在同一时刻只有一个线程可以访问被锁住的代码块
        std::lock_guard<std::mutex> lock(_x_feedback_mutex);
        // _recent_x_feedback的值在另一个线程中进行更新，在本线程中获取其值，使用互锁可以保证不会同时对_recent_x_feedback执行读写操作
        // 这行代码用于判断是否有新的状态数据可用。它包含两个条件：
        // _recent_x_feedback.size() > 0：检查 _recent_x_feedback 容器中是否有数据。
        //(t - _recent_x_time).toSec() < 2.0 * dt：检查距离最近一次状态更新的时间是否小于 2.0 * dt 秒，其中 t 表示当前时间，_recent_x_time 表示最近一次状态数据更新的时间，而 dt 表示时间步长。
        //如果这两个条件都满足，将 new_x 设置为 true，表示有新的状态数据可用。
        new_x = _recent_x_feedback.size() > 0 && (t - _recent_x_time).toSec() < 2.0 * dt;
        // 如果 new_x 的值为 true，即有新的状态数据可用，程序会将 _recent_x_feedback 复制给 x，从而更新机器人的当前状态。
        if (new_x) x = _recent_x_feedback;
    }
    // 如果没有新的状态数据可用，且（没有时间序列数据 _x_ts，或者该时间序列为空，或者无法在时间间隔 dt 内插值得到状态 x ），
    // 则执行条件中的程序，直接将PoseSE2格式的变量start转换为 Eigen::VectorXd格式的变量 x
    if (!new_x && (!_x_ts || _x_ts->isEmpty() || !_x_ts->getValuesInterpolate(dt, x)))  // predict with previous state sequence
    {
        _dynamics->getSteadyStateFromPoseSE2(start, x);  // otherwise initialize steady state
    }
    if (!new_x || !_prefer_x_feedback)
    {
         //如果需要，将状态反馈与里程计反馈合并。
         //注意，一些模型（如独轮车）会通过odom反馈覆盖整个状态，除非_preferr_x_measurement设置为true。
        _dynamics->mergeStateFeedbackAndOdomFeedback(start, vel, x);
    }

   （7）检查是否需要重新初始化

    if (_force_reinit_num_steps > 0 && _ocp_seq % _force_reinit_num_steps == 0) _grid->clear();
    if (!_grid->isEmpty() && ((goal.position() - _last_goal.position()).norm() > _force_reinit_new_goal_dist ||
                              std::abs(normalize_theta(goal.theta() - _last_goal.theta())) > _force_reinit_new_goal_angular))
    {
        // goal pose diverges a lot from the previous one, so force reinit
        _grid->clear();
    }

    if (_grid->isEmpty())
    {
        // 检查目标点是否在当前点的后方，并估计是否需要执行后退运动，参数_guess_backwards_motion是是否启用该功能的开关
        // 开启该功能时，计算机器人的初始位置到目标位置的矢量与初始方向的点积，如果点积小于0，表示矢量与方向相反，因此机器人可能需要执行反向运动。
        bool backward = _guess_backwards_motion && (goal.position() - start.position()).dot(start.orientationUnitVec()) < 0;
        // 将给定的初始路径initial_plan，添加时间序列信息以及姿态信息，从而转换为初始轨迹，并采用线性差值对相邻两个轨迹点之间的轨迹进行差值
        // 生成的轨迹存放在controller类的变量_x_seq_init中
        generateInitialStateTrajectory(x, xf, initial_plan, backward);
    }

 _ocp_successful = PredictiveController::step(x, xref, uref, corbo::Duration(dt), time, u_seq, x_seq, nullptr, nullptr, &_x_seq_init);

if (_publish_ocp_results) publishOptimalControlResult();  // TODO(roesmann): we could also pass time t from above

 ROS_INFO_STREAM_COND(_print_cpu_time, "Cpu time: " << _statistics.step_time.toSec() * 1000.0 << " ms.");

 ++_ocp_seq;

_last_goal = goal;

return _ocp_successful;

bool PredictiveController::step(const ControllerInterface::StateVector& x, ReferenceTrajectoryInterface& xref, ReferenceTrajectoryInterface& uref,
                                const Duration& dt, const Time& t, TimeSeries::Ptr u_sequence, TimeSeries::Ptr x_sequence,
                                SignalTargetInterface* signal_target, ReferenceTrajectoryInterface* sref, ReferenceTrajectoryInterface* xinit,
                                ReferenceTrajectoryInterface* uinit, const std::string& ns)

    if (!_initialized)
    {
        if (!initialize(x, xref, uref, dt, t, sref)) return false;
    }

ControlVector u;

if (!_ocp) return false;

if (_auto_update_prev_control) _ocp->setPreviousControlInputDt(dt.toSec());

    // 创建一个名为 success 的布尔变量，用于跟踪计算步骤是否成功。
    bool success = false;
    // 获取当前时间，用于计算计算步骤的执行时间。
    Time t_pre = Time::now();

for (int i = 0; i < _num_ocp_iterations; ++i) success = _ocp->compute(x, xref, uref, sref, t, i == 0, signal_target, xinit, uinit, ns);

 _statistics.step_time = Time::now() - t_pre;

 success = success && _ocp->getFirstControlInput(u);
    // 如果启用了自动更新前一控制输入的选项，使用计算得到的控制输入 u 来更新前一控制输入。
 if (_auto_update_prev_control) _ocp->setPreviousControlInput(u, dt.toSec());  // cache control input for next step call

    _ocp->getTimeSeries(x_sequence, u_sequence);
    _x_ts = x_sequence;  //更新内部状态变量 _x_ts，将其设置为状态序列 x_sequence。
    _u_ts = u_sequence;  //更新内部状态变量 _u_ts，将其设置为控制输入序列 u_sequence。