读了大神的论文以后自己动手跑了一下，感觉很有趣，想细读一下大神的代码，反正当作一个自娱自乐的系列好了~MichaelFYang/far_planner: Fast, Attemptable Route Planner for Navigation in Known and Unknown Environments (github.com)

graph_decoder系列（1）

Node [/graph_decoder]
Publications:* /decoded_vgraph [visibility_graph_msg/Graph]* /graph_decoder_viz [visualization_msgs/MarkerArray]* /rosout [rosgraph_msgs/Log]Subscriptions:* /read_file_dir [std_msgs/String]* /robot_vgraph [visibility_graph_msg/Graph]* /save_file_dir [std_msgs/String]Services:* /graph_decoder/get_loggers* /graph_decoder/set_logger_level* /request_graph_servicecontacting node http://DESKTOP-DT3VKG6:41593/ ...
Pid: 4979
Connections:* topic: /rosout* to: /rosout* direction: outbound (60537 - 127.0.0.1:59588) [11]* transport: TCPROS* topic: /decoded_vgraph* to: /far_planner* direction: outbound (60537 - 127.0.0.1:59624) [14]* transport: TCPROS* topic: /robot_vgraph* to: /far_planner (http://DESKTOP-DT3VKG6:39071/)* direction: inbound (44532 - DESKTOP-DT3VKG6:53855) [10]* transport: TCPROS* topic: /save_file_dir* to: /far_rviz (http://DESKTOP-DT3VKG6:36425/)* direction: inbound (49746 - DESKTOP-DT3VKG6:43299) [18]* transport: TCPROS* topic: /read_file_dir* to: /far_rviz (http://DESKTOP-DT3VKG6:36425/)* direction: inbound (49744 - DESKTOP-DT3VKG6:43299) [17]* transport: TCPROS

一        先看一下 src/graph_decoder目录里的结构：

  1. config->default.yaml 存放Graph Decoder的默认参数 world_frame和visual_scale_ratio 在decoder_node.cpp中初始化中LoadParam会用到。
  2. include/graph_decoder  point_struct.h存放Point点云的定义的结构体，其中包括点云坐标的加减乘除、对比、点云哈希等。decoder_node.h最主要建立了GraphDecoder的类，定义了一个NavNode的结构体，在类中申明了大量的函数。这些函数都会在decoder_node.cpp中进行定义。
    class GraphDecoder {
public:GraphDecoder() = default;~GraphDecoder() = default;void Init();void Loop();private://  GraphDecoder 含有的参数   ros::NodeHandle nh;                                 //创建句柄nh 也就是把NodeHandle实例化了ros::Subscriber graph_sub_;                         //Subscriber创建了一个  graph_sub_  的对象ros::Subscriber save_graph_sub_, read_graph_sub_;   //Subscriber创建了一个  save_graph_sub_  和一个read_graph_sub_  的对象ros::Publisher  graph_pub_, graph_viz_pub_;         //Publisher创建了一个   graph_pub_  和一个  graph_viz_pub_  的对象/*  graph_sub_  订阅  graphsave_graph_sub_ 订阅 save_graphread_graph_sub_ 订阅 read_graph*/ ros::ServiceServer request_graph_service_;      //Server创建 request_graph_service_ 服务器端GraphDecoderParams gd_params_;                  //创建gd_params_的结构体对象 里面有frame_id和viz_scale_ratioNodePtrStack received_graph_;                   //创建NodePtrStack指针栈 看名字应该是存收到的graphMarkerArray graph_marker_array_;                // MarkerArray 是msg消息的一种类型，里面有很多东西std::size_t robot_id_;void LoadParmas();void SetMarker(const VizColor& color, const std::string& ns,const float scale,const float alpha, Marker& scan_marker);void SetColor(const VizColor& color, const float alpha, Marker& scan_marker);void GraphCallBack(const visibility_graph_msg::GraphConstPtr& msg);void EncodeGraph(const NodePtrStack& graphIn, visibility_graph_msg::Graph& graphOut);void SaveGraphCallBack(const std_msgs::StringConstPtr& msg);void ReadGraphCallBack(const std_msgs::StringConstPtr& msg);bool SaveGraphService(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);bool ReadGraphFromFile(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);bool RequestGraphService(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); void VisualizeGraph(const NodePtrStack& graphIn);void CreateNavNode(std::string str, NavNodePtr& node_ptr);void CreateNavNode(const visibility_graph_msg::Node& msg, NavNodePtr& node_ptr);void AssignConnectNodes(const std::unordered_map<std::size_t, std::size_t>& idxs_map,const NodePtrStack& graph,std::vector<std::size_t>& node_idxs,std::vector<NavNodePtr>& connects);//在ROS的 geometry_msgs::Point 下加入了一个内联函数  ToGeoMsgP
//把当前的点拷贝一份 返回geo_ptemplate <typename Point>geometry_msgs::Point inline ToGeoMsgP(const Point& p) {geometry_msgs::Point geo_p;geo_p.x = p.x;geo_p.y = p.y; geo_p.z = p.z;return geo_p;}//判断 'elem' 在不在 'T_stack' 里template <typename T>bool IsTypeInStack(const T& elem, const std::vector<T>& T_stack) {if (std::find(T_stack.begin(), T_stack.end(), elem) != T_stack.end()) {return true;}return false;}//ResetGraph 就把NodePtrStack& graphOut给清空了  所以NodePtrStack 应该有好多个不同的对象inline void ResetGraph(NodePtrStack& graphOut) {graphOut.clear();}//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);}//判断NavNodePtr& node_ptr 是否加入到了NodePtrStack& graphOut里
//如果node_ptr不为空 那就加入进去inline bool AddNodePtrToGraph(const NavNodePtr& node_ptr, NodePtrStack& graphOut) {if (node_ptr != NULL) {graphOut.push_back(node_ptr);return true;}return false;}};
    struct NavNode {NavNode() = default;std::size_t id;                 //定义当前Node的idPoint3D position;               //定义一个3D点云NodeFreeDirect free_direct;     //定义free_direct 看不懂 什么 未知0；凸1；凹2；支柱3 不明白什么意思PointPair surf_dirs;            //定义一个点对bool is_covered;bool is_frontier;bool is_navpoint;bool is_boundary;std::vector<std::size_t> connect_idxs;                  //创建connect_idxs的vector   这个connect_idxs拿存放图的id。也就是graphstd::vector<std::shared_ptr<NavNode>> connect_nodes;    //创建connect_nodes的vector  里面存放的是导航时候用到的Node把？std::vector<std::size_t> poly_idxs;                     //创建poly_idxs 这个是拿来存放障碍物的边的id的std::vector<std::shared_ptr<NavNode>> poly_connects;    //poly_connects用来存放障碍物的nav_ptr的 相当于点集合。std::vector<std::size_t> contour_idxs;                  //contour 相当于boundary，这个是用来存放boundary的点的id的。std::vector<std::shared_ptr<NavNode>> contour_connects; //ontour_connects 存放可以连接的属于边界的nav_ptrstd::vector<std::size_t> traj_idxs;                     //traj_idxs 存放轨迹点的idstd::vector<std::shared_ptr<NavNode>> traj_connects;    //traj_connects 存放连起来的轨迹？
};
  3. launch->decoder.launch 最主要的还是LoadParam会用到它其中的参数 
    <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>
    这里面定义了 rosparam 并指向graph_decoder/config/default.yaml，从而解析到world_frame和visual_scale_ratio （这个是调节显示的时候point大小的尺寸的系数）
  4. src->decoder_node.cpp 这里面主要实现了decoder_node.h中的类函数们的定义，以及 GraphDecoder 类对象的实例化

二         src/graph_decoder/decoder_node.cpp：

int main(int argc, char** argv){ros::init(argc, argv, "graph_decoder_node");GraphDecoder gd_node;gd_node.Init();gd_node.Loop();
}

首先进行了ros的初始化 然后实例化了一个 GraphDecoder gd_node对象，调用Init()对其进行初始化。

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_);
}

