[SubmapsDisplay::update] line_220 Could not compute submap fading: "map" passed to lookupTransform argument target_frame does not exist.
运行程序carto_pose运行时持续报警:
1
Lookup would require extrapolation into the past. Requested time 1609143852.338626800 but the earliest data is at time 1609143860.343082666, when looking up transform from frame [base_footprint] to frame [map]
// store initial plan for further initializations (must be valid for the // lifetime of this object or clearPlanner() is called!) initial_plan_ = &initial_plan;
// Update old TEBs with new start, goal and velocity updateAllTEBs(&start, &goal, start_vel);
// Init new TEBs based on newly explored homotopy classes exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel); // update via-points if activated updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates); // Optimize all trajectories in alternative homotopy classes optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); // Select which candidate (based on alternative homotopy classes) should be used selectBestTeb();
initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature); returntrue; }
voidHomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop) { // optimize TEBs in parallel since they are independend of each other
if (cfg_->hcp.enable_multithreading) { // Must prevent .join_all() from throwing exception if interruption was // requested, as this can lead to multiple threads operating on the same // TEB, which leads to SIGSEGV boost::this_thread::disable_interruption di;
EquivalenceClassContainer equivalence_classes_; //!< Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.
// The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).
if (!observation_list_.empty() ) { list<Observation>::iterator obs_it = observation_list_.begin(); // if keeping observations for no time, only keep one observation
// observation_keep_time_ 就是参数 observation_persistence if (observation_keep_time_ == ros::Duration(0.0)) { observation_list_.erase(++obs_it, observation_list_.end()); return; } // or loop through the observations to see which ones are stale for ( obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it ) { Observation& obs = *obs_it; // if observation is out of date, remove it and those follow from the list // kinetic: ros::Duration time_diff = last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp; if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_) { observation_list_.erase(obs_it, observation_list_.end()); return; } } }
// check if we'll add this buffer to our marking observation buffers if (marking) marking_buffers_.push_back(observation_buffers_.back()); // check if we'll also add this buffer to our clearing observation buffers if (clearing) clearing_buffers_.push_back(observation_buffers_.back());
// std::list<Observation> observation_list_; // create a new observation on the list to be populated observation_list_.push_front(Observation()); // check whether the origin frame has been set explicitly or // whether we should get it from the cloud // yaml里可以不设置sensor_frame，在这里取点云的坐标系名称 string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
try { // given these observations come from sensors, we store the origin pt of the sensor Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0), pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);
// pass on the raytrace/obstacle range of the observation buffer to the observations observation_list_.front().raytrace_range_ = raytrace_range_; observation_list_.front().obstacle_range_ = obstacle_range_;
// copy over the points that are within our height bounds // 从 global_frame_cloud中取出符合 z坐标的点，放到 observation_cloud for (unsignedint i = 0; i < cloud_size; ++i) { if (global_frame_cloud.points[i].z <= max_obstacle_height_ && global_frame_cloud.points[i].z >= min_obstacle_height_) { observation_cloud.points[point_count++] = global_frame_cloud.points[i]; } }
// resize the cloud for the number of legal points observation_cloud.points.resize(point_count); // 时间戳和坐标系也复制过来 observation_cloud.header.stamp = cloud.header.stamp; observation_cloud.header.frame_id = global_frame_cloud.header.frame_id; } catch (TransformException& ex) { // if an exception occurs, we need to remove the empty observation from the list observation_list_.pop_front(); ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); return; } // if the update was successful, we want to update the last updated time last_updated_ = ros::Time::now();
// remove any stale observations from the list purgeStaleObservations(); }
