Far planner代码系列（1）
读了大神的论文以后自己动手跑了一下，感觉很有趣，想细读一下大神的代码，反正当作一个自娱自乐的系列好了~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_service
contacting 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目录里的结构：
- config->default.yaml 存放Graph Decoder的默认参数 world_frame和visual_scale_ratio 在decoder_node.cpp中初始化中LoadParam会用到。
- 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_ 订阅 graph save_graph_sub_ 订阅 save_graph read_graph_sub_ 订阅 read_graph */ ros::ServiceServer request_graph_service_; //Server创建 request_graph_service_ 服务器端 GraphDecoderParams gd_params_; //创建gd_params_的结构体对象 里面有frame_id和viz_scale_ratio NodePtrStack received_graph_; //创建NodePtrStack指针栈 看名字应该是存收到的graph MarkerArray 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& connects); //在ROS的 geometry_msgs::Point 下加入了一个内联函数 ToGeoMsgP //把当前的点拷贝一份 返回geo_p template 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 bool IsTypeInStack(const T& elem, const std::vector & 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_msg inline 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_msg EncodeGraph(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的id Point3D 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 - launch->decoder.launch 最主要的还是LoadParam会用到它其中的参数
这里面定义了 rosparam 并指向graph_decoder/config/default.yaml，从而解析到world_frame和visual_scale_ratio （这个是调节显示的时候point大小的尺寸的系数） - 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("decoded_vgraph", 5);
graph_viz_pub_ = nh.advertise("/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的一个指针，这代表什么呢？代表里面有好多nodes
const visibility_graph_msg::Graph shared_graph = *msg;
this->ResetGraph(received_graph_); //调用ResetGraph清空received_graph_指针栈
NavNodePtr temp_node_ptr = NULL; //建立一个临时指针temp_node_ptr
robot_id_ = msg->robot_id; //传入robot_id
std::unordered_map nodeIdx_idx_map; //创建一个无序map容器nodeIdx_idx_map
//unordered_map 容器底层采用的是哈希表存储结构，该结构本身不具有对数据的排序功能，所以此容器内部不会自行对存储的键值对进行排序。
for (std::size_t i=0; iconnect_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(); //使用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 ( expression ) 将表达式转换为 type-id 的类型
node_ptr->free_direct = static_cast(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的点对
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& idxs_map,
const NodePtrStack& graph,
std::vector& node_idxs,
std::vector& connects)
{
std::vector clean_idx;
connects.clear(); //清空传入的connects
for (const auto& cid : node_idxs) {
const auto it = idxs_map.find(cid); //nodeIdx_idx_map中找到各个idxs里的cid赋值给it
if (it != idxs_map.end()) {
const std::size_t idx = idxs_map.find(cid)->second; //idxs_map 里是{node.id, i}，其中i就是上组循环时传入的数字，这里把i赋值给idx
const 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=3
Header 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_materialsVisualizeGraph函数
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 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 connections
if (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 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 connections
if (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拷贝过来
// connect
for (const auto& cnode : node_ptr->connect_nodes) { //一个for循环，从p1所在的node_ptr里调用connect_nodes里的node
p2 = ToGeoMsgP(cnode->position); //将connect_nodes里的node挨个赋值给p2
edge_marker.points.push_back(p1); //在edge_marker.points里放入p1
edge_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_marker
free_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 connections
if (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_node
for (const auto& nav_node_ptr : graphIn) {
if (nav_node_ptr == NULL) {
continue;
}
const geometry_msgs::Point cpoint = ToGeoMsgP(nav_node_ptr->position); //把当前的点读出来设成cpoint
nav_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_大概的流程应该是这样的，