这里初始化的东西很多，大部分是ros里的Publisher和Subscriber。我们一个个来看。

grap_sub_  是decoder_node.h中的 ros::Subscriber graph_sub_  。之后的我不在一个个列举了，大家看看decoder_node.h就明白。

这里也说一下subscriber（topic,queue_size,fp,obj）其中topic就是节点的名称（string类型），queue_size就是数据大小，fp是当消息执行到这一步需要调用的回调函数，obj是指针（this就是指向GraphDecoder自己的对象指针。

void GraphDecoder::GraphCallBack(const visibility_graph_msg::GraphConstPtr& msg) {//  visibility_graph_msg/Node[] nodes//shared_graph是它收到的msg的一个指针，这代表什么呢？代表里面有好多nodesconst visibility_graph_msg::Graph shared_graph = *msg;this->ResetGraph(received_graph_);                              //调用ResetGraph清空received_graph_指针栈NavNodePtr temp_node_ptr = NULL;                                //建立一个临时指针temp_node_ptrrobot_id_ = msg->robot_id;                                      //传入robot_idstd::unordered_map<std::size_t, std::size_t> nodeIdx_idx_map;   //创建一个无序map容器nodeIdx_idx_map//unordered_map 容器底层采用的是哈希表存储结构，该结构本身不具有对数据的排序功能，所以此容器内部不会自行对存储的键值对进行排序。for (std::size_t i=0; i<shared_graph.nodes.size(); i++) {       //把当前的nodes进行循环遍历const auto node = shared_graph.nodes[i];CreateNavNode(node, temp_node_ptr);                         //创建navnode 并对结构体内部的数据赋值 其值来源于msg消息//temp_node_ptr就是当前的临时navnode指针。// 返回true的话，在AddNodePtrToGraph层级里 把temp_node_ptr加入到received_graph_里if (AddNodePtrToGraph(temp_node_ptr, received_graph_)) {//在nodeIdx_idx_map中插入i和node.idnodeIdx_idx_map.insert({node.id, i});   }}// add connections to nodesfor (const auto& node_ptr : received_graph_) { AssignConnectNodes(nodeIdx_idx_map, received_graph_, node_ptr->connect_idxs, node_ptr->connect_nodes);AssignConnectNodes(nodeIdx_idx_map, received_graph_, node_ptr->poly_idxs, node_ptr->poly_connects);AssignConnectNodes(nodeIdx_idx_map, received_graph_, node_ptr->contour_idxs, node_ptr->contour_connects);AssignConnectNodes(nodeIdx_idx_map, received_graph_, node_ptr->traj_idxs, node_ptr->traj_connects);}// ROS_INFO("Graph extraction completed. Total size of graph nodes: %ld", received_graph_.size());this->VisualizeGraph(received_graph_);
}

