graph_decoder系列（2）

承接上文我们接着来看decoder_node.cpp的代码部分

void GraphDecoder::Init() {/* initialize subscriber and publisher *///graph_sub_ 订阅节点/robot_vgraph    其类型是visibility_graph_msg/Graph的消息graph_sub_     = nh.subscribe("/robot_vgraph", 5, &GraphDecoder::GraphCallBack, this);graph_pub_     = nh.advertise<visibility_graph_msg::Graph>("decoded_vgraph", 5);graph_viz_pub_ = nh.advertise<MarkerArray>("/graph_decoder_viz",5);this->LoadParmas();save_graph_sub_ = nh.subscribe("/save_file_dir", 5, &GraphDecoder::SaveGraphCallBack, this);read_graph_sub_ = nh.subscribe("/read_file_dir", 5, &GraphDecoder::ReadGraphCallBack, this);request_graph_service_  = nh.advertiseService("/request_graph_service",  &GraphDecoder::RequestGraphService, this);robot_id_ = 0;this->ResetGraph(received_graph_);
}

上回说完了GraphCallBack函数也就是grap_sub_订阅到msg消息以后的回调机制，接着这个 graph_pub_ 定义了一个发送节点，他将visibility_graph_msg::Graph类型的消息发送到decoded_vgraph上，我们待会可以看到它的调用。graph_viz_pub_同样的，将MarkerArray发送到/graph_decoder_viz上。

接着初始化加载Params 也就是this->LoadParmas();

