feat: refact param namespace

master
邱棚 2024-07-16 15:15:04 +08:00
parent 977eeebf89
commit 26f54ab8a6
2 changed files with 121 additions and 43 deletions

View File

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

View File

@ -351,7 +351,7 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
_robot_vel.linear.y = robot_vel_tf.pose.position.y; _robot_vel.linear.y = robot_vel_tf.pose.position.y;
_robot_vel.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation); _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); pruneGlobalPlan(*_tf, robot_pose, _global_plan, _params.global_plan_prune_distance);
// Convert To Local frame // Convert To Local frame
@ -366,7 +366,7 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt
return mbf_msgs::ExePathResult::INTERNAL_ERROR; return mbf_msgs::ExePathResult::INTERNAL_ERROR;
} }
// 更新路径点容器 // 更新路径点容器根据global_plan_viapoint_sep间隔距离向_viapoint中插入路径点
if (!_custom_via_points_active) updateViaPointsContainer(transformed_plan, _params.global_plan_viapoint_sep); if (!_custom_via_points_active) updateViaPointsContainer(transformed_plan, _params.global_plan_viapoint_sep);
// 检查是否到达全局目标 // 检查是否到达全局目标
@ -561,6 +561,7 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCostmap()
// 检查障碍物是否在机器人前方(例如,不远离机器人) // 检查障碍物是否在机器人前方(例如,不远离机器人)
Eigen::Vector2d obs_dir = obs - _robot_pose.position(); 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; if (obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > _params.costmap_obstacles_behind_robot_dist) continue;
_obstacles.push_back(ObstaclePtr(new PointObstacle(obs))); _obstacles.push_back(ObstaclePtr(new PointObstacle(obs)));
@ -704,7 +705,7 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles()
* *
* *
* @param transformed_plan * @param transformed_plan
* @param min_separation * @param min_separation -1,使
*/ */
void MpcLocalPlannerROS::updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation) void MpcLocalPlannerROS::updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation)
{ {
@ -847,7 +848,7 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st
double sq_dist_threshold = dist_threshold * dist_threshold; double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 1e10; double sq_dist = 1e10;
// 我们需要循环直到找到一个距离机器人一定距离内的计划 // 我们需要循环直到找到一个距离机器人最近的路径
for (int j = 0; j < (int)global_plan.size(); ++j) for (int j = 0; j < (int)global_plan.size(); ++j)
{ {
double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x; double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x;
@ -866,7 +867,7 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st
double plan_length = 0; // 检查沿计划的累计欧几里得距离 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)) 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]; const geometry_msgs::PoseStamped& pose = global_plan[i];