devbox
程序性能提升者
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Far planner代码系列（1）_far_planner 代码系列(1)

作者：程序性能提升者 | 2024-01-30 22:22:08

赞

踩

far_planner 代码系列(1)

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

graph_decoder系列（1）

  1. Node [/graph_decoder]
  2. Publications:
  3.  * /decoded_vgraph [visibility_graph_msg/Graph]
  4.  * /graph_decoder_viz [visualization_msgs/MarkerArray]
  5.  * /rosout [rosgraph_msgs/Log]
  6.  
  7. Subscriptions:
  8.  * /read_file_dir [std_msgs/String]
  9.  * /robot_vgraph [visibility_graph_msg/Graph]
  10.  * /save_file_dir [std_msgs/String]
  11.  
  12. Services:
  13.  * /graph_decoder/get_loggers
  14.  * /graph_decoder/set_logger_level
  15.  * /request_graph_service
  16.  
  17.  
  18. contacting node http://DESKTOP-DT3VKG6:41593/ ...
  19. Pid: 4979
  20. Connections:
  21.  * topic: /rosout
  22.     * to: /rosout
  23.     * direction: outbound (60537 - 127.0.0.1:59588) [11]
  24.     * transport: TCPROS
  25.  * topic: /decoded_vgraph
  26.     * to: /far_planner
  27.     * direction: outbound (60537 - 127.0.0.1:59624) [14]
  28.     * transport: TCPROS
  29.  * topic: /robot_vgraph
  30.     * to: /far_planner (http://DESKTOP-DT3VKG6:39071/)
  31.     * direction: inbound (44532 - DESKTOP-DT3VKG6:53855) [10]
  32.     * transport: TCPROS
  33.  * topic: /save_file_dir
  34.     * to: /far_rviz (http://DESKTOP-DT3VKG6:36425/)
  35.     * direction: inbound (49746 - DESKTOP-DT3VKG6:43299) [18]
  36.     * transport: TCPROS
  37.  * topic: /read_file_dir
  38.     * to: /far_rviz (http://DESKTOP-DT3VKG6:36425/)
  39.     * direction: inbound (49744 - DESKTOP-DT3VKG6:43299) [17]
  40.     * 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中进行定义。 
    1. class GraphDecoder {
    2. public:
    3.     GraphDecoder() = default;
    4.     ~GraphDecoder() = default;
    5.  
    6.     void Init();
    7.     void Loop();
    8.  
    9. private:
    10.     //  GraphDecoder 含有的参数   
    11.     ros::NodeHandle nh;                                 //创建句柄nh 也就是把NodeHandle实例化了
    12.     ros::Subscriber graph_sub_;                         //Subscriber创建了一个  graph_sub_  的对象
    13.     ros::Subscriber save_graph_sub_, read_graph_sub_;   //Subscriber创建了一个  save_graph_sub_  和一个read_graph_sub_  的对象
    14.     ros::Publisher  graph_pub_, graph_viz_pub_;         //Publisher创建了一个   graph_pub_  和一个  graph_viz_pub_  的对象
    15.     /*  graph_sub_  订阅  graph
    16.         save_graph_sub_ 订阅 save_graph
    17.         read_graph_sub_ 订阅 read_graph
    18.     */ 
    19.  
    20.     ros::ServiceServer request_graph_service_;      //Server创建 request_graph_service_ 服务器端
    21.     GraphDecoderParams gd_params_;                  //创建gd_params_的结构体对象 里面有frame_id和viz_scale_ratio
    22.     NodePtrStack received_graph_;                   //创建NodePtrStack指针栈 看名字应该是存收到的graph
    23.     MarkerArray graph_marker_array_;                // MarkerArray 是msg消息的一种类型，里面有很多东西
    24.     std::size_t robot_id_;
    25.  
    26.     void LoadParmas();
    27.  
    28.     void SetMarker(const VizColor& color, 
    29.                    const std::string& ns,
    30.                    const float scale,
    31.                    const float alpha, 
    32.                    Marker& scan_marker);
    33.     
    34.     void SetColor(const VizColor& color, const float alpha, Marker& scan_marker);
    35.  
    36.     void GraphCallBack(const visibility_graph_msg::GraphConstPtr& msg);
    37.  
    38.     void EncodeGraph(const NodePtrStack& graphIn, visibility_graph_msg::Graph& graphOut);
    39.  
    40.     void SaveGraphCallBack(const std_msgs::StringConstPtr& msg);
    41.  
    42.     void ReadGraphCallBack(const std_msgs::StringConstPtr& msg);
    43.  
    44.     bool SaveGraphService(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
    45.  
    46.     bool ReadGraphFromFile(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
    47.  
    48.     bool RequestGraphService(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 
    49.  
    50.     void VisualizeGraph(const NodePtrStack& graphIn);
    51.  
    52.     void CreateNavNode(std::string str, NavNodePtr& node_ptr);
    53.  
    54.     void CreateNavNode(const visibility_graph_msg::Node& msg, NavNodePtr& node_ptr);
    55.  
    56.     void AssignConnectNodes(const std::unordered_map<std::size_t, std::size_t>& idxs_map,
    57.                             const NodePtrStack& graph,
    58.                             std::vector<std::size_t>& node_idxs,
    59.                             std::vector<NavNodePtr>& connects);
    60.  
    61. //在ROS的 geometry_msgs::Point 下加入了一个内联函数  ToGeoMsgP 
    62. //把当前的点拷贝一份 返回geo_p
    63.     template <typename Point>
    64.     geometry_msgs::Point inline ToGeoMsgP(const Point& p) {
    65.         geometry_msgs::Point geo_p;
    66.         geo_p.x = p.x;
    67.         geo_p.y = p.y; 
    68.         geo_p.z = p.z;
    69.         return geo_p;
    70.     }
    71.  
    72. //判断 'elem' 在不在 'T_stack' 里
    73.     template <typename T>
    74.     bool IsTypeInStack(const T& elem, const std::vector<T>& T_stack) {
    75.         if (std::find(T_stack.begin(), T_stack.end(), elem) != T_stack.end()) {
    76.             return true;
    77.         }
    78.         return false;
    79.     }
    80.  
    81. //ResetGraph 就把NodePtrStack& graphOut给清空了  所以NodePtrStack 应该有好多个不同的对象
    82.     inline void ResetGraph(NodePtrStack& graphOut) {
    83.         graphOut.clear();
    84.     }
    85.  
    86. //ConvertGraphToMsg 把grap变成graph_msg
    87.     inline void ConvertGraphToMsg(const NodePtrStack& graph, visibility_graph_msg::Graph& graph_msg) {
    88.         graph_msg.header.frame_id = gd_params_.frame_id;
    89.         graph_msg.header.stamp = ros::Time::now();
    90.         graph_msg.robot_id = robot_id_;
    91.         //这个 EncodeGraph 才是最主要的从graph -> graph_msg
    92.         EncodeGraph(graph, graph_msg);
    93.     }
    94.  
    95. //判断NavNodePtr& node_ptr 是否加入到了NodePtrStack& graphOut里
    96. //如果node_ptr不为空 那就加入进去
    97.     inline bool AddNodePtrToGraph(const NavNodePtr& node_ptr, NodePtrStack& graphOut) {
    98.         if (node_ptr != NULL) {
    99.             graphOut.push_back(node_ptr);
    100.             return true;
    101.         }
    102.         return false;
    103.     }
    104.     
    105. };
    1. struct NavNode {
    2.     NavNode() = default;
    3.     std::size_t id;                 //定义当前Node的id
    4.     Point3D position;               //定义一个3D点云
    5.     NodeFreeDirect free_direct;     //定义free_direct 看不懂 什么 未知0；凸1；凹2；支柱3 不明白什么意思
    6.     PointPair surf_dirs;            //定义一个点对
    7.     bool is_covered;
    8.     bool is_frontier;
    9.     bool is_navpoint;
    10.     bool is_boundary;
    11.     std::vector<std::size_t> connect_idxs;                  //创建connect_idxs的vector   这个connect_idxs拿存放图的id。也就是graph
    12.     std::vector<std::shared_ptr<NavNode>> connect_nodes;    //创建connect_nodes的vector  里面存放的是导航时候用到的Node把？
    13.  
    14.     std::vector<std::size_t> poly_idxs;                     //创建poly_idxs 这个是拿来存放障碍物的边的id的
    15.     std::vector<std::shared_ptr<NavNode>> poly_connects;    //poly_connects用来存放障碍物的nav_ptr的 相当于点集合。
    16.  
    17.     std::vector<std::size_t> contour_idxs;                  //contour 相当于boundary，这个是用来存放boundary的点的id的。
    18.     std::vector<std::shared_ptr<NavNode>> contour_connects; //ontour_connects 存放可以连接的属于边界的nav_ptr
    19.  
    20.     std::vector<std::size_t> traj_idxs;                     //traj_idxs 存放轨迹点的id
    21.     std::vector<std::shared_ptr<NavNode>> traj_connects;    //traj_connects 存放连起来的轨迹？
    22. };
  3. launch->decoder.launch 最主要的还是LoadParam会用到它其中的参数 
    1. <node pkg="graph_decoder" type="graph_decoder" name="graph_decoder" output="screen">
    2.         <rosparam command="load" file="$(find graph_decoder)/config/default.yaml" />
    3.         <remap from="/planner_nav_graph" to="$(arg graph_topic)"/> 
    4.     </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：

  1. int main(int argc, char** argv){
  2.   ros::init(argc, argv, "graph_decoder_node");
  3.   GraphDecoder gd_node;
  4.   gd_node.Init();
  5.   gd_node.Loop();
  6. }

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

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

        这里初始化的东西很多，大部分是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自己的对象指针。

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

        我们先看看流程图是怎么样的：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_

  1.     inline bool AddNodePtrToGraph(const NavNodePtr& node_ptr, NodePtrStack& graphOut) {
  2.         if (node_ptr != NULL) {
  3.             graphOut.push_back(node_ptr);   
  4.             return true;
  5.         }
  6.         return false;
  7.     }
  8.     
  9. };
  1. void GraphDecoder::CreateNavNode(const visibility_graph_msg::Node& msg,
  2.                                  NavNodePtr& node_ptr)
  3. {
  4.     //node_ptr就是NavNode结构体的指针
  5.     node_ptr = std::make_shared<NavNode>();                                         //使用make_shared进行初始化NavNode结构体
  6.     node_ptr->position = Point3D(msg.position.x, msg.position.y, msg.position.z);   //把点云的x,y,z获取进来
  7.     node_ptr->id = msg.id;                                                          //赋id
  8.     // static_cast <type-id> ( expression ) 将表达式转换为 type-id 的类型
  9.     node_ptr->free_direct = static_cast<NodeFreeDirect>(msg.FreeType);              //把msg.FreeType类型转换为NodeFreeDirect
  10.     
  11.     // msg是对应src/msg中的Node.msg和Graph.msg
  12.     /*
  13.         msg里 geometry_msgs/Point[] surface_dirs  
  14.         surface_dirs是Point[]类型的。 Point[0] 表示第一个点， Point[0].x表示第一个点的x坐标值 以此类推
  15.     */
  16.     //surface_dir是一个存放point的数组
  17.     // 存了一个surf_dirs的点对 <point1,point2>
  18.     if (msg.surface_dirs.size() != 2) {
  19.         ROS_ERROR_THROTTLE(1.0, "node surface directions error.");
  20.         node_ptr->surf_dirs.first = node_ptr->surf_dirs.second = Point3D(0,0,-1);
  21.     } else {
  22.         node_ptr->surf_dirs.first  = Point3D(msg.surface_dirs[0].x,
  23.                                                 msg.surface_dirs[0].y,0.0f);
  24.         node_ptr->surf_dirs.second = Point3D(msg.surface_dirs[1].x,
  25.                                                 msg.surface_dirs[1].y,0.0f);
  26.     }
  27.     node_ptr->is_covered  = msg.is_covered;
  28.     node_ptr->is_frontier = msg.is_frontier;
  29.     node_ptr->is_navpoint = msg.is_navpoint;
  30.     node_ptr->is_boundary = msg.is_boundary;
  31.  
  32.  
  33.     // assigan connection idxs
  34.  
  35.     // 将navnode里的所有的idxs的vector容器清空
  36.     // 每次都清空所有的idxs 然后再把msg里读到的所有idxs在写进去，这样就维护了一定长度的idxs，把过早的idxs丢弃?我是这么觉得的
  37.     node_ptr->connect_idxs.clear(), node_ptr->poly_idxs.clear(), node_ptr->contour_idxs.clear(), node_ptr->traj_idxs.clear();
  38.  
  39.     //for(auto &a:b)中加了引用符号，可以对容器中的内容进行赋值，即可通过对a赋值来做到容器b的内容填充
  40.     //cid是msg.connect_nodes[]里的值，循环i次就是msg.connect_nodes[i]
  41.     for (const auto& cid : msg.connect_nodes) {
  42.         node_ptr->connect_idxs.push_back((std::size_t)cid); //难道是把对应connect_nodes的东西放到connect_idxs里？
  43.     }
  44.     for (const auto& cid : msg.poly_connects) {
  45.         node_ptr->poly_idxs.push_back((std::size_t)cid);
  46.     }
  47.     for (const auto& cid : msg.contour_connects) {
  48.         node_ptr->contour_idxs.push_back((std::size_t)cid);
  49.     }
  50.     for (const auto& cid : msg.trajectory_connects) {
  51.         node_ptr->traj_idxs.push_back((std::size_t)cid);
  52.     }
  53.     //把connect_nodes、poly_connects、contour_connects、traj_connects都清空 
  54.     node_ptr->connect_nodes.clear(), node_ptr->poly_connects.clear(), node_ptr->contour_connects.clear(), node_ptr->traj_connects.clear();
  55. }

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

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

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

        让我们来看看AssignConnectNodes：

  1. void GraphDecoder::AssignConnectNodes(const std::unordered_map<std::size_t, std::size_t>& idxs_map,
  2.                                       const NodePtrStack& graph,
  3.                                       std::vector<std::size_t>& node_idxs,
  4.                                       std::vector<NavNodePtr>& connects)
  5. {
  6.     std::vector<std::size_t> clean_idx;
  7.     connects.clear();                                           //清空传入的connects
  8.     for (const auto& cid : node_idxs) {
  9.         const auto it = idxs_map.find(cid);                     //nodeIdx_idx_map中找到各个idxs里的cid赋值给it
  10.         if (it != idxs_map.end()) {
  11.             const std::size_t idx = idxs_map.find(cid)->second; //idxs_map 里是{node.id, i}，其中i就是上组循环时传入的数字，这里把i赋值给idx
  12.             const NavNodePtr cnode_ptr = graph[idx];            //cnode_ptr  =  received_graph_[idx]
  13.             connects.push_back(cnode_ptr);                      //把cnode_ptr添加到connects里
  14.             clean_idx.push_back(cnode_ptr->id);                 //把conde_ptr里的id添加到clean_idx里
  15.         }
  16.     }
  17.     node_idxs = clean_idx;                                      //把各个idxs重新用clean_idx赋值
  18. }

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

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

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

DisplayTypes/Marker

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

 VisualizeGraph函数

  1. void GraphDecoder::VisualizeGraph(const NodePtrStack& graphIn) {
  2.     MarkerArray graph_marker_array;
  3.     Marker nav_node_marker, covered_node_marker, frontier_node_marker, internav_node_marker, edge_marker, poly_edge_marker, 
  4.            contour_edge_marker, free_edge_marker, traj_edge_marker, boundary_edge_marker, corner_surf_marker, corner_helper_marker;
  5.     nav_node_marker.type       = Marker::SPHERE_LIST;
  6.     covered_node_marker.type   = Marker::SPHERE_LIST;
  7.     internav_node_marker.type  = Marker::SPHERE_LIST;
  8.     frontier_node_marker.type  = Marker::SPHERE_LIST;
  9.     edge_marker.type           = Marker::LINE_LIST;
  10.     poly_edge_marker.type      = Marker::LINE_LIST;
  11.     free_edge_marker.type      = Marker::LINE_LIST;
  12.     boundary_edge_marker.type  = Marker::LINE_LIST;
  13.     contour_edge_marker.type   = Marker::LINE_LIST;
  14.     traj_edge_marker.type      = Marker::LINE_LIST;
  15.     corner_surf_marker.type    = Marker::LINE_LIST;
  16.     corner_helper_marker.type  = Marker::CUBE_LIST;
  17.     this->SetMarker(VizColor::WHITE,   "global_vertex",     0.5f,  0.5f,  nav_node_marker);
  18.     this->SetMarker(VizColor::BLUE,    "freespace_vertex",  0.5f,  0.8f,  covered_node_marker);
  19.     this->SetMarker(VizColor::YELLOW,  "trajectory_vertex", 0.5f,  0.8f,  internav_node_marker);
  20.     this->SetMarker(VizColor::ORANGE,  "frontier_vertex",   0.5f,  0.8f,  frontier_node_marker);
  21.     this->SetMarker(VizColor::WHITE,   "global_vgraph",     0.1f,  0.2f,  edge_marker);
  22.     this->SetMarker(VizColor::EMERALD, "freespace_vgraph",  0.1f,  0.2f,  free_edge_marker);
  23.     this->SetMarker(VizColor::EMERALD, "visibility_edge",   0.1f,  0.2f,  poly_edge_marker);
  24.     this->SetMarker(VizColor::RED,     "polygon_edge",      0.15f, 0.2f,  contour_edge_marker);
  25.     this->SetMarker(VizColor::GREEN,   "trajectory_edge",   0.1f,  0.5f,  traj_edge_marker);
  26.     this->SetMarker(VizColor::ORANGE,  "boundary_edge",     0.2f,  0.25f, boundary_edge_marker);
  27.     this->SetMarker(VizColor::YELLOW,  "vertex_angle",      0.15f, 0.75f, corner_surf_marker);
  28.     this->SetMarker(VizColor::YELLOW,  "angle_direct",      0.25f, 0.75f, corner_helper_marker);
  29.     /* Lambda Function */
  30.     auto Draw_Edge = [&](const NavNodePtr& node_ptr) {
  31.         geometry_msgs::Point p1, p2;
  32.         p1 = ToGeoMsgP(node_ptr->position);
  33.         for (const auto& cnode : node_ptr->connect_nodes) {
  34.             p2 = ToGeoMsgP(cnode->position);
  35.             edge_marker.points.push_back(p1);
  36.             edge_marker.points.push_back(p2);
  37.         }
  38.         for (const auto& cnode : node_ptr->poly_connects) {
  39.             p2 = ToGeoMsgP(cnode->position);
  40.             poly_edge_marker.points.push_back(p1);
  41.             poly_edge_marker.points.push_back(p2);
  42.             if (node_ptr->is_covered && cnode->is_covered) {
  43.                 free_edge_marker.points.push_back(p1);
  44.                 free_edge_marker.points.push_back(p2);
  45.             }
  46.         }
  47.         // contour edges
  48.         for (const auto& ct_cnode : node_ptr->contour_connects) {
  49.             p2 = ToGeoMsgP(ct_cnode->position);
  50.             contour_edge_marker.points.push_back(p1);
  51.             contour_edge_marker.points.push_back(p2);
  52.             if (node_ptr->is_boundary && ct_cnode->is_boundary) {
  53.                 boundary_edge_marker.points.push_back(p1);
  54.                 boundary_edge_marker.points.push_back(p2);
  55.             }
  56.         }
  57.         // inter navigation trajectory connections
  58.         if (node_ptr->is_navpoint) {
  59.             for (const auto& tj_cnode : node_ptr->traj_connects) {
  60.                 p2 = ToGeoMsgP(tj_cnode->position);
  61.                 traj_edge_marker.points.push_back(p1);
  62.                 traj_edge_marker.points.push_back(p2);
  63.             }
  64.         }
  65.     };
  66.     auto Draw_Surf_Dir = [&](const NavNodePtr& node_ptr) {
  67.         geometry_msgs::Point p1, p2, p3;
  68.         p1 = ToGeoMsgP(node_ptr->position);
  69.         Point3D end_p;
  70.         if (node_ptr->free_direct != NodeFreeDirect::PILLAR) {
  71.             end_p = node_ptr->position + node_ptr->surf_dirs.first * gd_params_.viz_scale_ratio;
  72.             p2 = ToGeoMsgP(end_p);
  73.             corner_surf_marker.points.push_back(p1);
  74.             corner_surf_marker.points.push_back(p2);
  75.             corner_helper_marker.points.push_back(p2);
  76.             end_p = node_ptr->position + node_ptr->surf_dirs.second * gd_params_.viz_scale_ratio;
  77.             p3 = ToGeoMsgP(end_p);
  78.             corner_surf_marker.points.push_back(p1);
  79.             corner_surf_marker.points.push_back(p3);
  80.             corner_helper_marker.points.push_back(p3);
  81.         }
  82.     };
  83.     std::size_t idx = 0;
  84.     const std::size_t graph_size = graphIn.size();
  85.     nav_node_marker.points.resize(graph_size);
  86.     for (const auto& nav_node_ptr : graphIn) {
  87.         if (nav_node_ptr == NULL) {
  88.             continue;
  89.         }
  90.         const geometry_msgs::Point cpoint = ToGeoMsgP(nav_node_ptr->position);
  91.         nav_node_marker.points[idx] = cpoint;
  92.         if (nav_node_ptr->is_navpoint) {
  93.             internav_node_marker.points.push_back(cpoint);
  94.         }
  95.         if (nav_node_ptr->is_covered) {
  96.             covered_node_marker.points.push_back(cpoint);
  97.         }
  98.         if (nav_node_ptr->is_frontier) {
  99.             frontier_node_marker.points.push_back(cpoint);
  100.         }
  101.         Draw_Edge(nav_node_ptr);
  102.         Draw_Surf_Dir(nav_node_ptr);
  103.         idx ++;    
  104.     } 
  105.     nav_node_marker.points.resize(idx);
  106.     graph_marker_array.markers.push_back(nav_node_marker);
  107.     graph_marker_array.markers.push_back(covered_node_marker);
  108.     graph_marker_array.markers.push_back(internav_node_marker);
  109.     graph_marker_array.markers.push_back(frontier_node_marker);
  110.     graph_marker_array.markers.push_back(edge_marker);
  111.     graph_marker_array.markers.push_back(poly_edge_marker);
  112.     graph_marker_array.markers.push_back(boundary_edge_marker);
  113.     graph_marker_array.markers.push_back(free_edge_marker);
  114.     graph_marker_array.markers.push_back(contour_edge_marker);
  115.     graph_marker_array.markers.push_back(traj_edge_marker);
  116.     graph_marker_array.markers.push_back(corner_surf_marker);
  117.     graph_marker_array.markers.push_back(corner_helper_marker);
  118.     graph_viz_pub_.publish(graph_marker_array);
  119. }

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

 SetMarker函数

  1. void GraphDecoder::SetMarker(const VizColor& color, 
  2.                              const std::string& ns,
  3.                              const float scale,
  4.                              const float alpha,  
  5.                              Marker& scan_marker) 
  6. {
  7.     scan_marker.header.frame_id = gd_params_.frame_id;
  8.     scan_marker.header.stamp = ros::Time::now();
  9.     scan_marker.id = 0;
  10.     scan_marker.ns = ns;
  11.     scan_marker.action = Marker::ADD;
  12.     scan_marker.scale.x = scan_marker.scale.y = scan_marker.scale.z = scale * gd_params_.viz_scale_ratio;
  13.     scan_marker.pose.orientation.x = 0.0;
  14.     scan_marker.pose.orientation.y = 0.0;
  15.     scan_marker.pose.orientation.z = 0.0;
  16.     scan_marker.pose.orientation.w = 1.0;
  17.     scan_marker.pose.position.x = 0.0;
  18.     scan_marker.pose.position.y = 0.0;
  19.     scan_marker.pose.position.z = 0.0;
  20.     this->SetColor(color, alpha, scan_marker);
  21. }

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

SetColor函数

  1. void GraphDecoder::SetColor(const VizColor& color, 
  2.                             const float alpha, 
  3.                             Marker& scan_marker)
  4. {
  5.     std_msgs::ColorRGBA c;
  6.     c.a = alpha;
  7.     if (color == VizColor::RED) {
  8.     c.r = 0.9f, c.g = c.b = 0.f;
  9.     }
  10.     else if (color == VizColor::ORANGE) {
  11.     c.r = 1.0f, c.g = 0.45f, c.b = 0.1f;
  12.     }
  13.     else if (color == VizColor::BLACK) {
  14.     c.r = c.g = c.b = 0.1f;
  15.     }
  16.     else if (color == VizColor::YELLOW) {
  17.     c.r = c.g = 0.9f, c.b = 0.1;
  18.     }
  19.     else if (color == VizColor::BLUE) {
  20.     c.b = 1.0f, c.r = 0.1f, c.g = 0.1f;
  21.     }
  22.     else if (color == VizColor::GREEN) {
  23.     c.g = 0.9f, c.r = c.b = 0.f;
  24.     }
  25.     else if (color == VizColor::EMERALD) {
  26.     c.g = c.b = 0.9f, c.r = 0.f;
  27.     }
  28.     else if (color == VizColor::WHITE) {
  29.     c.r = c.g = c.b = 0.9f;
  30.     }
  31.     else if (color == VizColor::MAGNA) {
  32.     c.r = c.b = 0.9f, c.g = 0.f;
  33.     }
  34.     else if (color == VizColor::PURPLE) {
  35.     c.r = c.b = 0.5f, c.g = 0.f;
  36.     }
  37.     scan_marker.color = c;
  38. }

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

  1. /* Lambda Function */
  2.     auto Draw_Edge = [&](const NavNodePtr& node_ptr) {
  3.         geometry_msgs::Point p1, p2;
  4.         p1 = ToGeoMsgP(node_ptr->position);
  5.         for (const auto& cnode : node_ptr->connect_nodes) {
  6.             p2 = ToGeoMsgP(cnode->position);
  7.             edge_marker.points.push_back(p1);
  8.             edge_marker.points.push_back(p2);
  9.         }
  10.         for (const auto& cnode : node_ptr->poly_connects) {
  11.             p2 = ToGeoMsgP(cnode->position);
  12.             poly_edge_marker.points.push_back(p1);
  13.             poly_edge_marker.points.push_back(p2);
  14.             if (node_ptr->is_covered && cnode->is_covered) {
  15.                 free_edge_marker.points.push_back(p1);
  16.                 free_edge_marker.points.push_back(p2);
  17.             }
  18.         }
  19.         // contour edges
  20.         for (const auto& ct_cnode : node_ptr->contour_connects) {
  21.             p2 = ToGeoMsgP(ct_cnode->position);
  22.             contour_edge_marker.points.push_back(p1);
  23.             contour_edge_marker.points.push_back(p2);
  24.             if (node_ptr->is_boundary && ct_cnode->is_boundary) {
  25.                 boundary_edge_marker.points.push_back(p1);
  26.                 boundary_edge_marker.points.push_back(p2);
  27.             }
  28.         }
  29.         // inter navigation trajectory connections
  30.         if (node_ptr->is_navpoint) {
  31.             for (const auto& tj_cnode : node_ptr->traj_connects) {
  32.                 p2 = ToGeoMsgP(tj_cnode->position);
  33.                 traj_edge_marker.points.push_back(p1);
  34.                 traj_edge_marker.points.push_back(p2);
  35.             }
  36.         }
  37.     };
  38.     auto Draw_Surf_Dir = [&](const NavNodePtr& node_ptr) {
  39.         geometry_msgs::Point p1, p2, p3;
  40.         p1 = ToGeoMsgP(node_ptr->position);
  41.         Point3D end_p;
  42.         if (node_ptr->free_direct != NodeFreeDirect::PILLAR) {
  43.             end_p = node_ptr->position + node_ptr->surf_dirs.first * gd_params_.viz_scale_ratio;
  44.             p2 = ToGeoMsgP(end_p);
  45.             corner_surf_marker.points.push_back(p1);
  46.             corner_surf_marker.points.push_back(p2);
  47.             corner_helper_marker.points.push_back(p2);
  48.             end_p = node_ptr->position + node_ptr->surf_dirs.second * gd_params_.viz_scale_ratio;
  49.             p3 = ToGeoMsgP(end_p);
  50.             corner_surf_marker.points.push_back(p1);
  51.             corner_surf_marker.points.push_back(p3);
  52.             corner_helper_marker.points.push_back(p3);
  53.         }
  54.     };
  55.     std::size_t idx = 0;
  56.     const std::size_t graph_size = graphIn.size();
  57.     nav_node_marker.points.resize(graph_size);
  58.     for (const auto& nav_node_ptr : graphIn) {
  59.         if (nav_node_ptr == NULL) {
  60.             continue;
  61.         }
  62.         const geometry_msgs::Point cpoint = ToGeoMsgP(nav_node_ptr->position);
  63.         nav_node_marker.points[idx] = cpoint;
  64.         if (nav_node_ptr->is_navpoint) {
  65.             internav_node_marker.points.push_back(cpoint);
  66.         }
  67.         if (nav_node_ptr->is_covered) {
  68.             covered_node_marker.points.push_back(cpoint);
  69.         }
  70.         if (nav_node_ptr->is_frontier) {
  71.             frontier_node_marker.points.push_back(cpoint);
  72.         }
  73.         Draw_Edge(nav_node_ptr);
  74.         Draw_Surf_Dir(nav_node_ptr);
  75.         idx ++;    
  76.     } 
  77.     nav_node_marker.points.resize(idx);
  78.     graph_marker_array.markers.push_back(nav_node_marker);
  79.     graph_marker_array.markers.push_back(covered_node_marker);
  80.     graph_marker_array.markers.push_back(internav_node_marker);
  81.     graph_marker_array.markers.push_back(frontier_node_marker);
  82.     graph_marker_array.markers.push_back(edge_marker);
  83.     graph_marker_array.markers.push_back(poly_edge_marker);
  84.     graph_marker_array.markers.push_back(boundary_edge_marker);
  85.     graph_marker_array.markers.push_back(free_edge_marker);
  86.     graph_marker_array.markers.push_back(contour_edge_marker);
  87.     graph_marker_array.markers.push_back(traj_edge_marker);
  88.     graph_marker_array.markers.push_back(corner_surf_marker);
  89.     graph_marker_array.markers.push_back(corner_helper_marker);
  90.     graph_viz_pub_.publish(graph_marker_array);
  91. }

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

  1.     auto Draw_Edge = [&](const NavNodePtr& node_ptr) {
  2.         geometry_msgs::Point p1, p2;            //定义两个点 
  3.         p1 = ToGeoMsgP(node_ptr->position);     //p1是node_ptr传入的起始点，通过ToGeoMsgP拷贝过来
  4.         //  connect
  5.         for (const auto& cnode : node_ptr->connect_nodes) {     //一个for循环，从p1所在的node_ptr里调用connect_nodes里的node
  6.             p2 = ToGeoMsgP(cnode->position);                    //将connect_nodes里的node挨个赋值给p2
  7.             edge_marker.points.push_back(p1);                   //在edge_marker.points里放入p1
  8.             edge_marker.points.push_back(p2);                   //在edge_marker.points里放入p2
  9.         }
  10.         //ploy_connect 应该是Polygon                            
  11.         for (const auto& cnode : node_ptr->poly_connects) {     //多边形连接
  12.             p2 = ToGeoMsgP(cnode->position);
  13.             poly_edge_marker.points.push_back(p1);
  14.             poly_edge_marker.points.push_back(p2);
  15.             if (node_ptr->is_covered && cnode->is_covered) {   //如果这些node被标定为is_covered  那么就把他们放到free_edge_marker
  16.                 free_edge_marker.points.push_back(p1);
  17.                 free_edge_marker.points.push_back(p2);
  18.             }
  19.         }
  20.         // contour edges    轮廓边
  21.         for (const auto& ct_cnode : node_ptr->contour_connects) {
  22.             p2 = ToGeoMsgP(ct_cnode->position);
  23.             contour_edge_marker.points.push_back(p1);
  24.             contour_edge_marker.points.push_back(p2);
  25.             if (node_ptr->is_boundary && ct_cnode->is_boundary) {
  26.                 boundary_edge_marker.points.push_back(p1);
  27.                 boundary_edge_marker.points.push_back(p2);
  28.             }
  29.         }
  30.         // inter navigation trajectory connections
  31.         if (node_ptr->is_navpoint) {
  32.             for (const auto& tj_cnode : node_ptr->traj_connects) {
  33.                 p2 = ToGeoMsgP(tj_cnode->position);
  34.                 traj_edge_marker.points.push_back(p1);
  35.                 traj_edge_marker.points.push_back(p2);
  36.             }
  37.         }
  38.     };

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

  1. auto Draw_Surf_Dir = [&](const NavNodePtr& node_ptr) {
  2.         geometry_msgs::Point p1, p2, p3;
  3.         p1 = ToGeoMsgP(node_ptr->position);
  4.         Point3D end_p;
  5.         if (node_ptr->free_direct != NodeFreeDirect::PILLAR) {
  6.             end_p = node_ptr->position + node_ptr->surf_dirs.first * gd_params_.viz_scale_ratio;
  7.             p2 = ToGeoMsgP(end_p);
  8.             corner_surf_marker.points.push_back(p1);
  9.             corner_surf_marker.points.push_back(p2);
  10.             corner_helper_marker.points.push_back(p2);
  11.             end_p = node_ptr->position + node_ptr->surf_dirs.second * gd_params_.viz_scale_ratio;
  12.             p3 = ToGeoMsgP(end_p);
  13.             corner_surf_marker.points.push_back(p1);
  14.             corner_surf_marker.points.push_back(p3);
  15.             corner_helper_marker.points.push_back(p3);
  16.         }
  17.     };
  1.  //这一部分就是来画图了
  2.     std::size_t idx = 0;
  3.     const std::size_t graph_size = graphIn.size();      //读取receive_graph的大小
  4.     nav_node_marker.points.resize(graph_size);          //把nav_node_marker的size设置成receive_graph一样大。因为在receive_graph里所有的点都是Nav_node
  5.     for (const auto& nav_node_ptr : graphIn) {
  6.         if (nav_node_ptr == NULL) {
  7.             continue;
  8.         }
  9.         const geometry_msgs::Point cpoint = ToGeoMsgP(nav_node_ptr->position);  //把当前的点读出来设成cpoint
  10.         nav_node_marker.points[idx] = cpoint;                                   //把cpoint填入nav_node_marker.points[]里
  11.         //根据类型来划分
  12.         if (nav_node_ptr->is_navpoint) {
  13.             internav_node_marker.points.push_back(cpoint);
  14.         }
  15.         if (nav_node_ptr->is_covered) {
  16.             covered_node_marker.points.push_back(cpoint);
  17.         }
  18.         if (nav_node_ptr->is_frontier) {
  19.             frontier_node_marker.points.push_back(cpoint);
  20.         }
  21.         Draw_Edge(nav_node_ptr);
  22.         Draw_Surf_Dir(nav_node_ptr);
  23.         idx ++;    
  24.     } 
  25.     nav_node_marker.points.resize(idx);
  26.     graph_marker_array.markers.push_back(nav_node_marker);
  27.     graph_marker_array.markers.push_back(covered_node_marker);
  28.     graph_marker_array.markers.push_back(internav_node_marker);
  29.     graph_marker_array.markers.push_back(frontier_node_marker);
  30.     graph_marker_array.markers.push_back(edge_marker);
  31.     graph_marker_array.markers.push_back(poly_edge_marker);
  32.     graph_marker_array.markers.push_back(boundary_edge_marker);
  33.     graph_marker_array.markers.push_back(free_edge_marker);
  34.     graph_marker_array.markers.push_back(contour_edge_marker);
  35.     graph_marker_array.markers.push_back(traj_edge_marker);
  36.     graph_marker_array.markers.push_back(corner_surf_marker);
  37.     graph_marker_array.markers.push_back(corner_helper_marker);
  38.     //graph_viz_pub_发布消息
  39.     graph_viz_pub_.publish(graph_marker_array);
  40. }

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

声明：本文内容由网友自发贡献，转载请注明出处：【wpsshop】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号