我们先看看流程图是怎么样的：PS.我个人的理解

这个Callback函数都干了啥，首先把msg消息读进来，创建shared_graph指针，其中包含所有当前读取到的Node。清空received_graph_指针栈，建立一个Navnode临时指针temp_node_ptr，创建一个无序map容器nodeIdx_idx_map

        开始遍历循环：从src/msg/Graph.msg里的visibility_graph_msg/Node[] nodes读取nodes的size，进行循环遍历。创建 navnode 并对结构体内部的数据赋值 其值来源于msg/Node.msg

总之，目前的作用就是，读取msg里所有的node 我们可以叫做 original nodes，然后再把original nodes 变成navnode的node  听起来有点绕~

node_ptr->is_covered  = msg.is_covered;

node_ptr->is_frontier = msg.is_frontier;

node_ptr->is_navpoint = msg.is_navpoint; 

node_ptr->is_boundary = msg.is_boundary;

所有的点都会变成Nav_node 再根据这些应来判断正在维护的node是属于哪部分的

用AddNodePtrToGraph来判断temp_node_ptr是否在received_graph_里，如果在的话，往nodeIdx_idx_map里插入{node.id, i}

注意！AddNodePtrToGraph本身如果node_ptr非空 也就是上面的temp_node_ptr非空，他会执行操作graphOut.push_back(node_ptr) 也就是把temp_node_ptr添加进received_graph_

    inline bool AddNodePtrToGraph(const NavNodePtr& node_ptr, NodePtrStack& graphOut) {if (node_ptr != NULL) {graphOut.push_back(node_ptr);   return true;}return false;}};
void GraphDecoder::CreateNavNode(const visibility_graph_msg::Node& msg,NavNodePtr& node_ptr)
{//node_ptr就是NavNode结构体的指针node_ptr = std::make_shared<NavNode>();                                         //使用make_shared进行初始化NavNode结构体node_ptr->position = Point3D(msg.position.x, msg.position.y, msg.position.z);   //把点云的x,y,z获取进来node_ptr->id = msg.id;                                                          //赋id// static_cast <type-id> ( expression ) 将表达式转换为 type-id 的类型node_ptr->free_direct = static_cast<NodeFreeDirect>(msg.FreeType);              //把msg.FreeType类型转换为NodeFreeDirect// msg是对应src/msg中的Node.msg和Graph.msg/*msg里 geometry_msgs/Point[] surface_dirs  surface_dirs是Point[]类型的。 Point[0] 表示第一个点， Point[0].x表示第一个点的x坐标值 以此类推*///surface_dir是一个存放point的数组// 存了一个surf_dirs的点对 <point1,point2>if (msg.surface_dirs.size() != 2) {ROS_ERROR_THROTTLE(1.0, "node surface directions error.");node_ptr->surf_dirs.first = node_ptr->surf_dirs.second = Point3D(0,0,-1);} else {node_ptr->surf_dirs.first  = Point3D(msg.surface_dirs[0].x,msg.surface_dirs[0].y,0.0f);node_ptr->surf_dirs.second = Point3D(msg.surface_dirs[1].x,msg.surface_dirs[1].y,0.0f);}node_ptr->is_covered  = msg.is_covered;node_ptr->is_frontier = msg.is_frontier;node_ptr->is_navpoint = msg.is_navpoint;node_ptr->is_boundary = msg.is_boundary;// assigan connection idxs// 将navnode里的所有的idxs的vector容器清空// 每次都清空所有的idxs 然后再把msg里读到的所有idxs在写进去，这样就维护了一定长度的idxs，把过早的idxs丢弃?我是这么觉得的node_ptr->connect_idxs.clear(), node_ptr->poly_idxs.clear(), node_ptr->contour_idxs.clear(), node_ptr->traj_idxs.clear();//for(auto &a:b)中加了引用符号，可以对容器中的内容进行赋值，即可通过对a赋值来做到容器b的内容填充//cid是msg.connect_nodes[]里的值，循环i次就是msg.connect_nodes[i]for (const auto& cid : msg.connect_nodes) {node_ptr->connect_idxs.push_back((std::size_t)cid); //难道是把对应connect_nodes的东西放到connect_idxs里？}for (const auto& cid : msg.poly_connects) {node_ptr->poly_idxs.push_back((std::size_t)cid);}for (const auto& cid : msg.contour_connects) {node_ptr->contour_idxs.push_back((std::size_t)cid);}for (const auto& cid : msg.trajectory_connects) {node_ptr->traj_idxs.push_back((std::size_t)cid);}//把connect_nodes、poly_connects、contour_connects、traj_connects都清空 node_ptr->connect_nodes.clear(), node_ptr->poly_connects.clear(), node_ptr->contour_connects.clear(), node_ptr->traj_connects.clear();
}

遍历循环建立相同的nodes之间的连接关系