void GraphDecoder::LoadParmas() {const std::string prefix = "/graph_decoder/";//  param(param_name,param_val,param_default_val)  这个param_name必须要在parameter server里能找得到//  /graph_decoder/"world_frame 这个必须要在parameter server里能找得到/*在launch里：<node pkg="graph_decoder" type="graph_decoder" name="graph_decoder" output="screen"><rosparam command="load" file="$(find graph_decoder)/config/default.yaml" /><remap from="/planner_nav_graph" to="$(arg graph_topic)"/> </node>在launch提到的default.yaml里：world_frame                             : mapvisual_scale_ratio                      : 0.5*/nh.param<std::string>(prefix + "world_frame", gd_params_.frame_id, "map");nh.param<float>(prefix + "visual_scale_ratio", gd_params_.viz_scale_ratio, 1.0);}

这LoadParam就是在对应的launch里和yaml配置文件里找到相应的param。这么做是为了方便调参，我们改yaml的配置文件即可调整参数。

save_graph_sub_ = nh.subscribe("/save_file_dir", 5, &GraphDecoder::SaveGraphCallBack, this);

接着，这里定义了一个save_graph_sub_，订阅/save_file_dir，当订阅到信息后，执行它的回调函数SaveGraphCallBack。

补充一下：c++中的lambda表达式是c++11引用的最重要的特性之一，它定义了一个匿名函数，可以捕获一定范围的变量在函数内部使用，一般有如下语法形式：
auto func = [capture] (params) opt -> ret { func_body; };
一个完整的lambda表达式：
auto func1 = [](int a) -> int { return a + 1; };
auto func2 = [](int a) { return a + 2; };
cout << func1(1) << " " << func2(2) << endl;

我们来看看它订阅节点，再看它的回调函数 SaveGraphCallBack：

其中，订阅节点在teleop_panel.cpp 中声明了save_publisher_：

save_publisher_ = nh_.advertise<std_msgs::String>( "/save_file_dir", 5 );

在teleop_panel.cpp 中可以看到保存地图信息的qt button部分的代码：

void TeleopPanel::pressButton4()
{if ( ros::ok() && velocity_publisher_ ){QString qFilename = QFileDialog::getSaveFileName(this, tr("Save File"), "/", tr("VGH - Visibility Graph Files (*.vgh)"));std::string filename = qFilename.toStdString();if (filename != "") {int length = filename.length();if (length < 4) {filename += ".vgh";} else if (filename[length - 4] != '.' || filename[length - 3] != 'v' || filename[length - 2] != 'g' || filename[length - 1] != 'h') {filename += ".vgh";}}std_msgs::String msg;msg.data = filename;save_publisher_.publish(msg);}
}

其中就有std_msg::String msg 以及它的data属性

save_publisher_.publish(msg);

从这里也可以看到save_publisher_它发布了消息。所以SaveGraphCallBack就是来处理这个消息的。我们回到decoder_node.cpp中：

void GraphDecoder::SaveGraphCallBack(const std_msgs::StringConstPtr& msg) {if (received_graph_.empty()) return;                        //reveived_graph为空 直接返回const std::string file_path = msg->data;                    //把msg中的data提取出来 也就是teleop_panel.cpp中的file_nameif (file_path == "") return;                                //如果file_path为空 直接返回std::ofstream graph_file;                                   //定义输出流字节了graph_file.open(file_path);// Lambda functionauto OutputPoint3D = [&](const Point3D& p) {graph_file << std::to_string(p.x) << " ";graph_file << std::to_string(p.y) << " ";graph_file << std::to_string(p.z) << " ";};for (const auto& node_ptr : received_graph_) {graph_file << std::to_string(node_ptr->id) << " ";graph_file << std::to_string(static_cast<int>(node_ptr->free_direct)) << " ";OutputPoint3D(node_ptr->position);OutputPoint3D(node_ptr->surf_dirs.first);OutputPoint3D(node_ptr->surf_dirs.second);graph_file << std::to_string(static_cast<int>(node_ptr->is_covered))  << " ";graph_file << std::to_string(static_cast<int>(node_ptr->is_frontier)) << " ";graph_file << std::to_string(static_cast<int>(node_ptr->is_navpoint)) << " ";graph_file << std::to_string(static_cast<int>(node_ptr->is_boundary)) << " ";for (const auto& cidx : node_ptr->connect_idxs) {graph_file << std::to_string(cidx) << " ";}graph_file << "|" << " ";for (const auto& cidx : node_ptr->poly_idxs) {graph_file << std::to_string(cidx) << " ";} graph_file << "|" << " ";for (const auto& cidx : node_ptr->contour_idxs) {graph_file << std::to_string(cidx) << " ";}graph_file << "|" << " ";for (const auto& cidx : node_ptr->traj_idxs) {graph_file << std::to_string(cidx) << " ";}graph_file << "\n";}graph_file.close();
}

我们可以看看它保存的格式就明白了：

其存下来的格式为：

id NodeFreeDirect p.x p.y p.z surf_dirs.first.x surf_dirs.first.y surf_dirs.first.z surf_dirs.second.x surf_dirs.second.y surf_dirs.second.z is_covered is_frontier is_navpoint is_boundary connect_idx | poly_idx | contour_idx | traj_idx

SaveGraphCallBack函数这一部分就结束了

read_graph_sub_ = nh.subscribe("/read_file_dir", 5, &GraphDecoder::ReadGraphCallBack, this);

接下来我们来看它订阅的 /read_file_dir ，可以搜到也是从teletop_panel.cpp里写到的：

read_publisher_ = nh_.advertise<std_msgs::String>( "/read_file_dir", 5 );

void TeleopPanel::pressButton3()
{if ( ros::ok() && velocity_publisher_ ){QString qFilename = QFileDialog::getOpenFileName(this, tr("Read File"), "/", tr("VGH - Visibility Graph Files (*.vgh)"));std::string filename = qFilename.toStdString();std_msgs::String msg;msg.data = filename;read_publisher_.publish(msg);}
}

我们来看一下它执行的回调函数ReadGraphCallBack

void GraphDecoder::ReadGraphCallBack(const std_msgs::StringConstPtr& msg) {const std::string file_path = msg->data;if (file_path == "") return;std::ifstream graph_file(file_path);NodePtrStack loaded_graph;          //相当于之前的received_graphstd::string str;std::unordered_map<std::size_t, std::size_t> nodeIdx_idx_map;std::size_t ic = 0;NavNodePtr temp_node_ptr = NULL;loaded_graph.clear();// 读取vgh里面保存的内容了while (std::getline(graph_file, str)) {CreateNavNode(str, temp_node_ptr);if (AddNodePtrToGraph(temp_node_ptr, loaded_graph)) { nodeIdx_idx_map.insert({temp_node_ptr->id, ic});ic ++;}}for (const auto& node_ptr : loaded_graph) { // add connections to nodesAssignConnectNodes(nodeIdx_idx_map, loaded_graph, node_ptr->connect_idxs, node_ptr->connect_nodes);AssignConnectNodes(nodeIdx_idx_map, loaded_graph, node_ptr->poly_idxs, node_ptr->poly_connects);AssignConnectNodes(nodeIdx_idx_map, loaded_graph, node_ptr->contour_idxs, node_ptr->contour_connects);AssignConnectNodes(nodeIdx_idx_map, loaded_graph, node_ptr->traj_idxs, node_ptr->traj_connects);}this->VisualizeGraph(loaded_graph);visibility_graph_msg::Graph graph_msg;ConvertGraphToMsg(loaded_graph, graph_msg);graph_pub_.publish(graph_msg);
}

我们重点来看读取.vgh里面内容并且把它放进去成为loaded_graph的这个循环：

   while (std::getline(graph_file, str)) {                把当前一行行读出来 赋值给strCreateNavNode(str, temp_node_ptr);if (AddNodePtrToGraph(temp_node_ptr, loaded_graph)) { nodeIdx_idx_map.insert({temp_node_ptr->id, ic});ic ++;}}

这里重载了CreateNavNode函数，他输入的是str和temp_node_ptr。在循环里，str是读到的当前行，我们看看这个CreateNavNode函数：

void GraphDecoder::CreateNavNode(std::string str, NavNodePtr& node_ptr) {node_ptr = std::make_shared<NavNode>();std::vector<std::string> components;std::size_t pos = 0;    std::string delimiter = " ";                                //分隔符//pos到达分隔符，把从0-pos的字串读出来，放到c,判断c的长度是否>0//如果大于0 那么就把c存到components的vector容器里。//再把str的0到pos+pos + delimiter.length()的部分删掉//比如 "hei yo"  ->  "yo"while ((pos = str.find(delimiter)) != std::string::npos) {  //npos表示size_t的最大值std::string c = str.substr(0, pos);if (c.length() > 0) {components.push_back(str.substr(0, pos));}str.erase(0, pos + delimiter.length());}bool is_connect, is_poly, is_contour;is_connect = is_poly = is_contour = false;node_ptr->connect_idxs.clear(), node_ptr->contour_idxs.clear(), node_ptr->traj_idxs.clear();for (std::size_t i=0; i<components.size(); i++) {if (i == 0) node_ptr->id = (std::size_t)std::stoi(components[i]);if (i == 1) node_ptr->free_direct = static_cast<NodeFreeDirect>(std::stoi(components[i]));// positionif (i == 2) node_ptr->position.x = std::stof(components[i]);if (i == 3) node_ptr->position.y = std::stof(components[i]);if (i == 4) node_ptr->position.z = std::stof(components[i]);// surface direcion firstif (i == 5) node_ptr->surf_dirs.first.x = std::stof(components[i]);if (i == 6) node_ptr->surf_dirs.first.y = std::stof(components[i]);if (i == 7) node_ptr->surf_dirs.first.z = std::stof(components[i]);// surface direction secondif (i == 8)  node_ptr->surf_dirs.second.x = std::stof(components[i]);if (i == 9)  node_ptr->surf_dirs.second.y = std::stof(components[i]);if (i == 10) node_ptr->surf_dirs.second.z = std::stof(components[i]);// node internal infosif (i == 11) node_ptr->is_covered  = std::stoi(components[i]) == 0 ? false : true;if (i == 12) node_ptr->is_frontier = std::stoi(components[i]) == 0 ? false : true;if (i == 13) node_ptr->is_navpoint = std::stoi(components[i]) == 0 ? false : true;if (i == 14) node_ptr->is_boundary = std::stoi(components[i]) == 0 ? false : true;if (i  > 14 && !is_connect && !is_poly && !is_contour) {if (components[i] != "|") {node_ptr->connect_idxs.push_back((std::size_t)std::stoi(components[i]));} else {is_connect = true;}} else if (i > 14 && is_connect && !is_poly && !is_contour) {if (components[i] != "|") {node_ptr->poly_idxs.push_back((std::size_t)std::stoi(components[i]));} else {is_poly = true;}} else if (i > 14 && is_connect && is_poly && !is_contour) {if (components[i] != "|") {node_ptr->contour_idxs.push_back((std::size_t)std::stoi(components[i]));} else {is_contour = true;}} else if (i > 14 && is_connect && is_poly && is_contour) {node_ptr->traj_idxs.push_back((std::size_t)std::stoi(components[i]));}}
}

这样子处理好以后就得到了一个由str “填充” 好的temp_node_ptr 相当于把一个Navpoint的属性填充好了。接着看while循环里剩下的：

if (AddNodePtrToGraph(temp_node_ptr, loaded_graph)) { nodeIdx_idx_map.insert({temp_node_ptr->id, ic});ic ++;}

这里面的loaded_graph相当于之前的received_graph，这一部分和GraphCallBack之前的一样。用AddNodePtrToGraph来判断temp_node_ptr是否在loaded_graph里，如果在的话，往nodeIdx_idx_map里插入{id, ic}，不同的是它多了个ic++。

同时AddNodePtrToGraph本身如果node_ptr非空也就是上面的temp_node_ptr非空，他会执行操作graphOut.push_back(node_ptr) 也就是把temp_node_ptr添加进loaded_graph。

接下来再看ReadGraphCallBack剩下的部分：

for (const auto& node_ptr : loaded_graph) { // add connections to nodesAssignConnectNodes(nodeIdx_idx_map, loaded_graph, node_ptr->connect_idxs, node_ptr->connect_nodes);AssignConnectNodes(nodeIdx_idx_map, loaded_graph, node_ptr->poly_idxs, node_ptr->poly_connects);AssignConnectNodes(nodeIdx_idx_map, loaded_graph, node_ptr->contour_idxs, node_ptr->contour_connects);AssignConnectNodes(nodeIdx_idx_map, loaded_graph, node_ptr->traj_idxs, node_ptr->traj_connects);}this->VisualizeGraph(loaded_graph);visibility_graph_msg::Graph graph_msg;ConvertGraphToMsg(loaded_graph, graph_msg);graph_pub_.publish(graph_msg);

首先是建立node之间的连接这个和之前一样。然后通过VisualizeGraph函数去画出来。再把loaded_graph转变成graph_msg

来看看ConvertGraphToMsg，这里主要是填充了Msg里的header，其中EncodeGraph才是最主要的函数。

//ConvertGraphToMsg 把grap变成graph_msginline void ConvertGraphToMsg(const NodePtrStack& graph, visibility_graph_msg::Graph& graph_msg) {graph_msg.header.frame_id = gd_params_.frame_id;graph_msg.header.stamp = ros::Time::now();graph_msg.robot_id = robot_id_;//这个 EncodeGraph 才是最主要的从graph -> graph_msgEncodeGraph(graph, graph_msg);}

来看看 EncodeGraph

void GraphDecoder::EncodeGraph(const NodePtrStack& graphIn, visibility_graph_msg::Graph& graphOut) {graphOut.nodes.clear();                 //先清空node[]const std::string frame_id = graphOut.header.frame_id;// 循环graphIn里的每一个值，赋给node_ptr指针for (const auto& node_ptr : graphIn) {//这里面又嵌套了Node消息的内容，所以本质就是 node->graph //因此graph的消息包含了nodevisibility_graph_msg::Node msg_node;msg_node.header.frame_id = frame_id;msg_node.position    = ToGeoMsgP(node_ptr->position);msg_node.id          = node_ptr->id;msg_node.FreeType    = static_cast<int>(node_ptr->free_direct);msg_node.is_covered  = node_ptr->is_covered;msg_node.is_frontier = node_ptr->is_frontier;msg_node.is_navpoint = node_ptr->is_navpoint;msg_node.is_boundary = node_ptr->is_boundary;msg_node.surface_dirs.clear();msg_node.surface_dirs.push_back(ToGeoMsgP(node_ptr->surf_dirs.first));msg_node.surface_dirs.push_back(ToGeoMsgP(node_ptr->surf_dirs.second));// Encode connectionsmsg_node.connect_nodes.clear();for (const auto& cid : node_ptr->connect_idxs) {msg_node.connect_nodes.push_back(cid);}msg_node.poly_connects.clear();for (const auto& cid : node_ptr->poly_idxs) {msg_node.poly_connects.push_back(cid);}msg_node.contour_connects.clear();for (const auto& cid : node_ptr->contour_idxs) {msg_node.contour_connects.push_back(cid);}msg_node.trajectory_connects.clear();for (const auto& cid : node_ptr->traj_idxs) {msg_node.trajectory_connects.push_back(cid);}graphOut.nodes.push_back(msg_node);}graphOut.size = graphOut.nodes.size();
}

大意就是传入loaded_graph，目标是解析它，并把它放到graph_msg和node的消息里。

graph_pub_.publish(graph_msg) 最后把graph_msg的消息pub到 decoded_vgraph。

read_graph_sub_ 这部分也就结束了。

Far planner代码系列（2）相关推荐

Far planner代码系列（1）
读了大神的论文以后自己动手跑了一下,感觉很有趣,想细读一下大神的代码,反正当作一个自娱自乐的系列好了~MichaelFYang/far_planner: Fast, Attemptable Route ...
Far planner 代码系列（33）关于real_world_contour和contour_graph
我们知道far planner前部分的流程是这样子的: 对于特定的地图,如下: 我们要从surround_obs_cloud里抽取生成realworld_contour_,如下图所示: 在这时候,每个 ...
Far planner代码系列（31）is_covered
今天讲的是UpdateNavGraph函数里的分析每个node的is_covered属性和is_frontier属性. 我们看代码,代码里把near_nav_nodes里的点挨个都取出来判断该node ...
深入学习SAP UI5框架代码系列之八：谈谈 SAP UI5 的视图控件 ID，以及 SAP UI5 视图和 Angular 视图的异同
今天是 2021 年 4 月 27 日,周二,SAP 全球心理健康日.SAP 全球的员工,今天放假一天. 这不,早在上周五,我所在的 SAP Spartacus 开发团队的开发经理,就贴心地在 Sla ...
深入学习SAP UI5框架代码系列之七：控件数据绑定的三种模式 - One Way, Two Way和OneTime实现原理比较
这是Jerry 2021年的第 8 篇文章,也是汪子熙公众号总共第 279 篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加 ...
深入学习SAP UI5框架代码系列之六：SAP UI5控件数据绑定的实现原理
这是Jerry 2021年的第 7 篇文章,也是汪子熙公众号总共第 278 篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加 ...
深入学习SAP UI5框架代码系列之五：SAP UI5控件的实例数据修改和读取逻辑
这是Jerry 2021年的第6篇文章,也是汪子熙公众号总共第277篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加载机制 ...
深入学习SAP UI5框架代码系列之四：SAP UI5控件的元数据实现
这是Jerry 2021年的第5篇文章,也是汪子熙公众号总共第276篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) UI5 module懒加载机制 (2) ...
深入学习SAP UI5框架代码系列之三：HTML原生事件 VS UI5 Semantic事件
这是Jerry 2020年的第80篇文章,也是汪子熙公众号总共第262篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) UI5 module懒加载机制 (2) ...

Far planner代码系列（2）

graph_decoder系列（2）

Far planner代码系列（2）相关推荐

最新文章

热门文章