boost::mutex::scoped_lock lock(mutex_); if (!initialized_) { ROS_ERROR( "This planner has not been initialized yet, but it is being used, please call initialize() before use"); returnfalse; } plan.clear();
getPlanFromPotential涉及到Traceback::virtual bool getPath，这是一个纯虚函数，实现有两种: class GradientPath : public Traceback(梯度路径), class GridPath : public Traceback(栅格路径)
if(oscillation_timeout_ > 0.0 && ros::Time::now() - last_oscillation_reset_ > ros::Duration(oscillation_timeout_) ) { publishZeroVelocity(); state_ = CLEARING; recovery_trigger_ = OSCILLATION_R; movebase_log.error("oscillate in a small area for a long time !"); }
//if our last recovery was caused by oscillation, we want to reset the recovery index if(recovery_trigger_ == OSCILLATION_R) recovery_index_ = 0; }
最后彻底失败，会abort:
1 2 3 4 5
elseif(recovery_trigger_ == OSCILLATION_R) { movebase_log.error("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors"); as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors."); }
[10:21:19.968 - INFO] (): robot is oscillating, and also recently_oscillated
# move_base CONTROLLING [10:21:20.262 - ERROR] (): move_base CONTROLLING: oscillate in a small area for a long time ! # 进入CLEARING状态，OSCILLATION_R，准备进入第1个恢复行为 [10:21:20.262 - INFO] (): robot is oscillating, and also recently_oscillated
# 执行第1个恢复行为，结束后，状态变为PLANNING [10:21:20.301 - INFO] (): move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2 [10:21:20.301 - WARN] (): Clearing costmap to unstuck robot (3.000000m).
[10:21:20.628 - INFO] (): robot is oscillating, and also recently_oscillated [10:21:33.657 - INFO] (): robot is oscillating, and also recently_oscillated [10:21:33.724 - INFO] (): robot is oscillating, and also recently_oscillated
# 第一个恢复行为失败，仍然进入CLEARING状态, OSCILLATION_R [10:21:33.846 - ERROR] (): move_base CONTROLLING: oscillate in a small area for a long time !
[10:21:33.848 - INFO] (): robot is oscillating, and also recently_oscillated
# 执行第2个恢复行为 [10:21:33.900 - INFO] (): move_base_recovery,state machine is CLEARING, Executing behavior 1 of 2 [10:21:33.900 - WARN] (): Clearing costmap to unstuck robot (0.000000m).
[10:21:37.559 - INFO] (): robot is oscillating, and also recently_oscillated [10:21:37.617 - INFO] (): robot is oscillating, and also recently_oscillated [10:21:37.805 - INFO] (): robot is oscillating, and also recently_oscillated [10:21:37.863 - INFO] (): robot is oscillating, and also recently_oscillated
# 省略一段时间
[10:21:48.917 - ERROR] (): move_base CONTROLLING: oscillate in a small area for a long time ! # 两个恢复行为都失败，状态又变为CLEARING OSCILLATION_R [10:21:48.919 - INFO] (): robot is oscillating, and also recently_oscillated
# 彻底失败，Abort [10:21:49.103 - ERROR] (): Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors
/** * Edge defining the cost function for keeping a minimum distance from obstacles. * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. * \e dist2point denotes the minimum distance to the point obstacle. * \e weight can be set using setInformation(). \n * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle * @remarks Do not forget to call setTebConfig() and setObstacle() */ classEdgeObstacle :public BaseTebUnaryEdge<1, const Obstacle*, VertexPose> { public: EdgeObstacle() { _measurement = NULL; } /** * @brief Set pointer to associated obstacle for the underlying cost function * @param obstacle 2D position vector containing the position of the obstacle */ voidsetObstacle(const Obstacle* obstacle) { _measurement = obstacle; } /** * @brief Set pointer to the robot model * @param robot_model Robot model required for distance calculation */ voidsetRobotModel(const BaseRobotFootprintModel* robot_model) { robot_model_ = robot_model; } /** * @brief Set all parameters at once * @param cfg TebConfig class * @param robot_model Robot model required for distance calculation * @param obstacle 2D position vector containing the position of the obstacle */ voidsetParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) { cfg_ = &cfg; robot_model_ = robot_model; _measurement = obstacle; } // cost function voidcomputeError() { ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()" ); const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]); /* inline double penaltyBoundFromBelow( const double& var, const double& a, const double& epsilon) { if (var >= a+epsilon) return 0.; else return (-var + (a+epsilon)); } */ double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
// Original obstacle cost _error[0] = penaltyBoundFromBelow( dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon ); if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0) { // Optional non-linear cost. Note the max cost (before weighting) is // the same as the straight line version and that all other costs are // below the straight line (for positive exponent), so it may be // necessary to increase weight_obstacle and/or the inflation_weight // when using larger exponents _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow( _error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent); } ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n", _error[0]); } protected: const BaseRobotFootprintModel* robot_model_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW };
auto iter_obstacle = obstacles_per_vertex_.begin();
auto create_edge = [inflated, &information, &information_inflated, this] (int index, const Obstacle* obstacle) { if (inflated) { // 一元边，观测值维度为2，类型为Obstacle基类，连接VertexPose顶点 EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle; dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index)); // 信息矩阵为2x2对角阵，(0,0) = weight_obstacle, (1,1) = weight_inflation dist_bandpt_obst->setInformation(information_inflated); dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obstacle); optimizer_->addEdge(dist_bandpt_obst); } else { // 一元边，观测值维度为1，测量值类型为Obstacle基类，连接VertexPose顶点 EdgeObstacle* dist_bandpt_obst = new EdgeObstacle; dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index)); // 信息矩阵为 1x1 weight_obstacle * weight_multiplier dist_bandpt_obst->setInformation(information); dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obstacle); optimizer_->addEdge(dist_bandpt_obst); }; }; /* iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too*/ constint first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0; for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i) { double left_min_dist = std::numeric_limits<double>::max(); double right_min_dist = std::numeric_limits<double>::max(); ObstaclePtr left_obstacle; ObstaclePtr right_obstacle; const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec(); // iterate obstacles for (const ObstaclePtr& obst : *obstacles_) { // we handle dynamic obstacles differently below if(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic()) continue;
// calculate distance to robot model double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get()); // force considering obstacle if really close to the current pose if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor) { iter_obstacle->push_back(obst); continue; } // cut-off distance if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor) continue; // determine side (left or right) and assign obstacle if closer than the previous one if (cross2d(pose_orient, obst->getCentroid()) > 0) // left { if (dist < left_min_dist) { left_min_dist = dist; left_obstacle = obst; } } else { if (dist < right_min_dist) { right_min_dist = dist; right_obstacle = obst; } } } // create obstacle edges if (left_obstacle) iter_obstacle->push_back(left_obstacle); if (right_obstacle) iter_obstacle->push_back(right_obstacle);
// continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges if (i == 0) { ++iter_obstacle; continue; } // create obstacle edges for (const ObstaclePtr obst : *iter_obstacle) create_edge(i, obst.get()); ++iter_obstacle; } }
# Original "straight line" obstacle cost. The max possible value # before weighting is min_obstacle_dist，和 EdgeObstacle的相同 error[0] = (dist > min_obstacle_dist+ε) ? 0 : (min_obstacle_dist+ε) - dist error[1] = (dist > inflation_dist) ? 0 : (inflation_dist) - dist
但是这两个类的computeError在error[0]赋值结束后又补充这样一段:
1 2 3 4 5 6 7 8 9 10
if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0) { /* Optional 非线性代价. Note the max cost (before weighting) is the same as the straight line version and that all other costs are below the straight line (for positive exponent), so it may be necessary to increase weight_obstacle and/or the inflation_weight when using larger exponents */ _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow( _error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent); }
// 对汽车机器人，convert rot-vel to steering angle if desired // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself. if (cfg_.robot.cmd_angle_instead_rotvel) { // 目前是false，省略 } // a feasible solution should be found, reset counter no_infeasible_plans_ = 0; // 保存上一个速度命令 (for recovery analysis etc.) last_cmd_ = cmd_vel;
// Get the velocity command for this sampling interval if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.trajectory.control_look_ahead_poses, robot_vel_.linear.x, robot_vel_.angular.z) ) { planner_->clearPlanner(); ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner..."); ++no_infeasible_plans_; // increase number of infeasible solutions in a row time_last_infeasible_plan_ = ros::Time::now(); last_cmd_ = cmd_vel; returnfalse; }
// we do not reduce if the goal is already selected , because the orientation // might change -> can introduce oscillations // no_infeasible_plans_ 改成 no_feasible_plans_ 比较合适 if (cfg_.recovery.shrink_horizon_backup && goal_idx < (int)transformed_plan.size()-1 && (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).toSec() < // keep short horizon for at least a few seconds cfg_.recovery.shrink_horizon_min_duration ) ) { ROS_INFO_COND(no_infeasible_plans_==1, "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_.recovery.shrink_horizon_min_duration);
// Shorten horizon if requested reduce to 50 percent: int horizon_reduction = goal_idx/2; if (no_infeasible_plans_ > 9) { ROS_INFO_COND(no_infeasible_plans_==10, "Infeasible trajectory detected 10 times in a row: further reducing horizon..."); horizon_reduction /= 2; } // we have a small overhead (过头) here, since we already transformed 50% // more of the trajectory. But that's ok for now, since we do not // need to make transformGlobalPlan more complex // reduced horizon 应当很少发生 int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1; goal_idx -= horizon_reduction;
// 从 transformed_plan 的 new_goal_idx 开始，一直删除到尾部 if (new_goal_idx_transformed_plan >0 && goal_idx >= 0) transformed_plan.erase(transformed_plan.begin() + new_goal_idx_transformed_plan, transformed_plan.end()); else goal_idx += horizon_reduction; // this should not happen, but safety first } // ......第二部分...... }
shrink_horizon_backup: true. 在自动检测到问题(plan not feasible)的情况下，允许planner临时缩小范围(50%). TEB将以更近的点作为规划目标，尝试重新规划出可行路径。调试时可关闭，以在可视化界面上观察原出错路径。另请参阅参数shrink_horizon_min_duration
failure_detector_.update(last_cmd_, cfg_.robot.max_vel_x, cfg_.robot.max_vel_x_backwards, max_vel_theta, cfg_.recovery.oscillation_v_eps, cfg_.recovery.oscillation_omega_eps); bool oscillating = failure_detector_.isOscillating(); // 距离上次振荡过了多久，若小于配置值(10秒)，说明最近也振荡了 bool recently_oscillated = (ros::Time::now()-time_last_oscillation_).toSec() < cfg_.recovery.oscillation_recovery_min_duration; // 如果现在是振荡状态 if (oscillating) { if (!recently_oscillated) // 最近的状态不是振荡 { // save current turning direction // 以下其实都是对 last_preferred_rotdir_ 赋值 if (robot_vel_.angular.z > 0) last_preferred_rotdir_ = RotType::left; else last_preferred_rotdir_ = RotType::right; ROS_WARN("TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization) "); } time_last_oscillation_ = ros::Time::now(); // 对 prefer_rotdir_ 赋值，最终影响 AddEdgesPreferRotDir planner_->setPreferredTurningDir(last_preferred_rotdir_); } // 现在和最近都没有振荡 clear recovery behavior elseif (!recently_oscillated && last_preferred_rotdir_ != RotType::none) { last_preferred_rotdir_ = RotType::none; planner_->setPreferredTurningDir(last_preferred_rotdir_); ROS_INFO("TebLocalPlannerROS: oscillation recovery disabled/expired."); } }
但是如果机器人确实stucked，最终会在move_base里Abort
setPreferredTurningDir
设置期望的初始转弯方向 (也就是惩罚相反的方向)。A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two solutions (in the same equivalence class!) with similar cost. 配置参数以调整惩罚的权重，Initial 说明惩罚只适用于trajectory的前几个位姿
voidHomotopyClassPlanner::setPreferredTurningDir(RotType dir) { // set preferred turning dir for all TEBs for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) { (*it_teb)->setPreferredTurningDir(dir); } }
voidTebOptimalPlanner::AddEdgesPreferRotDir() { /* roesmann 注释: these edges can result in odd predictions, in particular we can observe a substantional mismatch between open- and closed-loop planning leading to a poor control performance. 目前这个功能只用于振荡恢复，短期使用这个约束不会有重大影响 可能让机器人脱离振荡 */ if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir == 0) return; // 必须定义好转向 if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left) { ROS_WARN("TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation."); return; } // create edge for satisfiying kinematic constraints Eigen::Matrix<double,1,1> information_rotdir; information_rotdir.fill(cfg_->optim.weight_prefer_rotdir); // 目前只使用前三个旋转 for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i) { EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir; rotdir_edge->setVertex(0, teb_.PoseVertex(i)); rotdir_edge->setVertex(1, teb_.PoseVertex(i+1)); rotdir_edge->setInformation(information_rotdir); if (prefer_rotdir_ == RotType::left) rotdir_edge->preferLeft(); elseif (prefer_rotdir_ == RotType::right) rotdir_edge->preferRight(); optimizer_->addEdge(rotdir_edge); } }
// Set buffer length (measurement history) // length: number of measurements to be kept voidsetBufferLength(int length){ buffer_.set_capacity(length); }
voidTebOptimalPlanner::clearGraph() { // clear optimizer states if (optimizer_) { // neccessary, because optimizer->clear deletes pointer-targets // therefore it deletes TEB states optimizer_->vertices().clear(); optimizer_->clear(); } }
