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目录里的结构:


  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_  订阅  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<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_p
        template <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_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<std::size_t> connect_idxs;                  //创建connect_idxs的vector   这个connect_idxs拿存放图的id。也就是graph
        std::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_ptr
    
        std::vector<std::size_t> traj_idxs;                     //traj_idxs 存放轨迹点的id
        std::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_framevisual_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里的PublisherSubscriber。我们一个个来看。


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<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.id
            nodeIdx_idx_map.insert({node.id, i});   
        }
    }
    // add connections to nodes
    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_);
}

        我们先看看流程图是怎么样的: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();                                           //清空传入的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_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 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_EdgeDraw_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_大概的流程应该是这样的,

  • 4
    点赞
  • 5
    收藏
  • 打赏
    打赏
  • 10
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:1024 设计师:我叫白小胖 返回首页
评论 10

打赏作者

LZZ and MYY

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值