局部规划器

    private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));//create a local plannertry {tc_ = blp_loader_.createInstance(local_planner);ROS_INFO("Created local_planner %s", local_planner.c_str());tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);} catch (const pluginlib::PluginlibException& ex) {ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());exit(1);}

void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)
{// check if the plugin is already initializedif(!initialized_){	// create Node Handle with name of plugin (as used in move_base for loading)ros::NodeHandle nh("~/" + name);// get parameters of TebConfig via the nodehandle and override the default configcfg_.loadRosParamFromNodeHandle(nh);       // reserve some memory for obstaclesobstacles_.reserve(500);// create visualization instance	visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); 
//获取小车的参数，这些参数将用于模型优化，路径计算，比如阿克曼小车的最小转弯半径        // create robot footprint/contour model for optimizationRobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);
//这儿我们解读TebOptimalPlanner分支    // create the planner instanceif (cfg_.able_homotopy_class_planning){planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));ROS_INFO("Parallel planning in distinctive topologies enabled.");}else{planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));ROS_INFO("Parallel planning in distinctive topologies disabled.");}// init other variablestf_ = tf;costmap_ros_ = costmap_ros;costmap_ = costmap_ros_->getCostmap(); // locking should be done stmap_model_ = boost::make_shared(*costmap_);global_frame_ = costmap_ros_->getGlobalFrameID();cfg_.map_frame = global_frame_; // TODOrobot_base_frame_ = costmap_ros_->getBaseFrameID();//Initialize a costmap to polygon converterif (!cfg_.stmap_pty()){try{costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.stmap_converter_plugin);std::string converter_name = costmap_converter_loader_.getName(cfg_.stmap_converter_plugin);// replace '::' by '/' to convert the c++ namespace to a NodeHandle namespaceboost::replace_all(converter_name, "::", "/");costmap_converter_->setOdomTopic(cfg_.odom_topic);costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name));costmap_converter_->setCostmap2D(costmap_);costmap_converter_->startWorker(ros::Rate(cfg_.stmap_converter_rate), costmap_, cfg_.stmap_converter_spin_thread);ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.stmap_converter_plugin << " loaded.");        }catch(pluginlib::PluginlibException& ex){ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error message: %s", ex.what());costmap_converter_.reset();}}else ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");//根据小车自身形状，计算最小最大距离// Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.footprint_spec_ = costmap_ros_->getRobotFootprint();costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);    // init the odom helper to receive the robot's velocity from odom messagesodom_helper_.setOdomTopic(cfg_.odom_topic);// setup dynamic reconfiguredynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server >(nh);dynamic_reconfigure::Server::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2);dynamic_recfg_->setCallback(cb);// validate optimization footprint and costmap footprintvalidateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_.obstacles.min_obstacle_dist);// setup callback for custom obstaclescustom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);// setup callback for custom via-pointsvia_points_sub_ = nh.subscribe("via_points", 1, &TebLocalPlannerROS::customViaPointsCB, this);// initialize failure detectorros::NodeHandle nh_move_base("~");double controller_frequency = 5;nh_move_base.param("controller_frequency", controller_frequency, controller_frequency);failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency));// set initialized flaginitialized_ = true;ROS_DEBUG("teb_local_planner plugin initialized.");}else{ROS_WARN("teb_local_planner has already been initialized, doing nothing.");}
}

uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose,const geometry_msgs::TwistStamped& velocity,geometry_msgs::TwistStamped &cmd_vel,std::string &message)
{// check if plugin initializedif(!initialized_){ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner");message = "teb_local_planner has not been initialized";return mbf_msgs::ExePathResult::NOT_INITIALIZED;}static uint32_t seq = 0;cmd_vel.header.seq = seq++;cmd_vel.header.stamp = ros::Time::now();cmd_vel.header.frame_id = robot_base_frame_;cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;goal_reached_ = false;  // Get robot pose 获取小车位置geometry_msgs::PoseStamped robot_pose;costmap_ros_->getRobotPose(robot_pose);robot_pose_ = PoseSE2(robot_pose.pose);// Get robot velocity 从里程计中获取小车当前速度geometry_msgs::PoseStamped robot_vel_tf;odom_helper_.getRobotVel(robot_vel_tf);robot_vel_.linear.x = robot_vel_tf.pose.position.x;robot_vel_.linear.y = robot_vel_tf.pose.position.y;robot_vel_.angular.z = tf2::getYaw(robot_vel_ientation);//修剪plan，去掉已经行驶过的路径// prune global plan to cut off parts of the past (spatially before the robot)pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);// Transform global plan to the frame of interest ( the local costmap)std::vector transformed_plan;int goal_idx;geometry_msgs::TransformStamped tf_plan_to_global;if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, transformed_plan, &goal_idx, &tf_plan_to_global)){ROS_WARN("Could not transform the global plan to the frame of the controller");message = "Could not transform the global plan to the frame of the controller";return mbf_msgs::ExePathResult::INTERNAL_ERROR;}// update via-points containerif (!custom_via_points_active_)updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);// check if global goal is reached 检查是否到目的点，包括位置和方向geometry_msgs::PoseStamped global_goal;tf2::doTransform(global_plan_.back(), global_goal, tf_plan_to_global);double dx = global_goal.pose.position.x - robot_pose_.x();double dy = global_goal.pose.position.y - robot_pose_.y();double delta_orient = g2o::normalize_theta( tf2::getYaw(global_ientation) - robot_pose_.theta() );if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_._goal_tolerance&& fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance&& (!cfg_.goal_toleranceplete_global_plan || via_points_.size() == 0)){goal_reached_ = true;return mbf_msgs::ExePathResult::SUCCESS;}// check if we should enter any backup mode and apply settingsconfigureBackupModes(transformed_plan, goal_idx);// Return false if the transformed global plan is emptyif (pty()){ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");message = "Transformed plan is empty";return mbf_msgs::ExePathResult::INVALID_PATH;}// Get current goal point (last point of the transformed plan)robot_goal_.x() = transformed_plan.back().pose.position.x;robot_goal_.y() = transformed_plan.back().pose.position.y;// Overwrite goal orientation if neededif (cfg_.trajectory.global_plan_overwrite_orientation){robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global);// overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)tf2::Quaternion q;q.setRPY(0, 0, robot_goal_.theta());tf2::convert(q, transformed_plan.back().ientation);}  else{robot_goal_.theta() = tf2::getYaw(transformed_plan.back().ientation);}// overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory)if (transformed_plan.size()==1) // plan only contains the goal{transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized)}transformed_plan.front() = robot_pose; // update start// clear currently existing obstaclesobstacles_.clear();// Update obstacle container with costmap information or polygons provided by a costmap_converter pluginif (costmap_converter_)updateObstacleContainerWithCostmapConverter();elseupdateObstacleContainerWithCostmap();// also consider custom obstacles (must be called after other updates, since the container is not cleared)updateObstacleContainerWithCustomObstacles();// Do not allow config changes during the following optimization stepboost::mutex::scoped_lock cfg_lock(cfg_.configMutex());// Now perform the actual planning 调用TebOptimalPlanner::plan方法进行局部规划
//   bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_.goal_tolerance.free_goal_vel); // straight line initbool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);if (!success){planner_->clearPlanner(); // force reinitialization for next timeROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting.");++no_infeasible_plans_; // increase number of infeasible solutions in a rowtime_last_infeasible_plan_ = ros::Time::now();last_cmd_ = cmd_vel.twist;message = "teb_local_planner was not able to obtain a local plan";return mbf_msgs::ExePathResult::NO_VALID_CMD;}// Check feasibility (but within the first few states only)if(cfg_.robot.is_footprint_dynamic){// Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.footprint_spec_ = costmap_ros_->getRobotFootprint();costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);}bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);if (!feasible){cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;// now we reset everything to start again with the initialization of new trajectories.planner_->clearPlanner();ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. ");++no_infeasible_plans_; // increase number of infeasible solutions in a rowtime_last_infeasible_plan_ = ros::Time::now();last_cmd_ = cmd_vel.twist;message = "teb_local_planner trajectory is not feasible";return mbf_msgs::ExePathResult::NO_VALID_CMD;}// Get the velocity command for this sampling interval 获取速度命令，包括x,y,z分量和方向if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.l_look_ahead_poses)){planner_->clearPlanner();ROS_WARN("TebLocalPlannerROS: velocity command invalid. ");++no_infeasible_plans_; // increase number of infeasible solutions in a rowtime_last_infeasible_plan_ = ros::Time::now();last_cmd_ = cmd_vel.twist;message = "teb_local_planner velocity command invalid";return mbf_msgs::ExePathResult::NO_VALID_CMD;}//使速度更圆润// Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z,cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);// convert rot-vel to steering angle if desired (carlike robot).// 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_.d_angle_instead_rotvel){cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z,cfg_.robot.wheelbase, 0.95*cfg_.robot.min_turning_radius);if (!std::isfinite(cmd_vel.twist.angular.z)){cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0;last_cmd_ = cmd_vel.twist;planner_->clearPlanner();ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. ");++no_infeasible_plans_; // increase number of infeasible solutions in a rowtime_last_infeasible_plan_ = ros::Time::now();message = "teb_local_planner steering angle is not finite";return mbf_msgs::ExePathResult::NO_VALID_CMD;}}// a feasible solution should be found, reset counterno_infeasible_plans_ = 0;// store last command (for recovery analysis etc.)last_cmd_ = cmd_vel.twist;// Now visualize everything    planner_->visualize();visualization_->publishObstacles(obstacles_);visualization_->publishViaPoints(via_points_);visualization_->publishGlobalPlan(global_plan_);return mbf_msgs::ExePathResult::SUCCESS;
}

