// 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(); }
teb_planner.getFullTrajectory(msg.trajectories.front().trajectory); // add obstacles msg.obstacles_msg.obstacles.resize(obstacles.size()); for (std::size_t i=0; i<obstacles.size(); ++i) { msg.obstacles_msg.header = msg.header; // copy polygon msg.obstacles_msg.obstacles[i].header = msg.header; obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon); // copy id msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
if (visualization_) { // Visualize graph if (cfg_->hcp.visualize_hc_graph && graph_search_) visualization_->publishGraph(graph_search_->graph_);
// Visualize active tebs as marker visualization_->publishTebContainer(tebs_);
// Visualize best teb and feedback message if desired TebOptimalPlannerConstPtr best_teb = bestTeb(); if (best_teb) { // 只发布best_teb的局部路径，类型是nav_msgs::Path 和 geometry_msgs::PoseArray // 对应话题 local_plan 和 teb_poses // 位姿点的z = cfg_->hcp.visualize_with_time_as_z_axis_scale * // teb.getSumOfTimeDiffsUpToIdx(i)
visualization_->publishLocalPlanAndPoses(best_teb->teb() ); if (best_teb->teb().sizePoses() > 0) // 路径的位姿点个数 visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *robot_model_);
if (cfg_->trajectory.publish_feedback) // feedback message { int best_idx = bestTebIdx(); if (best_idx>=0) visualization_->publishFeedbackMessage(tebs_, (unsignedint) best_idx, *obstacles_ ); } } } elseROS_DEBUG("Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before.");
# The point cloud data may be organized 2d (image-like) or 1d # (unordered). Point clouds organized as 2d images may be produced by # camera depth sensors such as stereo or time-of-flight.
bool is_bigendian uint32 point_step # Length of a point in bytes uint32 row_step # Length of a row in bytes uint8[] data # 点的数据，size = row_step * height bool is_dense # True if there are no invalid points
PointCloud2的data是序列化后的数据，直接看不到物理意义。只能转为点云1类型
sensor_msgs/PointCloud类型如下:
1 2 3 4 5 6 7 8 9
Header header
# 在header坐标系中的3D点云 geometry_msgs/Point32[] points
# Each channel should have the same number of elements as points array, # and the data in each channel should correspond 1:1 with each point. # Channel names in common practice are listed in ChannelFloat32.msg. ChannelFloat32[] channels
PointCloudConverter initialized to transform from PointCloud(/points_in) to PointCloud2(/points2_out). PointCloudConverter initialized to transform from PointCloud2(/camera_/depth/points) to PointCloud(/pcl)
