[数据结构与算法]Far planner代码系列（2）

graph_decoder系列（2）

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

void GraphDecoder::Init() {
    /* initialize subscriber and publisher */
    //graph_sub_ 订阅节点/robot_vgraph    其类型是visibility_graph_msg/Graph的消息
    graph_sub_     = nh.subscribe("/robot_vgraph", 5, &GraphDecoder::GraphCallBack, this);
    graph_pub_     = nh.advertise<visibility_graph_msg::Graph>("decoded_vgraph", 5);
    graph_viz_pub_ = nh.advertise<MarkerArray>("/graph_decoder_viz",5);

    this->LoadParmas();
    save_graph_sub_ = nh.subscribe("/save_file_dir", 5, &GraphDecoder::SaveGraphCallBack, this);
    read_graph_sub_ = nh.subscribe("/read_file_dir", 5, &GraphDecoder::ReadGraphCallBack, this);
    request_graph_service_  = nh.advertiseService("/request_graph_service",  &GraphDecoder::RequestGraphService, this);
    robot_id_ = 0;
    this->ResetGraph(received_graph_);
}

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

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

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

}

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

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

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

? ? ? ? 补充一下：c++中的lambda表达式是c++11引用的最重要的特性之一，它定义了一个匿名函数，可以捕获一定范围的变量在函数内部使用，一般有如下语法形式：

auto func = [capture] (params) opt -> ret { func_body; };

一个完整的lambda表达式：

auto func1 = [](int a) -> int { return a + 1; };
auto func2 = [](int a) { return a + 2; };
cout << func1(1) << " " << func2(2) << endl;

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

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

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

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

void TeleopPanel::pressButton4()
{
  if ( ros::ok() && velocity_publisher_ )
  {
    QString qFilename = QFileDialog::getSaveFileName(this, tr("Save File"), "/", tr("VGH - Visibility Graph Files (*.vgh)"));

    std::string filename = qFilename.toStdString();
    if (filename != "") {
      int length = filename.length();
      if (length < 4) {
        filename += ".vgh";
      } else if (filename[length - 4] != '.' || filename[length - 3] != 'v' || filename[length - 2] != 'g' || filename[length - 1] != 'h') {
        filename += ".vgh";
      }
    }
    std_msgs::String msg;
    msg.data = filename;
    save_publisher_.publish(msg);
  }
}

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

save_publisher_.publish(msg);

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

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

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

?????????其存下来的格式为：

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

????????SaveGraphCallBack函数这一部分就结束了????

? ?

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

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

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

void TeleopPanel::pressButton3()
{
  if ( ros::ok() && velocity_publisher_ )
  {
    QString qFilename = QFileDialog::getOpenFileName(this, tr("Read File"), "/", tr("VGH - Visibility Graph Files (*.vgh)"));

    std::string filename = qFilename.toStdString();
    std_msgs::String msg;
    msg.data = filename;
    read_publisher_.publish(msg);
  }
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

//ConvertGraphToMsg 把grap变成graph_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);
    }

? ? ? ? 来看看?EncodeGraph

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

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

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

????????read_graph_sub_?这部分也就结束了。