.gitignore

@@ -23,4 +23,3 @@ build
 
 # Compiled files
 mpc_local_planner/cmake-build-debug/
-mpc_local_planner/build

mpc_local_planner/src/controller.cpp

@@ -59,9 +59,9 @@ bool Controller::configure(ros::NodeHandle& nh, const teb_local_planner::ObstCon
                            teb_local_planner::RobotFootprintModelPtr robot_model, const std::vector<teb_local_planner::PoseSE2>& via_points)
 {
 
-    // 创建机器人动力学模型
+    //创建机器模型
     _dynamics = configureRobotDynamics(nh);
-    if (!_dynamics) return false;  // 我们可能需要状态和控制维度来检查其他参数
+    if (!_dynamics) return false;  // we may need state and control dimensions to check other parameters
     //离散网络，比如多重打靶法。参考点，输入，状态，等变量也会存放在grid里面，会实时更新。而且grid也继承了顶点传入到超图问题构建中
     _grid   = configureGrid(nh);
     //求解器
@@ -74,23 +74,23 @@ bool Controller::configure(ros::NodeHandle& nh, const teb_local_planner::ObstCon
     nh.param("controller/outer_ocp_iterations", outer_ocp_iterations, outer_ocp_iterations);
     setNumOcpIterations(outer_ocp_iterations);
 
-    // 进一步的目标选项
+    // further goal opions
     nh.param("controller/force_reinit_new_goal_dist", _force_reinit_new_goal_dist, _force_reinit_new_goal_dist);
     nh.param("controller/force_reinit_new_goal_angular", _force_reinit_new_goal_angular, _force_reinit_new_goal_angular);
 
     nh.param("controller/allow_init_with_backward_motion", _guess_backwards_motion, _guess_backwards_motion);
     nh.param("controller/force_reinit_num_steps", _force_reinit_num_steps, _force_reinit_num_steps);
 
-    // 自定义反馈
+    // custom feedback:
     nh.param("controller/prefer_x_feedback", _prefer_x_feedback, _prefer_x_feedback);
     _x_feedback_sub = nh.subscribe("state_feedback", 1, &Controller::stateFeedbackCallback, this);
 
-    // 结果发布
+    // result publisher:
     _ocp_result_pub = nh.advertise<mpc_local_planner_msgs::OptimalControlResult>("ocp_result", 100);
     nh.param("controller/publish_ocp_results", _publish_ocp_results, _publish_ocp_results);
     nh.param("controller/print_cpu_time", _print_cpu_time, _print_cpu_time);
 
-    setAutoUpdatePreviousControl(false);  // 我们希望手动更新之前的控制值
+    setAutoUpdatePreviousControl(false);  // we want to update the previous control value manually
 
     if (_ocp->initialize())
         ROS_INFO("OCP initialized.");
@@ -111,30 +111,14 @@ bool Controller::step(const Controller::PoseSE2& start, const Controller::PoseSE
     return step(initial_plan, vel, dt, t, u_seq, x_seq);
 }
 
-/**
- * @brief 执行控制器的一步
- * 
- * 该函数根据初始计划、当前速度、时间步长和当前时间执行控制器的一步操作，生成控制输入序列和状态序列。
- * 
- * @param initial_plan 初始路径规划，由多个几何消息PoseStamped组成
- * @param vel 当前速度（Twist）
- * @param dt 时间步长（秒）
- * @param t 当前时间（ROS时间）
- * @param u_seq 输出控制输入序列（corbo::TimeSeries::Ptr）
- * @param x_seq 输出状态序列（corbo::TimeSeries::Ptr）
- * @return true 如果步骤成功执行
- * @return false 如果步骤执行失败
- */
 bool Controller::step(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t,
                       corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq)
 {
-    // 检查控制器配置是否完整
     if (!_dynamics || !_grid || !_structured_ocp)
     {
         ROS_ERROR("Controller must be configured before invoking step().");
         return false;
     }
-    // 检查初始计划至少包含两个姿态
     if (initial_plan.size() < 2)
     {
         ROS_ERROR("Controller::step(): initial plan must contain at least two poses.");
@@ -144,11 +128,10 @@ bool Controller::step(const std::vector<geometry_msgs::PoseStamped>& initial_pla
     PoseSE2 start(initial_plan.front().pose);
     PoseSE2 goal(initial_plan.back().pose);
 
-    // 获取目标姿态的稳定状态
     Eigen::VectorXd xf(_dynamics->getStateDimension());
     _dynamics->getSteadyStateFromPoseSE2(goal, xf);
 
-    // 检索或估计当前状态
+    // retrieve or estimate current state
     Eigen::VectorXd x(_dynamics->getStateDimension());
     // check for new measurements
     bool new_x = false;
@@ -157,28 +140,28 @@ bool Controller::step(const std::vector<geometry_msgs::PoseStamped>& initial_pla
         new_x = _recent_x_feedback.size() > 0 && (t - _recent_x_time).toSec() < 2.0 * dt;
         if (new_x) x = _recent_x_feedback;
     }
-    if (!new_x && (!_x_ts || _x_ts->isEmpty() || !_x_ts->getValuesInterpolate(dt, x)))  // 使用之前的状态序列预测
+    if (!new_x && (!_x_ts || _x_ts->isEmpty() || !_x_ts->getValuesInterpolate(dt, x)))  // predict with previous state sequence
     {
-        _dynamics->getSteadyStateFromPoseSE2(start, x);  // 否则初始化稳定状态
+        _dynamics->getSteadyStateFromPoseSE2(start, x);  // otherwise initialize steady state
     }
     if (!new_x || !_prefer_x_feedback)
     {
-        // 如果需要，将状态反馈与里程反馈合并
+        // Merge state feedback with odometry feedback if desired.
         // Note, some models like unicycle overwrite the full state by odom feedback unless _prefer_x_measurement is set to true.
         _dynamics->mergeStateFeedbackAndOdomFeedback(start, vel, x);
     }
 
-    // 检查目标
+    // now check goal
     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())
     {
-         // 基于初始计划生成自定义初始化
+        // generate custom initialization based on initial_plan
         // check if the goal is behind the start pose (w.r.t. start orientation)
         bool backward = _guess_backwards_motion && (goal.position() - start.position()).dot(start.orientationUnitVec()) < 0;
         generateInitialStateTrajectory(x, xf, initial_plan, backward);
@@ -186,12 +169,11 @@ bool Controller::step(const std::vector<geometry_msgs::PoseStamped>& initial_pla
     corbo::Time time(t.toSec());
     _x_seq_init.setTimeFromStart(time);
 
-    corbo::StaticReference xref(xf);  // 目前，我们仅支持在ros中进行点到点的转换
+    corbo::StaticReference xref(xf);  // currently, we only support point-to-point transitions in ros
     corbo::ZeroReference uref(_dynamics->getInputDimension());
 
-    // 执行预测控制器的一步
     _ocp_successful = PredictiveController::step(x, xref, uref, corbo::Duration(dt), time, u_seq, x_seq, nullptr, nullptr, &_x_seq_init);
-    // 如果需要，发布结果
+    // publish results if desired
     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;
@@ -243,45 +225,34 @@ void Controller::publishOptimalControlResult()
 
 void Controller::reset() { PredictiveController::reset(); }
 
-/**
- * @brief 配置离散网格
- * 
- * 该函数根据参数服务器中的配置，选择并创建适当的离散网格。
- * 
- * @param nh ROS节点句柄，用于获取参数
- * @return corbo::DiscretizationGridInterface::Ptr 返回创建的离散网格指针
- */
 corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::NodeHandle& nh)
 {
-    // 如果动力学模型未配置，返回空指针
     if (!_dynamics) return {};
 
-    // 默认网格类型为 "fd_grid"
     std::string grid_type = "fd_grid";
     nh.param("grid/type", grid_type, grid_type);
 
     if (grid_type == "fd_grid")
     {
         FiniteDifferencesGridSE2::Ptr grid;
-        // 配置可变网格
+
         bool variable_grid = true;
         nh.param("grid/variable_grid/enable", variable_grid, variable_grid);
         if (variable_grid)
         {
             FiniteDifferencesVariableGridSE2::Ptr var_grid = std::make_shared<FiniteDifferencesVariableGridSE2>();
-            // 从参数服务器中获取最小时间步长和最大时间步长
+
             double min_dt = 0.0;
             nh.param("grid/variable_grid/min_dt", min_dt, min_dt);
             double max_dt = 10.0;
             nh.param("grid/variable_grid/max_dt", max_dt, max_dt);
             var_grid->setDtBounds(min_dt, max_dt);
-            // 从参数服务器中获取是否启用网格适应，默认值为 true
+
             bool grid_adaptation = true;
             nh.param("grid/variable_grid/grid_adaptation/enable", grid_adaptation, grid_adaptation);
 
             if (grid_adaptation)
             {
-                // 从参数服务器中获取最大网格大小、时间步长滞后比率和最小网格大小
                 int max_grid_size = 50;
                 nh.param("grid/variable_grid/grid_adaptation/max_grid_size", max_grid_size, max_grid_size);
                 double dt_hyst_ratio = 0.1;
@@ -302,17 +273,15 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod
         {
             grid = std::make_shared<FiniteDifferencesGridSE2>();
         }
-
-        // 公共网格参数
-        // 从参数服务器中获取网格参考大小，默认值为 20
+        // common grid parameters
         int grid_size_ref = 20;
         nh.param("grid/grid_size_ref", grid_size_ref, grid_size_ref);
         grid->setNRef(grid_size_ref);
-        // 从参数服务器中获取时间步长参考值，默认值为 0.3
+
         double dt_ref = 0.3;
         nh.param("grid/dt_ref", dt_ref, dt_ref);
         grid->setDtRef(dt_ref);
-        // 从参数服务器中获取状态固定向量，默认值为 {true, true, true}
+
         std::vector<bool> xf_fixed = {true, true, true};
         nh.param("grid/xf_fixed", xf_fixed, xf_fixed);
         if ((int)xf_fixed.size() != _dynamics->getStateDimension())
@@ -324,11 +293,11 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod
         Eigen::Matrix<bool, -1, 1> xf_fixed_eigen(xf_fixed.size());  // we cannot use Eigen::Map as vector<bool> does not provide raw access
         for (int i = 0; i < (int)xf_fixed.size(); ++i) xf_fixed_eigen[i] = xf_fixed[i];
         grid->setXfFixed(xf_fixed_eigen);
-        // 从参数服务器中获取是否启用热启动，默认值为 true
+
         bool warm_start = true;
         nh.param("grid/warm_start", warm_start, warm_start);
         grid->setWarmStart(warm_start);
-        // 从参数服务器中获取配点方法，默认值为 "forward_differences"
+
         std::string collocation_method = "forward_differences";
         nh.param("grid/collocation_method", collocation_method, collocation_method);
 
@@ -349,7 +318,6 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod
             ROS_ERROR_STREAM("Unknown collocation method '" << collocation_method << "' specified. Falling back to default...");
         }
 
-        // 从参数服务器中获取代价积分方法，默认值为 "left_sum"
         std::string cost_integration_method = "left_sum";
         nh.param("grid/cost_integration_method", cost_integration_method, cost_integration_method);
 
@@ -376,33 +344,21 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod
     return {};
 }
 
-/**
- * @brief 配置机器人动力学模型
- * 
- * 该函数根据参数服务器中的配置，选择并创建适当的机器人动力学模型。
- * 
- * @param nh ROS节点句柄，用于获取参数
- * @return RobotDynamicsInterface::Ptr 返回创建的机器人动力学模型指针
- */
 RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHandle& nh)
 {
-    // 默认机器人类型为 "unicycle"（单轮车模型）
     _robot_type = "unicycle";
     nh.param("robot/type", _robot_type, _robot_type);
 
     if (_robot_type == "unicycle")
     {
-        // 如果机器人类型为 "unicycle"，返回 UnicycleModel 实例
         return std::make_shared<UnicycleModel>();
     }
     else if (_robot_type == "simple_car")
     {
-        // 如果机器人类型为 "simple_car"（简单汽车模型）
         double wheelbase = 0.5;
         nh.param("robot/simple_car/wheelbase", wheelbase, wheelbase);
         bool front_wheel_driving = false;
         nh.param("robot/simple_car/front_wheel_driving", front_wheel_driving, front_wheel_driving);
-        // 如果前轮驱动，返回 SimpleCarFrontWheelDrivingModel 实例
         if (front_wheel_driving)
             return std::make_shared<SimpleCarFrontWheelDrivingModel>(wheelbase);
         else
@@ -410,7 +366,6 @@ RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHa
     }
     else if (_robot_type == "kinematic_bicycle_vel_input")
     {
-        // 如果机器人类型为 "kinematic_bicycle_vel_input"（运动学自行车模型，速度输入）
         double length_rear = 1.0;
         nh.param("robot/kinematic_bicycle_vel_input/length_rear", length_rear, length_rear);
         double length_front = 1.0;
@@ -419,44 +374,32 @@ RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHa
     }
     else
     {
-        // 未知的机器人类型，输出错误信息
         ROS_ERROR_STREAM("Unknown robot type '" << _robot_type << "' specified.");
     }
 
     return {};
 }
 
-/**
- * @brief 配置非线性规划（NLP）求解器
- * 
- * 该函数根据参数服务器中的配置，选择并创建适当的NLP求解器。
- * 
- * @param nh ROS节点句柄，用于获取参数
- * @return corbo::NlpSolverInterface::Ptr 返回创建的NLP求解器指针
- */
 corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle& nh)
 {
-    // 从参数服务器中获取求解器类型，默认值为 "ipopt"
+
     std::string solver_type = "ipopt";
     nh.param("solver/type", solver_type, solver_type);
 
     if (solver_type == "ipopt")
     {
-        // 配置Ipopt求解器
         corbo::SolverIpopt::Ptr solver = std::make_shared<corbo::SolverIpopt>();
-        solver->initialize();  // 初始化以便之后设置参数
+        solver->initialize();  // requried for setting parameters afterward
 
-        // 从参数服务器中获取Ipopt求解器的迭代次数，默认值为100
         int iterations = 100;
         nh.param("solver/ipopt/iterations", iterations, iterations);
         solver->setIterations(iterations);
 
-        // 从参数服务器中获取Ipopt求解器的最大CPU时间，默认值为-1.0
         double max_cpu_time = -1.0;
         nh.param("solver/ipopt/max_cpu_time", max_cpu_time, max_cpu_time);
         solver->setMaxCpuTime(max_cpu_time);
 
-        // 设置其他Ipopt选项
+        // now check for additional ipopt options
         std::map<std::string, double> numeric_options;
         nh.param("solver/ipopt/ipopt_numeric_options", numeric_options, numeric_options);
         for (const auto& item : numeric_options)
@@ -498,15 +441,12 @@ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle
     //    }
     else if (solver_type == "lsq_lm")
     {
-        // 配置Levenberg-Marquardt求解器
         corbo::LevenbergMarquardtSparse::Ptr solver = std::make_shared<corbo::LevenbergMarquardtSparse>();
 
-        // 从参数服务器中获取Levenberg-Marquardt求解器的迭代次数，默认值为10
         int iterations = 10;
         nh.param("solver/lsq_lm/iterations", iterations, iterations);
         solver->setIterations(iterations);
 
-        // 从参数服务器中获取初始惩罚权重
         double weight_init_eq = 2;
         nh.param("solver/lsq_lm/weight_init_eq", weight_init_eq, weight_init_eq);
         double weight_init_ineq = 2;
@@ -516,7 +456,6 @@ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle
 
         solver->setPenaltyWeights(weight_init_eq, weight_init_ineq, weight_init_bounds);
 
-        // 从参数服务器中获取权重适应因子
         double weight_adapt_factor_eq = 1;
         nh.param("solver/lsq_lm/weight_adapt_factor_eq", weight_adapt_factor_eq, weight_adapt_factor_eq);
         double weight_adapt_factor_ineq = 1;
@@ -524,7 +463,6 @@ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle
         double weight_adapt_factor_bounds = 1;
         nh.param("solver/lsq_lm/weight_adapt_factor_bounds", weight_adapt_factor_bounds, weight_adapt_factor_bounds);
 
-        // 从参数服务器中获取最大权重适应值
         double weight_adapt_max_eq = 500;
         nh.param("solver/lsq_lm/weight_adapt_max_eq", weight_adapt_max_eq, weight_adapt_max_eq);
         double weight_adapt_max_ineq = 500;
@@ -545,30 +483,17 @@ corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle
     return {};
 }
 
-/**
- * @brief 配置结构化最优控制问题（OCP）
- * 
- * 该函数根据参数服务器中的配置，选择并创建适当的OCP。
- * 
- * @param nh ROS节点句柄，用于获取参数
- * @param obstacles 障碍物容器
- * @param robot_model 机器人轮廓模型
- * @param via_points 中间目标点
- * @return corbo::StructuredOptimalControlProblem::Ptr 返回创建的OCP指针
- */
 corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles,
                                                                      teb_local_planner::RobotFootprintModelPtr robot_model,
                                                                      const std::vector<teb_local_planner::PoseSE2>& via_points)
 {
-    // 创建超图优化问题
     corbo::BaseHyperGraphOptimizationProblem::Ptr hg = std::make_shared<corbo::HyperGraphOptimizationProblemEdgeBased>();
-    // 创建结构化最优控制问题实例
+
     corbo::StructuredOptimalControlProblem::Ptr ocp = std::make_shared<corbo::StructuredOptimalControlProblem>(_grid, _dynamics, hg, _solver);
 
-    const int x_dim = _dynamics->getStateDimension();   // 状态维度
-    const int u_dim = _dynamics->getInputDimension();   // 控制输入维度
+    const int x_dim = _dynamics->getStateDimension();
+    const int u_dim = _dynamics->getInputDimension();
 
-    // 根据机器人类型配置控制输入的上下界
     if (_robot_type == "unicycle")
     {
         double max_vel_x = 0.4;
@@ -626,7 +551,6 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::
         return {};
     }
 
-    // 配置阶段成本（stage cost）
     std::string objective_type = "minimum_time";
     nh.param("planning/objective/type", objective_type, objective_type);
     bool lsq_solver = _solver->isLsqSolver();
@@ -719,7 +643,6 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::
         return {};
     }
 
-    // 配置终端成本（terminal cost）
     std::string terminal_cost = "none";
     nh.param("planning/terminal_cost/type", terminal_cost, terminal_cost);
 
@@ -753,7 +676,6 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::
         return {};
     }
 
-    // 配置终端约束（terminal constraint）
     std::string terminal_constraint = "none";
     nh.param("planning/terminal_constraint/type", terminal_constraint, terminal_constraint);
 
@@ -789,28 +711,28 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::
         return {};
     }
 
-    // 配置阶段不等式约束（stage inequality constraint）
     _inequality_constraint = std::make_shared<StageInequalitySE2>();
     _inequality_constraint->setObstacleVector(obstacles);
     _inequality_constraint->setRobotFootprintModel(robot_model);
 
-    // 配置碰撞避免参数
+    // configure collision avoidance
+
     double min_obstacle_dist = 0.5;
-    nh.param("collision/min_obstacle_dist", min_obstacle_dist, min_obstacle_dist);
+    nh.param("collision_avoidance/min_obstacle_dist", min_obstacle_dist, min_obstacle_dist);
     _inequality_constraint->setMinimumDistance(min_obstacle_dist);
 
     bool enable_dynamic_obstacles = false;
-    nh.param("collision/enable_dynamic_obstacles", enable_dynamic_obstacles, enable_dynamic_obstacles);
+    nh.param("collision_avoidance/enable_dynamic_obstacles", enable_dynamic_obstacles, enable_dynamic_obstacles);
     _inequality_constraint->setEnableDynamicObstacles(enable_dynamic_obstacles);
 
     double force_inclusion_dist = 0.5;
-    nh.param("collision/force_inclusion_dist", force_inclusion_dist, force_inclusion_dist);
+    nh.param("collision_avoidance/force_inclusion_dist", force_inclusion_dist, force_inclusion_dist);
     double cutoff_dist = 2;
-    nh.param("collision/cutoff_dist", cutoff_dist, cutoff_dist);
+    nh.param("collision_avoidance/cutoff_dist", cutoff_dist, cutoff_dist);
     _inequality_constraint->setObstacleFilterParameters(force_inclusion_dist, cutoff_dist);
 
     // configure control deviation bounds
-    
+
     if (_robot_type == "unicycle")
     {
         double acc_lim_x = 0.0;
@@ -957,6 +879,7 @@ bool Controller::isPoseTrajectoryFeasible(base_local_planner::CostmapModel* cost
         ROS_ERROR("isPoseTrajectoriyFeasible is currently only implemented for fd grids");
         return true;
     }
+    ROS_INFO(" look_ahead_idx:%d",look_ahead_idx);
     if (look_ahead_idx < 0 || look_ahead_idx >= _grid->getN()) look_ahead_idx = _grid->getN() - 1;
 
     for (int i = 0; i <= look_ahead_idx; ++i)

mpc_local_planner/src/mpc_local_planner_ros.cpp

@@ -351,7 +351,7 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
     _robot_vel.linear.y  = robot_vel_tf.pose.position.y;
     _robot_vel.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation);
 
-    // 修剪全局计划，剪掉机器人之前的部分，从global的头部开始查找，直到距离机器人小于global_plan_prune_distance
+    // 修剪全局计划，剪掉机器人之前的部分
     pruneGlobalPlan(*_tf, robot_pose, _global_plan, _params.global_plan_prune_distance);
 
     // Convert To Local frame
@@ -366,9 +366,9 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
         return mbf_msgs::ExePathResult::INTERNAL_ERROR;
     }
 
-    // 更新路径点容器，根据global_plan_viapoint_sep间隔距离，向_viapoint中插入路径点
+    // 更新路径点容器
     if (!_custom_via_points_active) updateViaPointsContainer(transformed_plan, _params.global_plan_viapoint_sep);
-   
+
     // 检查是否到达全局目标
     geometry_msgs::PoseStamped global_goal;
     tf2::doTransform(_global_plan.back(), global_goal, tf_plan_to_global);
@@ -561,7 +561,6 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCostmap()
 
                     // 检查障碍物是否在机器人前方（例如，不远离机器人）
                     Eigen::Vector2d obs_dir = obs - _robot_pose.position();
-                    // 如果障礙物在機器人後方，而且距離大於參數設置的距離，則忽略
                     if (obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > _params.costmap_obstacles_behind_robot_dist) continue;
 
                     _obstacles.push_back(ObstaclePtr(new PointObstacle(obs)));
@@ -705,7 +704,7 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles()
  * 该函数根据转换后的计划和最小间隔距离更新路径点容器。
  *
  * @param transformed_plan 转换后的全局计划。
- * @param min_separation 路径点之间的最小间隔距离。默认为-1,表示不使用路径点。
+ * @param min_separation 路径点之间的最小间隔距离。
  */
 void MpcLocalPlannerROS::updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation)
 {
@@ -848,7 +847,7 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st
         double sq_dist_threshold = dist_threshold * dist_threshold;
         double sq_dist           = 1e10;
 
-        // 我们需要循环直到找到一个距离机器人最近的路径点
+        // 我们需要循环直到找到一个距离机器人一定距离内的计划点
         for (int j = 0; j < (int)global_plan.size(); ++j)
         {
             double x_diff      = robot_pose.pose.position.x - global_plan[j].pose.position.x;
@@ -867,7 +866,7 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st
 
         double plan_length = 0;  // 检查沿计划的累计欧几里得距离
 
-        // 现在我们将转换，直到点超出我们的距离阈值，从距离机器人的最近的点开始，直到距离满足max_plan_length
+        // 现在我们将转换，直到点超出我们的距离阈值
         while (i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length <= 0 || plan_length <= max_plan_length))
         {
             const geometry_msgs::PoseStamped& pose = global_plan[i];

mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml

@@ -31,13 +31,13 @@ MpcLocalPlannerROS:
 
   ## Collision avoidance
   collision:
-    min_obstacle_dist: 0.25 # Note, this parameter must be chosen w.r.t. the footprint_model  於障礙物的最小距離
-    enable_dynamic_obstacles: False           # 是否启用动态障碍物
-    force_inclusion_dist: 0.5                 # 強制包含障礙物的距離
-    cutoff_dist: 2.5                          # 截止距离，不考虑超过这个距离的障碍物
+    min_obstacle_dist: 0.27 # Note, this parameter must be chosen w.r.t. the footprint_model
+    enable_dynamic_obstacles: False
+    force_inclusion_dist: 0.5
+    cutoff_dist: 2.5 
     include_costmap_obstacles: True
-    costmap_obstacles_behind_robot_dist: 1.0  # 机器人后面的障碍物距离
-    collision_check_no_poses: 5               # 检查是否會碰撞的位資數量
+    costmap_obstacles_behind_robot_dist: 1.0
+    collision_check_no_poses: 5
     
 
   ## Planning grid 
@@ -73,13 +73,12 @@ MpcLocalPlannerROS:
     outer_ocp_iterations: 1
     xy_goal_tolerance: 0.2
     yaw_goal_tolerance: 0.1
-    global_plan_overwrite_orientation: True #
-    global_plan_prune_distance: 1.0         # 全局查找从第一个点开始查找，直到找到一个距离当前位置1m的点
+    global_plan_overwrite_orientation: True
+    global_plan_prune_distance: 1.0
     allow_init_with_backward_motion: True
-    max_global_plan_lookahead_dist: 1.5     # 从距离机器人当前最近的点开始查找，直到距离满足1.5m
+    max_global_plan_lookahead_dist: 1.5 
     force_reinit_new_goal_dist: 1.0
     force_reinit_new_goal_angular: 1.57
-    global_plan_viapoint_sep: 0.1           # 从全局路径中提取的点之间的最小距离
     prefer_x_feedback: False
     publish_ocp_results: False