After reading the great God's paper, I ran by myself. I feel very interesting. I want to read the great God's code carefully. Anyway, it's better to treat it as a series of self entertainment~ MichaelFYang/far_planner: Fast, Attemptable Route Planner for Navigation in Known and Unknown Environments (github.com)
graph_decoder series (1)
First, take a look at Src / graph_ Structure in the decoder Directory:
- config->default. Yaml # stores the default parameter world of Graph Decoder_ Frame and visual_scale_ratio in decoder_ node. LoadParam will be used in CPP initialization.
- include/graph_decoder point_struct.h structure for storing the definition of Point cloud, including addition, subtraction, multiplication and division of Point cloud coordinates, comparison, Point cloud hash, etc. decoder_node.h mainly establishes the class of GraphDecoder, defines a structure of NavNode, and declares a large number of functions in the class. These functions will be in the decoder_node.cpp.
class GraphDecoder { public: GraphDecoder() = default; ~GraphDecoder() = default; void Init(); void Loop(); private: // GraphDecoder contains parameters ros::NodeHandle nh; //Creating a handle nh means instantiating the NodeHandle ros::Subscriber graph_sub_; //Subscriber created a graph_sub_ Object of ros::Subscriber save_graph_sub_, read_graph_sub_; //Subscriber created a save_graph_sub_ And a read_graph_sub_ Object of ros::Publisher graph_pub_, graph_viz_pub_; //Publisher created a graph_pub_ And a graph_viz_pub_ Object of /* graph_sub_ Subscribe to graph save_graph_sub_ Subscribe to save_graph read_graph_sub_ Subscribe to read_graph */ ros::ServiceServer request_graph_service_; //Server create request_graph_service_ Server side GraphDecoderParams gd_params_; //Create gd_params_ There is a frame in the structure object of_ ID and viz_scale_ratio NodePtrStack received_graph_; //Create a NodePtrStack pointer stack. The name should be the received graph MarkerArray graph_marker_array_; // MarkerArray is a type of msg message. There are many things in it 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); //In the geometry of ROS_ An inline function ToGeoMsgP is added under msgs:: point //Return a copy of the current point to 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; } //Judge whether 'elem' is in'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 clears NodePtrStack & graphout, so NodePtrStack should have many different objects inline void ResetGraph(NodePtrStack& graphOut) { graphOut.clear(); } //ConvertGraphToMsg turns grap into a 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_; //This EncodeGraph is the most important from graph - > graph_ msg EncodeGraph(graph, graph_msg); } //Judge navnodeptr & node_ Whether PTR is added to nodeptrstack & graphout //If node_ If PTR is not empty, join it 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; //Defines the id of the current Node Point3D position; //Define a 3D point cloud NodeFreeDirect free_direct; //Define free_direct doesn't understand what's unknown 0; Convex 1; Concave 2; Pillar 3 doesn't understand what that means PointPair surf_dirs; //Define a point pair bool is_covered; bool is_frontier; bool is_navpoint; bool is_boundary; std::vector<std::size_t> connect_idxs; //Create connect_idxs vector connect_ What did idxs do with it? std::vector<std::shared_ptr<NavNode>> connect_nodes; //Create connect_ The nodes vector stores the nodes used in navigation? std::vector<std::size_t> poly_idxs; //Create poly_idxs doesn't know what he's doing here std::vector<std::shared_ptr<NavNode>> poly_connects; //poly_ Should connections be used to store poly that can be connected? std::vector<std::size_t> contour_idxs; //Contour literally means that the contour line doesn't know what to store std::vector<std::shared_ptr<NavNode>> contour_connects; //ontour_ Connections should store contour s that can be connected std::vector<std::size_t> traj_idxs; //traj_ Serial number of idxs track? std::vector<std::shared_ptr<NavNode>> traj_connects; //traj_ Connections store connected tracks? }; - launch->decoder. The most important thing about launch # is that LoadParam will use its parameters
This defines rosparam and points to graph_decoder/config/default.yaml, which can be parsed to the world_frame and visual_scale_ratio<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> - src->decoder_ node. Cpp# it mainly implements the decoder_ node. The definition of class functions in H and the instantiation of GraphDecoder class objects
2. First take a look at the 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();
}First, the ros is initialized, and then a {graphdecoder GD is instantiated_ Node object, call Init() to initialize it.
void GraphDecoder::Init() {
/* initialize subscriber and publisher */
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_);
}There are many things initialized here, most of which are Publisher and Subscriber in ros. Let's look at it one by one.
grap_sub_ It's a decoder_ node. ROS:: Subscriber graph in H_ sub_ . I won't list them one by one. Let's take a look at the decoder_ node. I understand.
Let's also talk about subscriber (topic,queue_size,fp,obj), where topic is the name of the node (string type), and queue_ Size is the data size, fp is the callback function to be called when the message is executed to this step, and obj is the pointer (this is the pointer to the GraphDecoder's own object).
void GraphDecoder::GraphCallBack(const visibility_graph_msg::GraphConstPtr& msg) {
const visibility_graph_msg::Graph shared_graph = *msg;
this->ResetGraph(received_graph_); //Call ResetGraph to clear received_graph_ Pointer stack
NavNodePtr temp_node_ptr = NULL; //Create a temporary pointer temp_node_ptr
robot_id_ = msg->robot_id; //Incoming robot_id
std::unordered_map<std::size_t, std::size_t> nodeIdx_idx_map; //Create an unordered map container nodeIdx_idx_map
//unordered_ The bottom layer of the map container adopts the hash table storage structure. The structure itself does not have the function of sorting data, so the stored key value pairs will not be sorted by itself.
for (std::size_t i=0; i<shared_graph.nodes.size(); i++) { //Loop through the current nodes
const auto node = shared_graph.nodes[i];
CreateNavNode(node, temp_node_ptr); //Create a navnode and assign a value to the data inside the structure. Its value comes from the msg message
//temp_node_ptr is the current temporary navnode pointer.
// If true is returned, set temp in AddNodePtrToGraph level_ node_ PTR added to received_graph_ in
if (AddNodePtrToGraph(temp_node_ptr, received_graph_)) {
//In nodeidx_ idx_ Insert i and node into the map 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_);
}What does this Callback function do? First, read in the msg message and clear the received_graph_ Pointer stack, create a temporary pointer temp_node_ptr, create an unordered map container nodeIdx_idx_map
Start loop traversal: from Src / MSG / graph Visibility in MSG_ graph_ MSG / node [] nodes reads the size of nodes and performs loop traversal. Create a # navnode and assign a value to the data inside the structure. Its value comes from MSG / node msg
Use AddNodePtrToGraph to judge temp_ node_ Whether PTR is received_graph_ Inside, if you are, go to nodeidx_ idx_ Insert {node.id, i} into the map
Attention! AddNodePtrToGraph itself_ PTR is not empty, that is, the above temp_ node_ If PTR is not empty, it will execute the operation graphout push_ Back (node_ptr)_ node_ Add PTR to 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 is the pointer of NavNode structure
node_ptr = std::make_shared<NavNode>(); //Using make_shared initializes the NavNode structure
node_ptr->position = Point3D(msg.position.x, msg.position.y, msg.position.z); //Get the x,y,z of the point cloud
node_ptr->id = msg.id; //Assign id
// static_ Cast < type ID > (expression) converts an expression to the type of type ID
node_ptr->free_direct = static_cast<NodeFreeDirect>(msg.FreeType); //Put MSG The freetype type is converted to NodeFreeDirect
// MSG is the node in src/msg MSG and graph msg
/*
msg Li geometry_msgs/Point[] surface_dirs
surface_dirs Is of type point []. Point[0] represents the first point, Point[0] x represents the x coordinate value of the first point, and so on
*/
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
// Empty the vector containers of all idxs in the navnode
node_ptr->connect_idxs.clear(), node_ptr->poly_idxs.clear(), node_ptr->contour_idxs.clear(), node_ptr->traj_idxs.clear();
//For (Auto & A: b) is added with a reference symbol, which can assign values to the contents of the container. You can fill the contents of container B by assigning values to a
//(std::size_t)cid doesn't know what it means and doesn't understand
for (const auto& cid : msg.connect_nodes) {
node_ptr->connect_idxs.push_back((std::size_t)cid); //Is it connect_ Put the things of nodes into 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_ All connections are cleared
node_ptr->connect_nodes.clear(), node_ptr->poly_connects.clear(), node_ptr->contour_connects.clear(), node_ptr->traj_connects.clear();
}Traverse the loop to establish the connection between nodes
For (Auto & A: b) is added with a reference symbol, which can assign values to the contents of the container. You can fill the contents of container B by assigning values to a
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_);Let's take a look at 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(); //Clear incoming connections
for (const auto& cid : node_idxs) {
const auto it = idxs_map.find(cid); //nodeIdx_ idx_ Find the cid in each idxs in the map and assign it
if (it != idxs_map.end()) {
const std::size_t idx = idxs_map.find(cid)->second; //idxs_ In the map is {node.id, i}, where i is the number passed in during the last group of loops. Here, i is assigned to idx
const NavNodePtr cnode_ptr = graph[idx]; //cnode_ptr = received_graph_[idx]
connects.push_back(cnode_ptr); //CNode_ Add PTR to connections
clean_idx.push_back(cnode_ptr->id); //Put Conde_ Add id in PTR to clean_idx Li
}
}
node_idxs = clean_idx; //Reuse clean for each idxs_ IDX assignment
}