bool TebOptimalPlanner::plan(const std::vector& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{    ROS_ASSERT_MSG(initialized_, "Call initialize() first.");if (!teb_.isInit()){//初始化轨迹，在起点到目标点之间抽样出规定的位置点和时间差值。teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);}else // warm start{PoseSE2 start_(initial_plan.front().pose);PoseSE2 goal_(initial_plan.back().pose);if (teb_.sizePoses()>0&& (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist&& fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start!teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEBelse // goal too far away -> reinit{ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");teb_.clearTimedElasticBand();teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);}}if (start_vel)setVelocityStart(*start_vel);if (free_goal_vel)setVelocityGoalFree();elsevel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)
//通过构建一个由顶点和边构成的图，然后优化得到最优的轨迹。  // now optimizereturn optimizeTEB(cfg_-&_inner_iterations, cfg_-&_outer_iterations);
}

bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards,double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{if (cfg_->optim.optimization_activate==false) return false;bool success = false;optimized_ = false;double weight_multiplier = 1.0;// TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles//                (which leads to better results in terms of x-y-t homotopy planning).//                 however, we have not tested this mode intensively yet, so we keep//                 the legacy fast mode as default until we finish our tests.bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles;for(int i=0; b_autosize){//teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode);}
//生成图success = buildGraph(weight_multiplier);if (!success) {clearGraph();return false;}
//优化图success = optimizeGraph(iterations_innerloop, false);if (!success) {clearGraph();return false;}optimized_ = true;if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iterationcomputeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);clearGraph();weight_multiplier *= cfg_->optim.weight_adapt_factor;}return true;
}

bool TebOptimalPlanner::buildGraph(double weight_multiplier)
{if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()){ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!");return false;}
//添加顶点，被优化的对象  // add TEB vertices AddTEBVertices();
//下面的都是边，方程的约束项，每个边都有computeError()方法来计算error，图优化的目的就是尽可能的使总体error最小  // add Edges (local cost functions)if (cfg_->obstacles.legacy_obstacle_association)AddEdgesObstaclesLegacy(weight_multiplier);elseAddEdgesObstacles(weight_multiplier);if (cfg_->obstacles.include_dynamic_obstacles)AddEdgesDynamicObstacles();AddEdgesViaPoints();AddEdgesVelocity();AddEdgesAcceleration();AddEdgesTimeOptimal();	AddEdgesShortestPath();if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)AddEdgesKinematicsDiffDrive(); // we have a differential drive robotelseAddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.AddEdgesPreferRotDir();if (cfg_->optim.weight_velocity_obstacle_ratio > 0)AddEdgesVelocityObstacleRatio();return true;  
}bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after)
{if (cfg_->robot.max_vel_x<0.01){ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. ");if (clear_after) clearGraph();return false;	}if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples){ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");if (clear_after) clearGraph();return false;	}optimizer_->setVerbose(cfg_->optim.optimization_verbose);optimizer_->initializeOptimization();int iter = optimizer_->optimize(no_iterations);// Save Hessian for visualization//  g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast (optimizer_->solver());//  lm->solver()->saveHessian("~/MasterThesis/");if(!iter){ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);return false;}if (clear_after) clearGraph();	return true;
}

void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const
{if (dt == 0){vx = 0;vy = 0;omega = 0;return;}Eigen::Vector2d deltaS = pose2.position() - pose1.position();if (cfg_->robot.max_vel_y == 0) // nonholonomic robot{Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) );// translational velocitydouble dir = deltaS.dot(conf1dir);vx = (double) g2o::sign(dir) * ()/dt;vy = 0;}else // holonomic robot{// transform pose 2 into the current robot frame (pose1)// for velocities only the rotation of the direction vector is necessary.// (map->pose1-frame: inverse 2d rotation matrix)double cos_theta1 = std::cos(pose1.theta());double sin_theta1 = std::sin(pose1.theta());double p1_dx =  cos_theta1*deltaS.x() + sin_theta1*deltaS.y();double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();vx = p1_dx / dt;vy = p1_dy / dt;    }// rotational velocitydouble orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());omega = orientdiff/dt;
}bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const
{if (teb_.sizePoses()<2){ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. Make sure to init and optimize/plan the trajectory fist.");vx = 0;vy = 0;omega = 0;return false;}look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));double dt = 0.0;for(int counter = 0; counter < look_ahead_poses; ++counter){dt += teb_.TimeDiff(counter);if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses)  // TODO: change to look-ahead time? Refine trajectory?{look_ahead_poses = counter + 1;break;}}if (dt<=0){	ROS_ERROR("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!");vx = 0;vy = 0;omega = 0;return false;}// Get velocity from the first two configurationsextractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);return true;
}