for(auto &a:b)中加了引用符号，可以对容器中的内容进行赋值，即可通过对a赋值来做到容器b的内容填充

for (const auto& node_ptr : received_graph_) { AssignConnectNodes(nodeIdx_idx_map, received_graph_, node_ptr->connect_idxs, node_ptr->connect_nodes);AssignConnectNodes(nodeIdx_idx_map, received_graph_, node_ptr->poly_idxs, node_ptr->poly_connects);AssignConnectNodes(nodeIdx_idx_map, received_graph_, node_ptr->contour_idxs, node_ptr->contour_connects);AssignConnectNodes(nodeIdx_idx_map, received_graph_, node_ptr->traj_idxs, node_ptr->traj_connects);}// ROS_INFO("Graph extraction completed. Total size of graph nodes: %ld", received_graph_.size());this->VisualizeGraph(received_graph_);

让我们来看看AssignConnectNodes：

void GraphDecoder::AssignConnectNodes(const std::unordered_map<std::size_t, std::size_t>& idxs_map,const NodePtrStack& graph,std::vector<std::size_t>& node_idxs,std::vector<NavNodePtr>& connects)
{std::vector<std::size_t> clean_idx;connects.clear();                                           //清空传入的connectsfor (const auto& cid : node_idxs) {const auto it = idxs_map.find(cid);                     //nodeIdx_idx_map中找到各个idxs里的cid赋值给itif (it != idxs_map.end()) {const std::size_t idx = idxs_map.find(cid)->second; //idxs_map 里是{node.id, i}，其中i就是上组循环时传入的数字，这里把i赋值给idxconst NavNodePtr cnode_ptr = graph[idx];            //cnode_ptr  =  received_graph_[idx]connects.push_back(cnode_ptr);                      //把cnode_ptr添加到connects里clean_idx.push_back(cnode_ptr->id);                 //把conde_ptr里的id添加到clean_idx里}}node_idxs = clean_idx;                                      //把各个idxs重新用clean_idx赋值
}

经过 AssignConnectNodes函数以后，得到新的各个idxs

最后是GraphCallBack函数最后调用的VisualizeGraph函数 传入的是received_graph_

这个是可视化，用到了 visualization_msgs/Marker 我们先看看Marker

DisplayTypes/Marker

http://wiki.ros.org/rviz/DisplayTypes/Marker
uint8 ARROW=0//箭头
uint8 CUBE=1//立方体
uint8 SPHERE=2//球
uint8 CYLINDER=3//圆柱体
uint8 LINE_STRIP=4//线条（点的连线）
uint8 LINE_LIST=5//线条序列
uint8 CUBE_LIST=6//立方体序列
uint8 SPHERE_LIST=7//球序列
uint8 POINTS=8//点集
uint8 TEXT_VIEW_FACING=9//显示3D的文字
uint8 MESH_RESOURCE=10//网格？
uint8 TRIANGLE_LIST=11//三角形序列
//对标记的操作
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3Header header
string ns   //命名空间namespace，就是你理解的那样
int32 id    //与命名空间联合起来，形成唯一的id，这个唯一的id可以将各个标志物区分开来，使得程序可以对指定的标志物进行操作
int32 type  //类型
int32 action    //操作，是添加还是修改还是删除
geometry_msgs/Pose pose       # Pose of the object
geometry_msgs/Vector3 scale   # Scale of the object 1,1,1 means default (usually 1 meter square)
std_msgs/ColorRGBA color      # Color [0.0-1.0]
duration lifetime             # How long the object should last before being automatically deleted.  0 means forever
bool frame_locked             # If this marker should be frame-locked, i.e. retransformed into its frame every timestep#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
geometry_msgs/Point[] points//这个是在序列、点集中才会用到，指明序列中每个点的位置
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
#number of colors must either be 0 or equal to the number of points
#NOTE: alpha is not yet used
std_msgs/ColorRGBA[] colors# NOTE: only used for text markers
string text# NOTE: only used for MESH_RESOURCE markers
string mesh_resource
bool mesh_use_embedded_materials

VisualizeGraph函数

