feat: refact param namespace
parent
977eeebf89
commit
26f54ab8a6
|
|
@ -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; // we may need state and control dimensions to check other parameters
|
||||
if (!_dynamics) return false; // 我们可能需要状态和控制维度来检查其他参数
|
||||
//离散网络，比如多重打靶法。参考点，输入，状态，等变量也会存放在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); // we want to update the previous control value manually
|
||||
setAutoUpdatePreviousControl(false); // 我们希望手动更新之前的控制值
|
||||
|
||||
if (_ocp->initialize())
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* @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.");
|
||||
|
|
@ -128,10 +144,11 @@ 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;
|
||||
|
|
@ -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;
|
||||
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)
|
||||
{
|
||||
// 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);
|
||||
|
|
@ -169,11 +186,12 @@ 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); // currently, we only support point-to-point transitions in ros
|
||||
corbo::StaticReference xref(xf); // 目前，我们仅支持在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;
|
||||
|
|
@ -225,34 +243,45 @@ 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;
|
||||
|
|
@ -273,15 +302,17 @@ corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::Nod
|
|||
{
|
||||
grid = std::make_shared<FiniteDifferencesGridSE2>();
|
||||
}
|
||||
// common grid parameters
|
||||
|
||||
// 公共网格参数
|
||||
// 从参数服务器中获取网格参考大小，默认值为 20
|
||||
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())
|
||||
|
|
@ -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
|
||||
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);
|
||||
|
||||
|
|
@ -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...");
|
||||
}
|
||||
|
||||
// 从参数服务器中获取代价积分方法，默认值为 "left_sum"
|
||||
std::string cost_integration_method = "left_sum";
|
||||
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 {};
|
||||
}
|
||||
|
||||
/**
|
||||
* @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
|
||||
|
|
@ -366,6 +410,7 @@ 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;
|
||||
|
|
@ -374,32 +419,44 @@ 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(); // requried for setting parameters afterward
|
||||
solver->initialize(); // 初始化以便之后设置参数
|
||||
|
||||
// 从参数服务器中获取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);
|
||||
|
||||
// now check for additional ipopt options
|
||||
// 设置其他Ipopt选项
|
||||
std::map<std::string, double> numeric_options;
|
||||
nh.param("solver/ipopt/ipopt_numeric_options", numeric_options, 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")
|
||||
{
|
||||
// 配置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;
|
||||
|
|
@ -456,6 +516,7 @@ 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;
|
||||
|
|
@ -463,6 +524,7 @@ 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;
|
||||
|
|
@ -483,17 +545,30 @@ 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;
|
||||
|
|
@ -551,6 +626,7 @@ 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();
|
||||
|
|
@ -643,6 +719,7 @@ 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);
|
||||
|
||||
|
|
@ -676,6 +753,7 @@ 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);
|
||||
|
||||
|
|
@ -711,28 +789,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_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);
|
||||
|
||||
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);
|
||||
|
||||
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;
|
||||
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);
|
||||
|
||||
// configure control deviation bounds
|
||||
|
||||
|
||||
if (_robot_type == "unicycle")
|
||||
{
|
||||
double acc_lim_x = 0.0;
|
||||
|
|
@ -879,7 +957,6 @@ 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)
|
||||
|
|
|
|||
|
|
@ -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,6 +561,7 @@ 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)));
|
||||
|
|
@ -704,7 +705,7 @@ void MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles()
|
|||
* 该函数根据转换后的计划和最小间隔距离更新路径点容器。
|
||||
*
|
||||
* @param transformed_plan 转换后的全局计划。
|
||||
* @param min_separation 路径点之间的最小间隔距离。
|
||||
* @param min_separation 路径点之间的最小间隔距离。默认为-1,表示不使用路径点。
|
||||
*/
|
||||
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 = 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;
|
||||
|
|
@ -866,7 +867,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];
|
||||
|
|
|
|||
Loading…
Reference in New Issue