amslabtech · ibuki1805 · Nov 13, 2021
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -136,6 +136,8 @@ include_directories(
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/dijkstra_global_planner_node.cpp)
 add_executable(dijkstra_node src/dijkstra_node.cpp)
+add_executable(global_path_creator src/global_path_creator.cpp)
+add_executable(local_goal_creator src/local_goal_creator.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -153,6 +155,8 @@ add_dependencies(dijkstra_node amsl_navigation_msgs_generate_messages_cpp)
 #   ${catkin_LIBRARIES}
 # )
 target_link_libraries(dijkstra_node ${catkin_LIBRARIES})
+target_link_libraries(global_path_creator ${catkin_LIBRARIES})
+target_link_libraries(local_goal_creator ${catkin_LIBRARIES})
 
 #############
 ## Install ##

diff --git a/launch/global_path_creator.launch b/launch/global_path_creator.launch
@@ -0,0 +1,15 @@
+<launch>
+	<!-- <arg name="init_node0_id" default="0"/>
+	<arg name="init_node1_id" default="1"/>
+	<arg name="init_progress" default="0.0"/>
+
+
+    <node pkg="dijkstra_global_planner" type="dijkstra_node" name="dijkstra" ns="global_planner" output="screen">
+		<param name="INIT_NODE0_ID" value="$(arg init_node0_id)" type="int"/>
+		<param name="INIT_NODE1_ID" value="$(arg init_node1_id)" type="int"/>
+	 	<param name="INIT_PROGRESS" value="$(arg init_progress)" type="double"/>
+	</node> -->
+    <!-- <node pkg="dijkstra_global_planner" type="global_path_viz.py" name="global_path_viz" ns="global_planner"/> -->
+    <node pkg="dijkstra_global_planner" type="global_path_creator" name="global_path_creator" output="screen">
+	</node>
+</launch>
diff --git a/launch/global_planner.launch b/launch/global_planner.launch
@@ -3,11 +3,15 @@
 	<arg name="init_node1_id" default="1"/>
 	<arg name="init_progress" default="0.0"/>
 
-    <node pkg="dijkstra_global_planner" type="global_path_viz.py" name="global_path_viz" ns="global_planner"/>
-    <node pkg="dijkstra_global_planner" type="dijkstra_node" name="dijkstra" ns="global_planner">
+    <!-- <node pkg="dijkstra_global_planner" type="global_path_viz.py" name="global_path_viz" ns="global_planner"/> -->
+
+    <node pkg="dijkstra_global_planner" type="dijkstra_node" name="dijkstra" ns="global_planner" output="screen">
 		<param name="INIT_NODE0_ID" value="$(arg init_node0_id)" type="int"/>
 		<param name="INIT_NODE1_ID" value="$(arg init_node1_id)" type="int"/>
 	 	<param name="INIT_PROGRESS" value="$(arg init_progress)" type="double"/>
 	</node>
+
+	<!-- <node pkg="dijkstra_global_planner" type="global_path_creator" name="global_path_creator" output="screen">
+	</node> -->
 
 </launch>
diff --git a/launch/local_goal.launch b/launch/local_goal.launch
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+
+<launch>
+
+    <arg name="hz" default="20"/>
+    <arg name="excess_detection_ratio" default="1.2"/>
+    <arg name="goal_radius" default="0.5"/>
+    <arg name="enable_requesting_replanning" default="false"/>
+    <arg name="intersection_acceptance_progress_ratio" default="0.7"/>
+    <arg name="robot_frame" default="base_link"/>
+    <arg name="goal_dis" default="5.0"/>
+    <arg name="global_path_index_offset" default="1"/>
+    <arg name="local_map" default="/local_map"/>
+    <arg name="local_goal_angle" default="2.0"/>
+    <arg name="d_local_goal_angle" default="0.4"/>
+    <arg name="w" default="0.4"/>
+    <arg name="border_distance" default="2.0"/>
+
+
+    <node pkg="dijkstra_global_planner" type="local_goal_creator" name="local_goal_creator" output="screen">
+        <param name="LOCAL_GOAL_ANGLE" value="$(arg local_goal_angle)"/>
+        <param name="D_LOCAL_GOAL_ANGLE" value="$(arg d_local_goal_angle)"/>
+        <param name="GOAL_DIS" value="$(arg goal_dis)"/>
+        <remap from="/local_map" to="$(arg local_map)"/>
+        <remap from="/local_map" to="/local_map/expand"/>
+        <param name="border_distance" value="$(arg border_distance)"/>
+    </node>
+
+    <!-- <node pkg="local_path_planner" type="dummy_locali" name="dummy_locali" output="screen">
+        <param name="w" value="$(arg w)"/>
+    </node> -->
+
+    <!-- <node pkg="dijkstra_global_planner" type="global_path_creator" name="global_path_creator" output="screen"> -->
+	<!-- </node> -->
+    <!-- <node pkg="rviz" type="rviz" name="rviz100" args="-d $(find local_path_planner)/config/check_local_goal.rviz"/> -->
+</launch>
diff --git a/scripts/global_path_viz.py b/scripts/global_path_viz.py
diff --git a/src/dijkstra_node.cpp b/src/dijkstra_node.cpp
@@ -78,13 +78,12 @@ Dijkstra::Dijkstra()
     node_edge_map_sub = nh.subscribe("/node_edge_map/map",1, &Dijkstra::NodeEdgeMapCallback, this);
     check_point_sub = nh.subscribe("/node_edge_map/checkpoint",1, &Dijkstra::CheckPointCallback, this);
     current_edge_sub = nh.subscribe("/estimated_pose/edge",1, &Dijkstra::CurrentEdgeCallback, this);
+    // service server
+    replan_server = nh.advertiseService("/global_path/replan", &Dijkstra::ReplanHandler, this);
 
     //publisher
     global_path_pub = nh.advertise<std_msgs::Int32MultiArray>("/global_path/path",1,true);
 
-    // service server
-    replan_server = nh.advertiseService("/global_path/replan", &Dijkstra::ReplanHandler, this);
-
     private_nh.param("INIT_NODE0_ID", INIT_NODE0_ID, {0});
     private_nh.param("INIT_NODE1_ID", INIT_NODE1_ID, {1});
     private_nh.param("INIT_PROGRESS", INIT_PROGRESS, {0.0});
@@ -95,21 +94,25 @@ Dijkstra::Dijkstra()
 
 void Dijkstra::CheckPointCallback(const std_msgs::Int32MultiArrayConstPtr& msg)
 {
+    // std::cout << "in the check point callback" << std::endl;
     std_msgs::Int32MultiArray check_points = *msg;
     checkpoints.clear();
     for(auto check_point : check_points.data){
         checkpoints.push_back(check_point);
     }
     num_nodes = nodes.size();
+    // std::cout << "num_nodes: " << num_nodes << std::endl;
+    // std::cout << "first_pub_path: " << first_pub_path << std::endl;
     if(!first_pub_path && num_nodes>0){
-        SetCurrentEdge(first_edge);
-        MakeAndPublishGlobalPath();    
+        SetCurrentEdge(first_edge); 
+        MakeAndPublishGlobalPath(); 
         first_pub_path = true;
     }
 }
 
 void Dijkstra::NodeEdgeMapCallback(const amsl_navigation_msgs::NodeEdgeMapConstPtr& msg)
 {
+    std::cout << "in the node edge map callback" << std::endl;
     amsl_navigation_msgs::NodeEdgeMap map = *msg;
     nodes.clear();
     edges.clear();
@@ -118,29 +121,31 @@ void Dijkstra::NodeEdgeMapCallback(const amsl_navigation_msgs::NodeEdgeMapConstP
             edges.push_back(edge);
         }
     }
+    // nodeの個数分
     for(auto n : map.nodes){
         int id = n.id;
         std::vector<int> child_id;
         std::vector<double> child_cost;
         for(auto edge : edges){
             if(edge.node0_id == id){
-                child_id.push_back(edge.node1_id);
-                child_cost.push_back(edge.distance);
+                child_id.push_back(edge.node1_id); // int
+                child_cost.push_back(edge.distance); // 距離感をみてコスト付
             }
         }
         int num_child = child_id.size();
         int i = 0;
         Node node(id, n.type, child_id,child_cost);
         nodes.push_back(node);
-        std::cout << "-------------" << std::endl;
-        std::cout << "node:"  << id << std::endl;
-        for(auto id: child_id){
-            std::cout << "child["<< i <<"]:"  << id 
-                << "(" << child_cost[i] << ")" << std::endl;
-            i++;
-        }
+        // std::cout << "-------------" << std::endl;
+        // std::cout << "node:"  << id << std::endl;
+        // for(auto id: child_id){
+        //     std::cout << "child["<< i <<"]:"  << id << "(" << child_cost[i] << ")" << std::endl;
+        //     i++;
+        // }
     }
     int num_checkpoints = checkpoints.size();
+    std::cout << "num_checkpoints" << num_checkpoints << std::endl;
+    std::cout << "first_pub_path: " << first_pub_path << std::endl;
     if(!first_pub_path && num_checkpoints>0){
         SetCurrentEdge(first_edge);
         MakeAndPublishGlobalPath();    
@@ -150,6 +155,7 @@ void Dijkstra::NodeEdgeMapCallback(const amsl_navigation_msgs::NodeEdgeMapConstP
 
 void Dijkstra::CurrentEdgeCallback(const amsl_navigation_msgs::EdgeConstPtr& msg)
 {
+    // std::cout << "current edge callback!!!!" << std::endl;
     amsl_navigation_msgs::Edge _edge = *msg;
     SetCurrentEdge(_edge);
     if(first_sub_edge_flag){
@@ -180,20 +186,22 @@ std::vector<int> Dijkstra::CalcDijkstra(std::vector<Node> nodes, int start_id, i
         float min_cost = 100000;
         for(auto n : nodes){
             if(n.open==true){
-                if(n.cost < min_cost){
+                if(n.cost < min_cost){ // もし、ターゲットのnodeへのコストが今持ってるコストより小さければ更新
+                    // 最終的に一番小さいコストのnodeが残る
                     min_cost = n.cost;
                     min_id = n.id;
                 }
             }
         }
-        // std::cout << "min_id: " << min_id << std::endl;
+        // std::cout << "min_cost:" << min_cost << std::endl;
+        // std::cout << "min_id:" << min_id << std::endl;
         int next_id = min_id;
-        if(min_id == -1){
+        if(min_id == -1){ // 特にidに更新がない
             resign = true;
             std::cout << "resign" << std::endl;
-        }else if(next_id == goal_id){
+        }else if(next_id == goal_id){ // 次の選択nodeがcheckpointなら
             found = true;
-            // std::cout << "goal" << std::endl;
+            // std::cout << "goal!!! yeah!!!!" << std::endl;
         }else{
             nodes[id2index(nodes, next_id)].open = false;
             int i = 0;
@@ -231,30 +239,38 @@ void Dijkstra::SetCurrentEdge(amsl_navigation_msgs::Edge& edge)
     int i = 0;
     for(auto n : nodes){
         if(n.type=="add_node"){
+            std::cout << "add nodes!!!!" << std::endl;
             nodes.erase(nodes.begin()+i);
             checkpoints.erase(checkpoints.begin());
+            std::cout << "nodes:" << nodes.size() << std::endl;
         }
         i++;
     }
     if(current_edge.node0_id!=checkpoints[0]){
         if(current_edge.progress != 0.0){
+            std::cout << "add edges!!!!" << std::endl;
             std::string type = "add_node";
             std::vector<int> child_id;
             std::vector<double> child_cost;
+            // 通れない？
             if(current_edge.impassable){
-                child_id.push_back(current_edge.node0_id);
+                child_id.push_back(current_edge.node0_id); //startセット？
                 child_cost.push_back(current_edge.distance*current_edge.progress);
-            }else{
-                child_id.push_back(current_edge.node0_id);
-                child_id.push_back(current_edge.node1_id);
+            }
+            else{
+                child_id.push_back(current_edge.node0_id); // どこから？
+                child_id.push_back(current_edge.node1_id); // どこまで？
                 child_cost.push_back(current_edge.distance*current_edge.progress);
                 child_cost.push_back(current_edge.distance*(1.0-current_edge.progress));
             }
             int add_node_id = nodes.size();
             Node node(add_node_id, type, child_id, child_cost);
             nodes.push_back(node);
             checkpoints.insert(checkpoints.begin(), add_node_id);
-        }else{
+            std::cout << "add_node_id size: " << add_node_id << std::endl;
+            std::cout << "child cost: " << child_cost.size() << std::endl;
+            std::cout << "child id: " << child_id.size() << std::endl;
+        }else{ // current == 0.0の時
             checkpoints.insert(checkpoints.begin(), current_edge.node0_id);
         }
     }
@@ -278,10 +294,10 @@ void Dijkstra::MakeAndPublishGlobalPath()
         }
         for(int i=0; i<num_checkpoints-1; i++){
             std::vector<int> path;
-            // std::cout << checkpoints[i] << " to "<< checkpoints[i+1] << std::endl;
-            path = CalcDijkstra(nodes,checkpoints[i],checkpoints[i+1]);
+            // std::cout << checkpoints[i] << " to "<< checkpoints[i+1] << std::endl; // nodeの移動ルート表示
+            path = CalcDijkstra(nodes, checkpoints[i], checkpoints[i+1]); // vector
             for(auto p : path){
-                global_path.data.push_back(p);
+                global_path.data.push_back(p); // 最小コストでのpathを詰めていく
             }
         }
         // global_path.data.erase(global_path.data.begin());

diff --git a/src/global_path_creator.cpp b/src/global_path_creator.cpp
@@ -0,0 +1,108 @@
+#include "ros/ros.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "geometry_msgs/Pose2D.h"
+#include "nav_msgs/Path.h"
+#include "geometry_msgs/PointStamped.h"
+#include <std_msgs/Int32MultiArray.h>
+#include <amsl_navigation_msgs/Node.h>
+#include <amsl_navigation_msgs/Edge.h>
+#include <amsl_navigation_msgs/NodeEdgeMap.h>
+#include <amsl_navigation_msgs/Replan.h>
+class GlobalPathCreator
+{
+public:
+    GlobalPathCreator();
+    void process();
+
+private:
+    void map_callback(const nav_msgs::OccupancyGrid::ConstPtr&);
+    void path_callback(const std_msgs::Int32MultiArray::ConstPtr&);
+    void node_edge_map_callback(const amsl_navigation_msgs::NodeEdgeMapConstPtr&);
+    void make_global_path();
+
+    bool replan_flag;
+    bool node_edge_flag;
+    bool global_path_flag;
+    ros::NodeHandle n;
+    ros::NodeHandle private_n;
+
+    ros::Publisher pub_path;
+    ros::Publisher pub_open_grid;
+    ros::Subscriber sub_map;
+    ros::Subscriber sub_path;
+    ros::Subscriber node_edge_map_sub;
+    ros::Subscriber check_point_sub;
+
+    nav_msgs::Path global_path;
+    nav_msgs::Path waypoint_path;
+    amsl_navigation_msgs::NodeEdgeMap map;
+    std_msgs::Int32MultiArray check_points;
+    std_msgs::Int32MultiArray global_path_num;
+};
+
+GlobalPathCreator::GlobalPathCreator() :private_n("~")
+{
+    // subscriber
+    node_edge_map_sub = n.subscribe("/node_edge_map/map",1, &GlobalPathCreator::node_edge_map_callback, this);
+    sub_path = n.subscribe("/global_path/path",10,&GlobalPathCreator::path_callback,this);
+
+    //publisher
+    pub_path = n.advertise<nav_msgs::Path>("/global_path/path_path",1);
+    replan_flag = false;
+    node_edge_flag = false;
+    global_path_flag = false;
+}
+
+
+void GlobalPathCreator::node_edge_map_callback(const amsl_navigation_msgs::NodeEdgeMapConstPtr& msg_map)
+{
+    map = *msg_map;
+    std::cout << "subscribe map information" << std::endl;
+}
+
+void GlobalPathCreator::path_callback(const std_msgs::Int32MultiArray::ConstPtr& msg_path)
+{
+    std::cout << "subscribe global_path" << std::endl;
+    global_path_num = *msg_path;
+    global_path_flag = true;
+    make_global_path();  
+}
+
+void GlobalPathCreator::make_global_path()
+{
+    int count_i = 0;
+    int count_j = 0;
+    for(auto path : global_path_num.data){
+        for(auto n : map.nodes){
+            if(path == n.id){
+                // std::cout << "path; " << path << std::endl;
+                geometry_msgs::PoseStamped temp_path_point;
+                temp_path_point.pose.position.x = n.point.x;
+                temp_path_point.pose.position.y = n.point.y;
+                temp_path_point.header.frame_id = "map"; 
+                global_path.poses.push_back(temp_path_point);
+            }
+            count_j ++;
+        }
+        count_i ++;
+    }
+    std::cout << "i: " << count_i << " j: " << count_j << std::endl;
+    std::cout << "global_path poses size:  " << global_path.poses.size() << std::endl;
+    global_path.header.frame_id = "map";
+    // while(!replan_flag) // 応急処置
+    // for(int i = 0; i < 1000000; i++) // for visualize
+        pub_path.publish(global_path);
+}
+void GlobalPathCreator::process()
+{
+    while(ros::ok()){
+        ros::spinOnce();
+    }
+}
+int main (int argc, char **argv)
+{
+  ros::init(argc,argv,"global_path_creator");
+  GlobalPathCreator global_path_creator;
+  global_path_creator.process();
+  return 0;
+}