扩展的find算法 -- find_nearest(查询最接近的数据)

介绍了一个通用模板函数find_nearest,用于在set或map中查找与指定值最接近的元素,并通过单元测试验证了其正确性。

通常使用需要快速查找定位的数据时,会通过 set/map 等保存数据,然后使用find等算法进行查找。

但有时想查找的数据不能完全匹配,而是查找一个最接近的值(比如,在两个不同的数据结构中,想匹配出ID相同,且发生时间最接近的数据)时,STL没有提供可用的函数,一般的做法就是将数据全部遍历计算一遍,然后选取最接近的值。其算法复杂度为 O(n)。

通过扩展,写出通用的模版算法 find_nearest 可以完成这个功能,适用于 set/map。

 

PS:原本想找现成的,可惜没有找到,只有自己写一个。感觉这个人的目的和我的比较接近:http://topic.youkuaiyun.com/u/20071112/10/226d912b-05b5-4e2f-8f8e-0a93ec76eeca.html

 

废话不多说了,直接上代码和UT。

 

代码:

 

 

UT(CppUnit):

bool rrt_sharp(const Eigen::Vector3d &s, const Eigen::Vector3d &g) { ros::Time rrt_start_time = ros::Time::now(); bool goal_found = false; double c_square = (g - s).squaredNorm() / 4.0; /* kd tree init */ kdtree *kd_tree = kd_create(3); // Add start and goal nodes to kd tree kd_insert3(kd_tree, start_node_->x[0], start_node_->x[1], start_node_->x[2], start_node_); /* main loop */ int idx = 0; for (idx = 0; (ros::Time::now() - rrt_start_time).toSec() < search_time_ && valid_tree_node_nums_ < max_tree_node_nums_; ++idx) { /* biased random sampling */ Eigen::Vector3d x_rand; sampler_.samplingOnce(x_rand); // samplingOnce(x_rand); if (!map_ptr_->isStateValid(x_rand)) { continue; } struct kdres *p_nearest = kd_nearest3(kd_tree, x_rand[0], x_rand[1], x_rand[2]); if (p_nearest == nullptr) { ROS_ERROR("nearest query error"); continue; } RRTNode3DPtr nearest_node = (RRTNode3DPtr)kd_res_item_data(p_nearest); kd_res_free(p_nearest); Eigen::Vector3d x_new = steer(nearest_node->x, x_rand, steer_length_); if (!map_ptr_->isSegmentValid(nearest_node->x, x_new)) { continue; } /* 1. find parent */ /* kd_tree bounds search for parent */ Neighbour neighbour_nodes; neighbour_nodes.nearing_nodes.reserve(50); neighbour_nodes.center = x_new; struct kdres *nbr_set; nbr_set = kd_nearest_range3(kd_tree, x_new[0], x_new[1], x_new[2], search_radius_); if (nbr_set == nullptr) { ROS_ERROR("bkwd kd range query error"); break; } while (!kd_res_end(nbr_set)) { RRTNode3DPtr curr_node = (RRTNode3DPtr)kd_res_item_data(nbr_set); neighbour_nodes.nearing_nodes.emplace_back(curr_node, false, false); // store range query result so that we dont need to query again for rewire; kd_res_next(nbr_set); // go to next in kd tree range query result } kd_res_free(nbr_set); // reset kd tree range query /* choose parent from kd tree range query result*/ double dist2nearest = calDist(nearest_node->x, x_new); double min_dist_from_start(nearest_node->cost_from_start + dist2nearest); double cost_from_p(dist2nearest); RRTNode3DPtr min_node(nearest_node); // set the nearest_node as the default parent // TODO sort by potential cost-from-start for (auto &curr_node : neighbour_nodes.nearing_nodes) { if (curr_node.node_ptr == nearest_node) // the nearest_node already calculated and checked collision free { continue; } // check potential first, then check edge collision double curr_dist = calDist(curr_node.node_ptr->x, x_new); double potential_dist_from_start = curr_node.node_ptr->cost_from_start + curr_dist; if (min_dist_from_start > potential_dist_from_start) { bool connected = map_ptr_->isSegmentValid(curr_node.node_ptr->x, x_new); curr_node.is_checked = true; if (connected) { curr_node.is_valid = true; cost_from_p = curr_dist; min_dist_from_start = potential_dist_from_start; min_node = curr_node.node_ptr; } } } /* parent found within radius, then add a node to rrt and kd_tree */ // sample rejection double dist_to_goal = calDist(x_new, goal_node_->x); if (min_dist_from_start + dist_to_goal >= goal_node_->cost_from_start) { // ROS_WARN("parent found but sample rejected"); continue; } /* 1.1 add the randomly sampled node to rrt_tree */ RRTNode3DPtr new_node(nullptr); new_node = addTreeNode(min_node, x_new, min_dist_from_start, cost_from_p, dist_to_goal); /* 1.2 add the randomly sampled node to kd_tree */ kd_insert3(kd_tree, x_new[0], x_new[1], x_new[2], new_node); // end of find parent /* 2. try to connect to goal if possible */ if (dist_to_goal <= search_radius_) { bool is_connected2goal = map_ptr_->isSegmentValid(x_new, goal_node_->x); if (is_connected2goal && goal_node_->cost_from_start > dist_to_goal + new_node->cost_from_start) // a better path found { if (!goal_found) { first_path_use_time_ = (ros::Time::now() - rrt_start_time).toSec(); } goal_found = true; changeNodeParent(goal_node_, new_node, dist_to_goal); vector<Eigen::Vector3d> curr_best_path; fillPath(goal_node_, curr_best_path); path_list_.emplace_back(curr_best_path); solution_cost_time_pair_list_.emplace_back(goal_node_->cost_from_start, (ros::Time::now() - rrt_start_time).toSec()); // vis_ptr_->visualize_path(curr_best_path, "rrt_sharp_final_path"); // vis_ptr_->visualize_pointcloud(curr_best_path, "rrt_sharp_final_wpts"); if (use_informed_sampling_) { scale_[0] = goal_node_->cost_from_start / 2.0; scale_[1] = sqrt(scale_[0] * scale_[0] - c_square); scale_[2] = scale_[1]; sampler_.setInformedSacling(scale_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans_, scale_, rot_); vis_ptr_->visualize_ellipsoids(ellps, "informed_set", visualization::yellow, 0.2); } else if (use_GUILD_sampling_) { RRTNode3DPtr beacon_node = beaconSelect(); calInformedSet(beacon_node->cost_from_start, start_node_->x, beacon_node->x, scale1_, trans1_, rot1_); calInformedSet(goal_node_->cost_from_start - beacon_node->cost_from_start, beacon_node->x, goal_node_->x, scale2_, trans2_, rot2_); sampler_.setGUILDInformed(scale1_, trans1_, rot1_, scale2_, trans2_, rot2_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans1_, scale1_, rot1_); ellps.emplace_back(trans2_, scale2_, rot2_); vis_ptr_->visualize_ellipsoids(ellps, "local_set", visualization::green, 0.2); } } } /* 3.rewire */ std::priority_queue<RRTNode3DPtr, RRTNode3DPtrVector, RRTNodeComparator> rewire_queue; for (auto &curr_node : neighbour_nodes.nearing_nodes) { double dist_to_potential_child = calDist(new_node->x, curr_node.node_ptr->x); bool not_consistent = new_node->cost_from_start + dist_to_potential_child < curr_node.node_ptr->cost_from_start ? 1 : 0; bool promising = new_node->cost_from_start + dist_to_potential_child + curr_node.node_ptr->heuristic_to_goal < goal_node_->cost_from_start ? 1 : 0; if (not_consistent && promising) { bool connected(false); if (curr_node.is_checked) connected = curr_node.is_valid; else connected = map_ptr_->isSegmentValid(new_node->x, curr_node.node_ptr->x); // If we can get to a node via the sampled_node faster than via it's existing parent then change the parent if (connected) { double best_cost_before_rewire = goal_node_->cost_from_start; changeNodeParent(curr_node.node_ptr, new_node, dist_to_potential_child); rewire_queue.emplace(curr_node.node_ptr); if (best_cost_before_rewire > goal_node_->cost_from_start) { vector<Eigen::Vector3d> curr_best_path; fillPath(goal_node_, curr_best_path); path_list_.emplace_back(curr_best_path); solution_cost_time_pair_list_.emplace_back(goal_node_->cost_from_start, (ros::Time::now() - rrt_start_time).toSec()); // vis_ptr_->visualize_path(curr_best_path, "rrt_sharp_final_path"); // vis_ptr_->visualize_pointcloud(curr_best_path, "rrt_sharp_final_wpts"); if (use_informed_sampling_) { scale_[0] = goal_node_->cost_from_start / 2.0; scale_[1] = sqrt(scale_[0] * scale_[0] - c_square); scale_[2] = scale_[1]; sampler_.setInformedSacling(scale_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans_, scale_, rot_); vis_ptr_->visualize_ellipsoids(ellps, "informed_set", visualization::yellow, 0.2); } else if (use_GUILD_sampling_) { RRTNode3DPtr beacon_node = beaconSelect(); calInformedSet(beacon_node->cost_from_start, start_node_->x, beacon_node->x, scale1_, trans1_, rot1_); calInformedSet(goal_node_->cost_from_start - beacon_node->cost_from_start, beacon_node->x, goal_node_->x, scale2_, trans2_, rot2_); sampler_.setGUILDInformed(scale1_, trans1_, rot1_, scale2_, trans2_, rot2_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans1_, scale1_, rot1_); ellps.emplace_back(trans2_, scale2_, rot2_); vis_ptr_->visualize_ellipsoids(ellps, "local_set", visualization::green, 0.2); } } } } /* go to the next entry */ } while (!rewire_queue.empty()) { RRTNode3DPtr curr_rewire_node = rewire_queue.top(); std::make_heap(const_cast<RRTNode3DPtr *>(&rewire_queue.top()), const_cast<RRTNode3DPtr *>(&rewire_queue.top()) + rewire_queue.size(), RRTNodeComparator()); // re-order the queue every time before pop a node, since the key may change during changeNodeParent() rewire_queue.pop(); struct kdres *nbr_set; nbr_set = kd_nearest_range3(kd_tree, curr_rewire_node->x[0], curr_rewire_node->x[1], curr_rewire_node->x[2], search_radius_); if (nbr_set == nullptr) { ROS_ERROR("bkwd kd range query error"); break; } while (!kd_res_end(nbr_set)) { RRTNode3DPtr curr_node = (RRTNode3DPtr)kd_res_item_data(nbr_set); double dist_to_potential_child = calDist(curr_rewire_node->x, curr_node->x); bool not_consistent = curr_rewire_node->cost_from_start + dist_to_potential_child < curr_node->cost_from_start ? 1 : 0; bool promising = curr_rewire_node->cost_from_start + dist_to_potential_child + curr_node->heuristic_to_goal < goal_node_->cost_from_start ? 1 : 0; if (not_consistent && promising) // If we can get to a node via the curr_rewire_node faster than via it's existing parent then change the parent { bool connected = map_ptr_->isSegmentValid(curr_rewire_node->x, curr_node->x); // If we can get to a node via the sampled_node faster than via it's existing parent then change the parent if (connected) { double best_cost_before_rewire = goal_node_->cost_from_start; changeNodeParent(curr_node, curr_rewire_node, dist_to_potential_child); rewire_queue.emplace(curr_node); if (best_cost_before_rewire > goal_node_->cost_from_start) { vector<Eigen::Vector3d> curr_best_path; fillPath(goal_node_, curr_best_path); path_list_.emplace_back(curr_best_path); solution_cost_time_pair_list_.emplace_back(goal_node_->cost_from_start, (ros::Time::now() - rrt_start_time).toSec()); vis_ptr_->visualize_path(curr_best_path, "rrt_sharp_final_path"); vis_ptr_->visualize_pointcloud(curr_best_path, "rrt_sharp_final_wpts"); } } } kd_res_next(nbr_set); // go to next in kd tree range query result } kd_res_free(nbr_set); // reset kd tree range query } /* end of rewire */ } /* end of sample once */ vector<Eigen::Vector3d> vertice; vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> edges; sampleWholeTree(start_node_, vertice, edges); std::vector<visualization::BALL> balls; balls.reserve(vertice.size()); visualization::BALL node_p; node_p.radius = 0.2; for (size_t i = 0; i < vertice.size(); ++i) { node_p.center = vertice[i]; balls.push_back(node_p); } vis_ptr_->visualize_balls(balls, "tree_vertice", visualization::Color::blue, 1.0); vis_ptr_->visualize_pairline(edges, "tree_edges", visualization::Color::red, 0.1); if (goal_found) { final_path_use_time_ = (ros::Time::now() - rrt_start_time).toSec(); fillPath(goal_node_, final_path_); ROS_INFO_STREAM("[RRT#]: first_path_use_time: " << first_path_use_time_ << ", length: " << solution_cost_time_pair_list_.front().first); } else if (valid_tree_node_nums_ == max_tree_node_nums_) { ROS_ERROR_STREAM("[RRT#]: NOT CONNECTED TO GOAL after " << max_tree_node_nums_ << " nodes added to rrt-tree"); } else { ROS_ERROR_STREAM("[RRT#]: NOT CONNECTED TO GOAL after " << (ros::Time::now() - rrt_start_time).toSec() << " seconds"); } return goal_found; } 注释上述代码
09-19
private: // nodehandle params ros::NodeHandle nh_; BiasSampler sampler_; // for informed sampling Eigen::Vector3d trans_, scale_; Eigen::Matrix3d rot_; bool use_informed_sampling_; double steer_length_; double search_radius_; double search_time_; int max_tree_node_nums_; int valid_tree_node_nums_; double first_path_use_time_; double final_path_use_time_; double cost_best_; std::vector<TreeNode *> nodes_pool_; TreeNode *start_node_; TreeNode *goal_node_; vector<Eigen::Vector3d> final_path_; vector<vector<Eigen::Vector3d>> path_list_; vector<std::pair<double, double>> solution_cost_time_pair_list_; // environment env::OccMap::Ptr map_ptr_; std::shared_ptr<visualization::Visualization> vis_ptr_; void reset() { final_path_.clear(); path_list_.clear(); cost_best_ = DBL_MAX; solution_cost_time_pair_list_.clear(); for (int i = 0; i < valid_tree_node_nums_; i++) { nodes_pool_[i]->parent = nullptr; nodes_pool_[i]->children.clear(); } valid_tree_node_nums_ = 0; } double calDist(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2) { return (p1 - p2).norm(); } RRTNode3DPtr addTreeNode(RRTNode3DPtr &parent, const Eigen::Vector3d &state, const double &cost_from_start, const double &cost_from_parent) { RRTNode3DPtr new_node_ptr = nodes_pool_[valid_tree_node_nums_]; valid_tree_node_nums_++; new_node_ptr->parent = parent; parent->children.push_back(new_node_ptr); new_node_ptr->x = state; new_node_ptr->cost_from_start = cost_from_start; new_node_ptr->cost_from_parent = cost_from_parent; return new_node_ptr; } void changeNodeParent(RRTNode3DPtr &node, RRTNode3DPtr &parent, const double &cost_from_parent) { if (node->parent) node->parent->children.remove(node); //DON'T FORGET THIS, remove it form its parent's children list node->parent = parent; node->cost_from_parent = cost_from_parent; node->cost_from_start = parent->cost_from_start + cost_from_parent; parent->children.push_back(node); // for all its descedants, change the cost_from_start and tau_from_start; RRTNode3DPtr descendant(node); std::queue<RRTNode3DPtr> Q; Q.push(descendant); while (!Q.empty()) { descendant = Q.front(); Q.pop(); for (const auto &leafptr : descendant->children) { leafptr->cost_from_start = leafptr->cost_from_parent + descendant->cost_from_start; Q.push(leafptr); } } } void fillPath(const RRTNode3DPtr &node_A, const RRTNode3DPtr &node_B, vector<Eigen::Vector3d> &path) { path.clear(); RRTNode3DPtr node_ptr = node_A; while (node_ptr->parent) { path.push_back(node_ptr->x); node_ptr = node_ptr->parent; } path.push_back(start_node_->x); std::reverse(std::begin(path), std::end(path)); node_ptr = node_B; while (node_ptr->parent) { path.push_back(node_ptr->x); node_ptr = node_ptr->parent; } path.push_back(goal_node_->x); } inline void sortNbrSet( Neighbour &nbrSet, Eigen::Vector3d &x_rand ) { std::sort(nbrSet.nearing_nodes.begin(), nbrSet.nearing_nodes.end(), [&x_rand](NodeWithStatus &node1, NodeWithStatus &node2){ return node1.node_ptr->cost_from_start + (node1.node_ptr->x - x_rand).norm() < node2.node_ptr->cost_from_start + (node2.node_ptr->x - x_rand).norm(); }); } inline void rewireTree( Neighbour &nbrSet, RRTNode3DPtr &new_node, const Eigen::Vector3d &x_target) { for(auto curr_node : nbrSet.nearing_nodes) { double dist_to_potential_child = calDist(new_node->x, curr_node.node_ptr->x); bool not_consistent = new_node->cost_from_start + dist_to_potential_child < curr_node.node_ptr->cost_from_start ? true : false; bool promising = new_node->cost_from_start + dist_to_potential_child + calDist(curr_node.node_ptr->x, x_target) < cost_best_ ? true : false; if( not_consistent && promising ) { bool connected(false); if (curr_node.is_checked) connected = curr_node.is_valid; else connected = map_ptr_->isSegmentValid(new_node->x, curr_node.node_ptr->x); if(connected) changeNodeParent(curr_node.node_ptr, new_node, dist_to_potential_child); } } } inline void chooseBestNode( Neighbour &nbrSet, const Eigen::Vector3d &x_rand, RRTNode3DPtr &min_node, double &cost_start, double &cost_parent) { for( auto &curr_node : nbrSet.nearing_nodes) { curr_node.is_checked = true; if(map_ptr_->isSegmentValid(curr_node.node_ptr->x, x_rand)) { curr_node.is_valid = true; min_node = curr_node.node_ptr; cost_parent = calDist(min_node->x, x_rand); cost_start = min_node->cost_from_start + cost_parent; break; }else{ curr_node.is_valid = false; continue; } } } bool brrt_star(const Eigen::Vector3d &s, const Eigen::Vector3d &g) { ros::Time rrt_start_time = ros::Time::now(); bool tree_connected = false; double c_square = (g - s).squaredNorm() / 4.0; /* kd tree init */ kdtree *treeA = kd_create(3); kdtree *treeB = kd_create(3); //Add start and goal nodes to kd trees kd_insert3(treeA, start_node_->x[0], start_node_->x[1], start_node_->x[2], start_node_); kd_insert3(treeB, goal_node_->x[0], goal_node_->x[1], goal_node_->x[2], goal_node_); /* main loop */ int idx = 0; for (idx = 0; (ros::Time::now() - rrt_start_time).toSec() < search_time_ && valid_tree_node_nums_ < max_tree_node_nums_; ++idx) { bool check_connect = false; bool selectTreeA = true; /* random sampling */ Eigen::Vector3d x_rand; sampler_.samplingOnce(x_rand); if (!map_ptr_->isStateValid(x_rand)) { continue; } /* Search neighbors in both treeA and treeB */ Neighbour neighbour_nodesA, neighbour_nodesB; neighbour_nodesA.nearing_nodes.reserve(80); neighbour_nodesB.nearing_nodes.reserve(80); neighbour_nodesB.center = neighbour_nodesA.center = x_rand; struct kdres *nbr_setA = kd_nearest_range3(treeA, x_rand[0], x_rand[1], x_rand[2], search_radius_); struct kdres *nbr_setB = kd_nearest_range3(treeB, x_rand[0], x_rand[1], x_rand[2], search_radius_); if ( nbr_setA == nullptr ) // TreeA { struct kdres *p_nearest = kd_nearest3(treeA, x_rand[0], x_rand[1], x_rand[2]); if (p_nearest == nullptr) { ROS_ERROR("nearest query error"); continue; } RRTNode3DPtr nearest_node = (RRTNode3DPtr)kd_res_item_data(p_nearest); kd_res_free(p_nearest); neighbour_nodesA.nearing_nodes.emplace_back(nearest_node, false, false); }else{ check_connect = true; while (!kd_res_end(nbr_setA)){ RRTNode3DPtr curr_node = (RRTNode3DPtr)kd_res_item_data(nbr_setA); neighbour_nodesA.nearing_nodes.emplace_back(curr_node, false, false); // store range query result so that we dont need to query again for rewire; kd_res_next(nbr_setA); //go to next in kd tree range query result } } kd_res_free(nbr_setA); //reset kd tree range query if ( nbr_setB == nullptr )// TreeB { struct kdres *p_nearest = kd_nearest3(treeB, x_rand[0], x_rand[1], x_rand[2]); if (p_nearest == nullptr) { ROS_ERROR("nearest query error"); continue; } RRTNode3DPtr nearest_node = (RRTNode3DPtr)kd_res_item_data(p_nearest); kd_res_free(p_nearest); neighbour_nodesB.nearing_nodes.emplace_back(nearest_node, false, false); }else{ check_connect = true; while (!kd_res_end(nbr_setB)){ RRTNode3DPtr curr_node = (RRTNode3DPtr)kd_res_item_data(nbr_setB); neighbour_nodesB.nearing_nodes.emplace_back(curr_node, false, false); // store range query result so that we dont need to query again for rewire; kd_res_next(nbr_setB); //go to next in kd tree range query result } } kd_res_free(nbr_setB); //reset kd tree range query /* Sort two neighbor sets */ sortNbrSet(neighbour_nodesA, x_rand); sortNbrSet(neighbour_nodesB, x_rand); /* Get the best parent node in each tree */ RRTNode3DPtr min_node_A(nullptr), min_node_B(nullptr); double min_cost_start_A(DBL_MAX), min_cost_start_B(DBL_MAX); double cost_parent_A(DBL_MAX), cost_parent_B(DBL_MAX); chooseBestNode(neighbour_nodesA, x_rand, min_node_A, min_cost_start_A, cost_parent_A); chooseBestNode(neighbour_nodesB, x_rand, min_node_B, min_cost_start_B, cost_parent_B); /* Select the best tree, insert the node and rewire the tree */ RRTNode3DPtr new_node(nullptr); if( (min_node_A != nullptr) || (min_node_B != nullptr) ) { if( min_cost_start_A < min_cost_start_B ){ if(min_cost_start_A + calDist(x_rand, goal_node_->x) >= cost_best_) continue; // Sampling rejection selectTreeA = true; new_node = addTreeNode(min_node_A, x_rand, min_cost_start_A, cost_parent_A); kd_insert3(treeA, x_rand[0], x_rand[1], x_rand[2], new_node); rewireTree(neighbour_nodesA, new_node, goal_node_->x); }else{ if(min_cost_start_B + calDist(x_rand, start_node_->x) >= cost_best_) continue; // Sampling rejection selectTreeA = false; new_node = addTreeNode(min_node_B, x_rand, min_cost_start_B, cost_parent_B); kd_insert3(treeB, x_rand[0], x_rand[1], x_rand[2], new_node); rewireTree(neighbour_nodesB, new_node, start_node_->x); } } if( (min_node_A == nullptr) || (min_node_B == nullptr) ) check_connect = false; // No possible connection /* Check connection */ if( check_connect ) { /* Accept connection if achieve better cost */ double cost_curr = min_cost_start_A + min_cost_start_B; if(cost_curr < cost_best_) { cost_best_ = cost_curr; tree_connected = true; vector<Eigen::Vector3d> curr_best_path; if( selectTreeA ) fillPath(new_node, min_node_B, curr_best_path); else fillPath(min_node_A, new_node, curr_best_path); path_list_.emplace_back(curr_best_path); solution_cost_time_pair_list_.emplace_back(cost_best_, (ros::Time::now() - rrt_start_time).toSec()); if(use_informed_sampling_) { /* Update informed set */ scale_[0] = cost_best_ / 2.0; scale_[1] = sqrt(scale_[0] * scale_[0] - c_square); scale_[2] = scale_[1]; sampler_.setInformedSacling(scale_); std::vector<visualization::ELLIPSOID> ellps; ellps.emplace_back(trans_, scale_, rot_); vis_ptr_->visualize_ellipsoids(ellps, "informed_set", visualization::yellow, 0.2); } } } }//End of one sampling iteration if (tree_connected) { final_path_use_time_ = (ros::Time::now() - rrt_start_time).toSec(); ROS_INFO_STREAM("[BRRT*]: find_path_use_time: " << solution_cost_time_pair_list_.front().second << ", length: " << solution_cost_time_pair_list_.front().first); visualizeWholeTree(); final_path_ = path_list_.back(); } else if (valid_tree_node_nums_ == max_tree_node_nums_) { visualizeWholeTree(); ROS_ERROR_STREAM("[BRRT*]: NOT CONNECTED TO GOAL after " << max_tree_node_nums_ << " nodes added to rrt-tree"); } else { ROS_ERROR_STREAM("[BRRT*]: NOT CONNECTED TO GOAL after " << (ros::Time::now() - rrt_start_time).toSec() << " seconds"); } return tree_connected; } 注释上述代码
最新发布
09-19
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值