/* called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds. */ virtualvoidupdateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y){}
/** Actually update the underlying costmap, only within the bounds * calculated during UpdateBounds(). */ virtualvoidupdateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){}
/* Stop publishers. */ virtualvoiddeactivate(){} /* Restart publishers if they've been stopped. */ virtualvoidactivate(){} virtualvoidreset(){} /* make this layer match the size of the parent costmap. */ virtualvoidmatchSize(){} /** LayeredCostmap calls this whenever the footprint there * changes (via LayeredCostmap::setFootprint()). Override to be notified of changes to the robot's footprint. */ virtualvoidonFootprintChanged(){} virtualvoidonInitialize(){}
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