void GraphDecoder::VisualizeGraph(const NodePtrStack& graphIn) {MarkerArray graph_marker_array;Marker nav_node_marker, covered_node_marker, frontier_node_marker, internav_node_marker, edge_marker, poly_edge_marker, contour_edge_marker, free_edge_marker, traj_edge_marker, boundary_edge_marker, corner_surf_marker, corner_helper_marker;nav_node_marker.type       = Marker::SPHERE_LIST;covered_node_marker.type   = Marker::SPHERE_LIST;internav_node_marker.type  = Marker::SPHERE_LIST;frontier_node_marker.type  = Marker::SPHERE_LIST;edge_marker.type           = Marker::LINE_LIST;poly_edge_marker.type      = Marker::LINE_LIST;free_edge_marker.type      = Marker::LINE_LIST;boundary_edge_marker.type  = Marker::LINE_LIST;contour_edge_marker.type   = Marker::LINE_LIST;traj_edge_marker.type      = Marker::LINE_LIST;corner_surf_marker.type    = Marker::LINE_LIST;corner_helper_marker.type  = Marker::CUBE_LIST;this->SetMarker(VizColor::WHITE,   "global_vertex",     0.5f,  0.5f,  nav_node_marker);this->SetMarker(VizColor::BLUE,    "freespace_vertex",  0.5f,  0.8f,  covered_node_marker);this->SetMarker(VizColor::YELLOW,  "trajectory_vertex", 0.5f,  0.8f,  internav_node_marker);this->SetMarker(VizColor::ORANGE,  "frontier_vertex",   0.5f,  0.8f,  frontier_node_marker);this->SetMarker(VizColor::WHITE,   "global_vgraph",     0.1f,  0.2f,  edge_marker);this->SetMarker(VizColor::EMERALD, "freespace_vgraph",  0.1f,  0.2f,  free_edge_marker);this->SetMarker(VizColor::EMERALD, "visibility_edge",   0.1f,  0.2f,  poly_edge_marker);this->SetMarker(VizColor::RED,     "polygon_edge",      0.15f, 0.2f,  contour_edge_marker);this->SetMarker(VizColor::GREEN,   "trajectory_edge",   0.1f,  0.5f,  traj_edge_marker);this->SetMarker(VizColor::ORANGE,  "boundary_edge",     0.2f,  0.25f, boundary_edge_marker);this->SetMarker(VizColor::YELLOW,  "vertex_angle",      0.15f, 0.75f, corner_surf_marker);this->SetMarker(VizColor::YELLOW,  "angle_direct",      0.25f, 0.75f, corner_helper_marker);/* Lambda Function */auto Draw_Edge = [&](const NavNodePtr& node_ptr) {geometry_msgs::Point p1, p2;p1 = ToGeoMsgP(node_ptr->position);for (const auto& cnode : node_ptr->connect_nodes) {p2 = ToGeoMsgP(cnode->position);edge_marker.points.push_back(p1);edge_marker.points.push_back(p2);}for (const auto& cnode : node_ptr->poly_connects) {p2 = ToGeoMsgP(cnode->position);poly_edge_marker.points.push_back(p1);poly_edge_marker.points.push_back(p2);if (node_ptr->is_covered && cnode->is_covered) {free_edge_marker.points.push_back(p1);free_edge_marker.points.push_back(p2);}}// contour edgesfor (const auto& ct_cnode : node_ptr->contour_connects) {p2 = ToGeoMsgP(ct_cnode->position);contour_edge_marker.points.push_back(p1);contour_edge_marker.points.push_back(p2);if (node_ptr->is_boundary && ct_cnode->is_boundary) {boundary_edge_marker.points.push_back(p1);boundary_edge_marker.points.push_back(p2);}}// inter navigation trajectory connectionsif (node_ptr->is_navpoint) {for (const auto& tj_cnode : node_ptr->traj_connects) {p2 = ToGeoMsgP(tj_cnode->position);traj_edge_marker.points.push_back(p1);traj_edge_marker.points.push_back(p2);}}};auto Draw_Surf_Dir = [&](const NavNodePtr& node_ptr) {geometry_msgs::Point p1, p2, p3;p1 = ToGeoMsgP(node_ptr->position);Point3D end_p;if (node_ptr->free_direct != NodeFreeDirect::PILLAR) {end_p = node_ptr->position + node_ptr->surf_dirs.first * gd_params_.viz_scale_ratio;p2 = ToGeoMsgP(end_p);corner_surf_marker.points.push_back(p1);corner_surf_marker.points.push_back(p2);corner_helper_marker.points.push_back(p2);end_p = node_ptr->position + node_ptr->surf_dirs.second * gd_params_.viz_scale_ratio;p3 = ToGeoMsgP(end_p);corner_surf_marker.points.push_back(p1);corner_surf_marker.points.push_back(p3);corner_helper_marker.points.push_back(p3);}};std::size_t idx = 0;const std::size_t graph_size = graphIn.size();nav_node_marker.points.resize(graph_size);for (const auto& nav_node_ptr : graphIn) {if (nav_node_ptr == NULL) {continue;}const geometry_msgs::Point cpoint = ToGeoMsgP(nav_node_ptr->position);nav_node_marker.points[idx] = cpoint;if (nav_node_ptr->is_navpoint) {internav_node_marker.points.push_back(cpoint);}if (nav_node_ptr->is_covered) {covered_node_marker.points.push_back(cpoint);}if (nav_node_ptr->is_frontier) {frontier_node_marker.points.push_back(cpoint);}Draw_Edge(nav_node_ptr);Draw_Surf_Dir(nav_node_ptr);idx ++;    } nav_node_marker.points.resize(idx);graph_marker_array.markers.push_back(nav_node_marker);graph_marker_array.markers.push_back(covered_node_marker);graph_marker_array.markers.push_back(internav_node_marker);graph_marker_array.markers.push_back(frontier_node_marker);graph_marker_array.markers.push_back(edge_marker);graph_marker_array.markers.push_back(poly_edge_marker);graph_marker_array.markers.push_back(boundary_edge_marker);graph_marker_array.markers.push_back(free_edge_marker);graph_marker_array.markers.push_back(contour_edge_marker);graph_marker_array.markers.push_back(traj_edge_marker);graph_marker_array.markers.push_back(corner_surf_marker);graph_marker_array.markers.push_back(corner_helper_marker);graph_viz_pub_.publish(graph_marker_array);
}

该函数定义了一系列Marker用来做显示化，其中，每个Marker的实例对象都分别调用了SetMarker进行设置。

SetMarker函数

void GraphDecoder::SetMarker(const VizColor& color, const std::string& ns,const float scale,const float alpha,  Marker& scan_marker)
{scan_marker.header.frame_id = gd_params_.frame_id;scan_marker.header.stamp = ros::Time::now();scan_marker.id = 0;scan_marker.ns = ns;scan_marker.action = Marker::ADD;scan_marker.scale.x = scan_marker.scale.y = scan_marker.scale.z = scale * gd_params_.viz_scale_ratio;scan_marker.pose.orientation.x = 0.0;scan_marker.pose.orientation.y = 0.0;scan_marker.pose.orientation.z = 0.0;scan_marker.pose.orientation.w = 1.0;scan_marker.pose.position.x = 0.0;scan_marker.pose.position.y = 0.0;scan_marker.pose.position.z = 0.0;this->SetColor(color, alpha, scan_marker);
}

header里包含 frame_id 和 stamp 剩下的部分参考Display/Marker，这里面又包含了SetColor函数，让我们接着看：

SetColor函数

void GraphDecoder::SetColor(const VizColor& color, const float alpha, Marker& scan_marker)
{std_msgs::ColorRGBA c;c.a = alpha;if (color == VizColor::RED) {c.r = 0.9f, c.g = c.b = 0.f;}else if (color == VizColor::ORANGE) {c.r = 1.0f, c.g = 0.45f, c.b = 0.1f;}else if (color == VizColor::BLACK) {c.r = c.g = c.b = 0.1f;}else if (color == VizColor::YELLOW) {c.r = c.g = 0.9f, c.b = 0.1;}else if (color == VizColor::BLUE) {c.b = 1.0f, c.r = 0.1f, c.g = 0.1f;}else if (color == VizColor::GREEN) {c.g = 0.9f, c.r = c.b = 0.f;}else if (color == VizColor::EMERALD) {c.g = c.b = 0.9f, c.r = 0.f;}else if (color == VizColor::WHITE) {c.r = c.g = c.b = 0.9f;}else if (color == VizColor::MAGNA) {c.r = c.b = 0.9f, c.g = 0.f;}else if (color == VizColor::PURPLE) {c.r = c.b = 0.5f, c.g = 0.f;}scan_marker.color = c;
}

接着我们继续看VisualizeGraph函数剩下的部分

/* Lambda Function */auto Draw_Edge = [&](const NavNodePtr& node_ptr) {geometry_msgs::Point p1, p2;p1 = ToGeoMsgP(node_ptr->position);for (const auto& cnode : node_ptr->connect_nodes) {p2 = ToGeoMsgP(cnode->position);edge_marker.points.push_back(p1);edge_marker.points.push_back(p2);}for (const auto& cnode : node_ptr->poly_connects) {p2 = ToGeoMsgP(cnode->position);poly_edge_marker.points.push_back(p1);poly_edge_marker.points.push_back(p2);if (node_ptr->is_covered && cnode->is_covered) {free_edge_marker.points.push_back(p1);free_edge_marker.points.push_back(p2);}}// contour edgesfor (const auto& ct_cnode : node_ptr->contour_connects) {p2 = ToGeoMsgP(ct_cnode->position);contour_edge_marker.points.push_back(p1);contour_edge_marker.points.push_back(p2);if (node_ptr->is_boundary && ct_cnode->is_boundary) {boundary_edge_marker.points.push_back(p1);boundary_edge_marker.points.push_back(p2);}}// inter navigation trajectory connectionsif (node_ptr->is_navpoint) {for (const auto& tj_cnode : node_ptr->traj_connects) {p2 = ToGeoMsgP(tj_cnode->position);traj_edge_marker.points.push_back(p1);traj_edge_marker.points.push_back(p2);}}};auto Draw_Surf_Dir = [&](const NavNodePtr& node_ptr) {geometry_msgs::Point p1, p2, p3;p1 = ToGeoMsgP(node_ptr->position);Point3D end_p;if (node_ptr->free_direct != NodeFreeDirect::PILLAR) {end_p = node_ptr->position + node_ptr->surf_dirs.first * gd_params_.viz_scale_ratio;p2 = ToGeoMsgP(end_p);corner_surf_marker.points.push_back(p1);corner_surf_marker.points.push_back(p2);corner_helper_marker.points.push_back(p2);end_p = node_ptr->position + node_ptr->surf_dirs.second * gd_params_.viz_scale_ratio;p3 = ToGeoMsgP(end_p);corner_surf_marker.points.push_back(p1);corner_surf_marker.points.push_back(p3);corner_helper_marker.points.push_back(p3);}};std::size_t idx = 0;const std::size_t graph_size = graphIn.size();nav_node_marker.points.resize(graph_size);for (const auto& nav_node_ptr : graphIn) {if (nav_node_ptr == NULL) {continue;}const geometry_msgs::Point cpoint = ToGeoMsgP(nav_node_ptr->position);nav_node_marker.points[idx] = cpoint;if (nav_node_ptr->is_navpoint) {internav_node_marker.points.push_back(cpoint);}if (nav_node_ptr->is_covered) {covered_node_marker.points.push_back(cpoint);}if (nav_node_ptr->is_frontier) {frontier_node_marker.points.push_back(cpoint);}Draw_Edge(nav_node_ptr);Draw_Surf_Dir(nav_node_ptr);idx ++;    } nav_node_marker.points.resize(idx);graph_marker_array.markers.push_back(nav_node_marker);graph_marker_array.markers.push_back(covered_node_marker);graph_marker_array.markers.push_back(internav_node_marker);graph_marker_array.markers.push_back(frontier_node_marker);graph_marker_array.markers.push_back(edge_marker);graph_marker_array.markers.push_back(poly_edge_marker);graph_marker_array.markers.push_back(boundary_edge_marker);graph_marker_array.markers.push_back(free_edge_marker);graph_marker_array.markers.push_back(contour_edge_marker);graph_marker_array.markers.push_back(traj_edge_marker);graph_marker_array.markers.push_back(corner_surf_marker);graph_marker_array.markers.push_back(corner_helper_marker);graph_viz_pub_.publish(graph_marker_array);
}

定义了两个函数 Draw_Edge和Draw_Surf_Dir，Draw_Edge就是把当前位置的node与connect_nodes里的点连线、与Polygon里面的点连线、与contour的点连线、最后就是路径trajectory连线。

    auto Draw_Edge = [&](const NavNodePtr& node_ptr) {geometry_msgs::Point p1, p2;            //定义两个点 p1 = ToGeoMsgP(node_ptr->position);     //p1是node_ptr传入的起始点，通过ToGeoMsgP拷贝过来//  connectfor (const auto& cnode : node_ptr->connect_nodes) {     //一个for循环，从p1所在的node_ptr里调用connect_nodes里的nodep2 = ToGeoMsgP(cnode->position);                    //将connect_nodes里的node挨个赋值给p2edge_marker.points.push_back(p1);                   //在edge_marker.points里放入p1edge_marker.points.push_back(p2);                   //在edge_marker.points里放入p2}//ploy_connect 应该是Polygon                            for (const auto& cnode : node_ptr->poly_connects) {     //多边形连接p2 = ToGeoMsgP(cnode->position);poly_edge_marker.points.push_back(p1);poly_edge_marker.points.push_back(p2);if (node_ptr->is_covered && cnode->is_covered) {   //如果这些node被标定为is_covered  那么就把他们放到free_edge_markerfree_edge_marker.points.push_back(p1);free_edge_marker.points.push_back(p2);}}// contour edges    轮廓边for (const auto& ct_cnode : node_ptr->contour_connects) {p2 = ToGeoMsgP(ct_cnode->position);contour_edge_marker.points.push_back(p1);contour_edge_marker.points.push_back(p2);if (node_ptr->is_boundary && ct_cnode->is_boundary) {boundary_edge_marker.points.push_back(p1);boundary_edge_marker.points.push_back(p2);}}// inter navigation trajectory connectionsif (node_ptr->is_navpoint) {for (const auto& tj_cnode : node_ptr->traj_connects) {p2 = ToGeoMsgP(tj_cnode->position);traj_edge_marker.points.push_back(p1);traj_edge_marker.points.push_back(p2);}}};

不知道这个corner_helper_marker具体的作用是什么

auto Draw_Surf_Dir = [&](const NavNodePtr& node_ptr) {geometry_msgs::Point p1, p2, p3;p1 = ToGeoMsgP(node_ptr->position);Point3D end_p;if (node_ptr->free_direct != NodeFreeDirect::PILLAR) {end_p = node_ptr->position + node_ptr->surf_dirs.first * gd_params_.viz_scale_ratio;p2 = ToGeoMsgP(end_p);corner_surf_marker.points.push_back(p1);corner_surf_marker.points.push_back(p2);corner_helper_marker.points.push_back(p2);end_p = node_ptr->position + node_ptr->surf_dirs.second * gd_params_.viz_scale_ratio;p3 = ToGeoMsgP(end_p);corner_surf_marker.points.push_back(p1);corner_surf_marker.points.push_back(p3);corner_helper_marker.points.push_back(p3);}};
 //这一部分就是来画图了std::size_t idx = 0;const std::size_t graph_size = graphIn.size();      //读取receive_graph的大小nav_node_marker.points.resize(graph_size);          //把nav_node_marker的size设置成receive_graph一样大。因为在receive_graph里所有的点都是Nav_nodefor (const auto& nav_node_ptr : graphIn) {if (nav_node_ptr == NULL) {continue;}const geometry_msgs::Point cpoint = ToGeoMsgP(nav_node_ptr->position);  //把当前的点读出来设成cpointnav_node_marker.points[idx] = cpoint;                                   //把cpoint填入nav_node_marker.points[]里//根据类型来划分if (nav_node_ptr->is_navpoint) {internav_node_marker.points.push_back(cpoint);}if (nav_node_ptr->is_covered) {covered_node_marker.points.push_back(cpoint);}if (nav_node_ptr->is_frontier) {frontier_node_marker.points.push_back(cpoint);}Draw_Edge(nav_node_ptr);Draw_Surf_Dir(nav_node_ptr);idx ++;    } nav_node_marker.points.resize(idx);graph_marker_array.markers.push_back(nav_node_marker);graph_marker_array.markers.push_back(covered_node_marker);graph_marker_array.markers.push_back(internav_node_marker);graph_marker_array.markers.push_back(frontier_node_marker);graph_marker_array.markers.push_back(edge_marker);graph_marker_array.markers.push_back(poly_edge_marker);graph_marker_array.markers.push_back(boundary_edge_marker);graph_marker_array.markers.push_back(free_edge_marker);graph_marker_array.markers.push_back(contour_edge_marker);graph_marker_array.markers.push_back(traj_edge_marker);graph_marker_array.markers.push_back(corner_surf_marker);graph_marker_array.markers.push_back(corner_helper_marker);//graph_viz_pub_发布消息graph_viz_pub_.publish(graph_marker_array);
}

最后init()里graph_sub_大概的流程应该是这样的，

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

  1. Far planner代码系列（2）

    graph_decoder系列(2) 承接上文 我们接着来看decoder_node.cpp的代码部分 void GraphDecoder::Init() {/* initialize subscri ...

  2. Far planner 代码系列（33） 关于real_world_contour和contour_graph

    我们知道far planner前部分的流程是这样子的: 对于特定的地图,如下: 我们要从surround_obs_cloud里抽取生成realworld_contour_,如下图所示: 在这时候,每个 ...

  3. Far planner代码系列（31）is_covered

    今天讲的是UpdateNavGraph函数里的分析每个node的is_covered属性和is_frontier属性. 我们看代码,代码里把near_nav_nodes里的点挨个都取出来判断该node ...

  4. 深入学习SAP UI5框架代码系列之八：谈谈 SAP UI5 的视图控件 ID，以及 SAP UI5 视图和 Angular 视图的异同

    今天是 2021 年 4 月 27 日,周二,SAP 全球心理健康日.SAP 全球的员工,今天放假一天. 这不,早在上周五,我所在的 SAP Spartacus 开发团队的开发经理,就贴心地在 Sla ...

  5. 深入学习SAP UI5框架代码系列之七：控件数据绑定的三种模式 - One Way, Two Way和OneTime实现原理比较

    这是Jerry 2021年的第 8 篇文章,也是汪子熙公众号总共第 279 篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加 ...

  6. 深入学习SAP UI5框架代码系列之六：SAP UI5控件数据绑定的实现原理

    这是Jerry 2021年的第 7 篇文章,也是汪子熙公众号总共第 278 篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加 ...

  7. 深入学习SAP UI5框架代码系列之五：SAP UI5控件的实例数据修改和读取逻辑

    这是Jerry 2021年的第6篇文章,也是汪子熙公众号总共第277篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加载机制 ...

  8. 深入学习SAP UI5框架代码系列之四：SAP UI5控件的元数据实现

    这是Jerry 2021年的第5篇文章,也是汪子熙公众号总共第276篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) UI5 module懒加载机制 (2) ...

  9. 深入学习SAP UI5框架代码系列之三：HTML原生事件 VS UI5 Semantic事件

    这是Jerry 2020年的第80篇文章,也是汪子熙公众号总共第262篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) UI5 module懒加载机制 (2) ...

最新文章

  1. 调用管道模型：高敏感、高性能
  2. 重磅！新一轮“双一流”，有重大变化！
  3. [20170728]oracle保留字.txt
  4. 一个身份证号码验证接口[2]
  5. centos6.5 设置ssh无密码登录
  6. latex 数学公式_技能分享——LaTeX篇I
  7. javascript 禁止复制网页
  8. vue 存储对象 不要监听_Vue源码解析----响应式原理
  9. Java中的抽象类与abstract关键字
  10. spark读取kafka数据 createStream和createDirectStream的区别
  11. 封包(一)(雷电模拟器+ProxyDroid+查尔斯3.93+WPE)
  12. 用科比一生的数据，回顾他的球场传奇
  13. web前端之JavaScript高级程序设计六：事件
  14. FontAwesome 字体图标库 使用
  15. 跨年巨作 13万字 腾讯高工手写JDK源码笔记 带你飙向实战
  16. 如何靠3D建模月入2W+？
  17. 零钱模拟器微信小程序源码下载
  18. android客户端框架,最新的一版，通用Android 客户端架构设计，只有你还没看过
  19. 常用m3u8，rtsp，rtmp，flv，mp4直播流在线测试地址
  20. 这种股权结构一定要远离！

热门文章

  1. 常用的几种GPRS模块介绍 GPRS模块选哪个好
  2. kuangbin J - Simpsons’ Hidden Talents
  3. mei yan xiao guo for android
  4. 不懂批判性思维，可能正在限制你的程序员生涯！
  5. “花式提涨薪，结果被套路”，不懂怎么跟老板提加薪？这个方法真的很好用
  6. 谱半径一定大于0_[转载]关于谱半径（spectrum radius）
  7. linux监控软件有哪些？用什么软件好？
  8. 【shell命令】拆分、合并、排序、比较文件
  9. AlphaGo Model-Based RL
  10. 人脸识别之人脸检测（二）--人脸识别样本制作及训练测试