2024年4月12日发(作者：)

~/selection_cost_hysteresis (double, default: 1.0)

#Extra scaling of obstacle cost terms just for selecting the 'best' candidate.

~/selection_obst_cost_scale (double, default: 100.0)

#Extra scaling of via-point cost terms just for selecting the 'best' candidate. New in version 0.4

~/selection_viapoint_cost_scale (double, default: 1.0)

miscellaneous parameters

code

void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)

{

// create the planner instance

if (cfg_._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.");

}

}

bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)

{

// prune global plan to cut off parts of the past (spatially before the robot)

pruneGlobalPlan(*tf_, robot_pose, global_plan_);

// Transform global plan to the frame of interest ( the local costmap)

if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_._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");

return false;

}

// check if we should enter any backup mode and apply settings

configureBackupModes(transformed_plan, goal_idx);

updateObstacleContainerWithCostmap();

// Now perform the actual planning

bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal__goal_vel);

bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.ility_check_no_poses);

if (!planner_->getVelocityCommand(cmd_.x, cmd_.y, cmd_r.z)){

}

}

bool TebOptimalPlanner::plan(const std::vector& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)

{

if (!teb_.isInit()){

// init trajectory

teb_.initTEBtoGoal(initial_plan, cfg_->_ref, cfg_->_plan_overwrite_orientation, cfg_->_samples, cfg_->_init_with_backwards_motion);

}

else{

PoseSE2 start_(initial_().pose);

PoseSE2 goal_(initial_().pose);

if (teb_.sizePoses()>0 && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->_reinit_new_goal_dist) // actual warm start!

teb_.updateAndPruneTEB(start_, goal_, cfg_->_samples); // update TEB

else // goal too far away -> reinit

{

ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");

teb_.clearTimedElasticBand();

teb_.initTEBtoGoal(initial_plan, cfg_->_ref, true, cfg_->_samples, cfg_->_init_with_backwards_motion);

}

}

// now optimize

return 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)

{

for(int i=0; i

{

if (cfg_->_autosize)

teb_.autoResize(cfg_->_ref, cfg_->_hysteresis, cfg_->_samples, cfg_->_samples);

//构建图

success = buildGraph(weight_multiplier);

if (!success)

{

clearGraph();

return false;

}

//优化图

success = optimizeGraph(iterations_innerloop, false);

if (!success)

{

clearGraph();

return false;

}

if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration

computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);

clearGraph();

weight_multiplier *= cfg_->_adapt_factor;

}

}

bool TebOptimalPlanner::buildGraph(double weight_multiplier)

{

// add TEB vertices

AddTEBVertices();

// add Edges (local cost functions)

if (cfg_->_obstacle_association)

AddEdgesObstaclesLegacy(weight_multiplier);

else

AddEdgesObstacles(weight_multiplier);

//AddEdgesDynamicObstacles();

AddEdgesViaPoints();

AddEdgesVelocity();

AddEdgesAcceleration();

AddEdgesTimeOptimal();

if (cfg_->_turning_radius == 0 || cfg_->_kinematics_turning_radius == 0)

AddEdgesKinematicsDiffDrive(); // we have a differential drive robot

else

AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.

AddEdgesPreferRotDir();

}

bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after)

{

if (cfg_->_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_->_samples)

{

ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");

if (clear_after) clearGraph();

return false;

}

// boost::shared_ptr optimizer_; //!< g2o optimizer for trajectory optimization

optimizer_->setVerbose(cfg_->zation_verbose);

optimizer_->initializeOptimization();

int iter = optimizer_->optimize(no_iterations);

if(!iter)

{

ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);

return false;

}

if (clear_after) clearGraph();

}

g2o

boost::shared_ptr TebOptimalPlanner::initOptimizer()

{

// Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)

static boost::once_flag flag = BOOST_ONCE_INIT;

boost::call_once(®isterG2OTypes, flag);

// allocating the optimizer

boost::shared_ptr optimizer = boost::make_shared();

//typedef g2o::LinearSolverCSparse TEBLinearSolver;

TEBLinearSolver* linearSolver = new TEBLinearSolver(); // see typedef in optimization.h

linearSolver->setBlockOrdering(true);

//typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > TEBBlockSolver;

TEBBlockSolver* blockSolver = new TEBBlockSolver(linearSolver);

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver);

optimizer->setAlgorithm(solver);

optimizer->initMultiThreading(); // required for >Eigen 3.1

return optimizer;

}