/usr/bin/ld: skipping incompatible /home/user/lib/libtinyalsa.so when searching for -ltinyalsa /usr/bin/ld: cannot find -ltinyalsa: No such file or directory
有时装完某个库后，需要使用命令sudo ldconfig -v，否则在程序运行后会出现找不到动态库的问题: error while loading shared libraries: liblog4cpp.so.5: cannot open shared object file: No such file or directory
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex() )); plan.clear(); if(planner_costmap_ros_ == NULL) { log.error("Planner costmap ROS is NULL, unable to create global plan"); returnfalse; } //get the starting pose of the robot tf::Stamped<tf::Pose> global_pose; if(!planner_costmap_ros_->getRobotPose(global_pose)) { log.warn("Unable to get starting pose of robot, unable to create global plan"); returnfalse; } geometry_msgs::PoseStamped start; tf::poseStampedTFToMsg(global_pose, start);
// 这里就是 GlobalPlanner::makePlan 了 if(!planner_->makePlan(start, goal, plan) || plan.empty()){ ROS_WARN("[move_base] Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y); returnfalse; } returntrue;
costmap raytraces to clear out obstacles. 障碍物要想从地图上清除，它所占据的空间必须被新的观测看到。但是参数observation_persistence常常无效。
last_updated_ is updated on every new cloud that needs to be added to the buffer. purgeStaleObservations函数 checks the cloud’s time stamp with last_updated_. When we stop publishing a point cloud last_updated_ will never be updated. Hence the observations will not time-out and remain in the observation_list_. This may result in the following warning, when the robot drives away from the clouds in observation_list_: Sensor origin at (x, y) is out of map bounds. The costmap cannot raytrace for it
voidTebOptimalPlanner::computeCurrentCost( double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) { // check if graph is empty/exist, important if function is called // between buildGraph and optimizeGraph/clearGraph boolgraph_exist_flag(false); if (optimizer_->edges().empty() && optimizer_->vertices().empty()) { // here the graph is build again, for time efficiency make sure to call this function // between buildGraph and Optimize (deleted), but it depends on the application buildGraph(); optimizer_->initializeOptimization(); } else graph_exist_flag = true;
optimizer_->computeInitialGuess(); // cost_ 在 TebOptimalPlanner构造函数里 初始化为 HUGE_VAL cost_ = 0; if (alternative_time_cost) { cost_ += teb_.getSumOfAllTimeDiffs(); /* we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs, since we are using an AutoResize Function with hysteresis */ } // now we need pointers to all edges -> calculate error for each edge-type // since we aren't storing edge pointers, we need to check every edge for (std::vector<g2o::OptimizableGraph::Edge*>::const_iterator it = optimizer_->activeEdges().begin(); it!= optimizer_->activeEdges().end(); it++) { double cur_cost = (*it)->chi2();
if (dynamic_cast<EdgeObstacle*>(*it) != nullptr || dynamic_cast<EdgeInflatedObstacle*>(*it) != nullptr || dynamic_cast<EdgeDynamicObstacle*>(*it) != nullptr) { cur_cost *= obst_cost_scale; } elseif (dynamic_cast<EdgeViaPoint*>(*it) != nullptr) cur_cost *= viapoint_cost_scale; elseif (dynamic_cast<EdgeTimeOptimal*>(*it) != nullptr && alternative_time_cost) { // skip these edges if alternative_time_cost is active continue; } cost_ += cur_cost; } // delete temporary created graph if (!graph_exist_flag) clearGraph(); }
obst_cost_scale Specify extra scaling for obstacle costs (only used if compute_cost_afterwards is true)
viapoint_cost_scale Specify extra scaling for via-point costs (only used if compute_cost_afterwards is true)
alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time (only used if compute_cost_afterwards is true)
总结
optimizeTEB包括两个循环:
通过调用TimedElasticBand::autoResize()，外循环resizes the trajectory
内外循环次数的比例defines the contraction behavior and convergence rate of the trajectory optimization. 2~6个内循环足够了
TebOptPlannerContainer tebs_; : Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs
The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate.
Optionally, the cost vector can be calculated by specifyingcompute_cost_afterwards, the cost vector can be accessed afterwards using getCurrentCost(). 目前使用TebOptimalPlanner不会计算代价，使用HomotopyClassPlanner会，第一次计算时，代价是一个很大很大的数，第二次减小很多，之后逐渐减小到1左右或者更小
/* 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(栅格路径)
