diff --git a/CMakeLists.txt b/CMakeLists.txt
index b6dd4ea..156d552 100644
--- 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
new file mode 100644
index 0000000..8a42855
--- /dev/null
+++ b/launch/global_path_creator.launch
@@ -0,0 +1,15 @@
+
+
+
+
+
+
diff --git a/launch/global_planner.launch b/launch/global_planner.launch
index ca3b09a..16336b4 100644
--- a/launch/global_planner.launch
+++ b/launch/global_planner.launch
@@ -3,11 +3,15 @@
-
-
+
+
+
+
+
diff --git a/launch/local_goal.launch b/launch/local_goal.launch
new file mode 100644
index 0000000..1dfff11
--- /dev/null
+++ b/launch/local_goal.launch
@@ -0,0 +1,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/scripts/global_path_viz.py b/scripts/global_path_viz.py
old mode 100755
new mode 100644
diff --git a/src/dijkstra_node.cpp b/src/dijkstra_node.cpp
index f10b113..c02ee59 100644
--- 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("/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 child_id;
std::vector 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 Dijkstra::CalcDijkstra(std::vector 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,22 +239,27 @@ 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 child_id;
std::vector 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));
}
@@ -254,7 +267,10 @@ void Dijkstra::SetCurrentEdge(amsl_navigation_msgs::Edge& edge)
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 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
new file mode 100644
index 0000000..2838ccd
--- /dev/null
+++ 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
+#include
+#include
+#include
+#include
+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("/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;
+}
\ No newline at end of file
diff --git a/src/local_goal_creator.cpp b/src/local_goal_creator.cpp
new file mode 100644
index 0000000..e882782
--- /dev/null
+++ b/src/local_goal_creator.cpp
@@ -0,0 +1,115 @@
+#include "ros/ros.h"
+#include "nav_msgs/Path.h"
+#include "geometry_msgs/PoseStamped.h"
+#include
+#include "amsl_navigation_msgs/Node.h"
+#include "amsl_navigation_msgs/Edge.h"
+#include "amsl_navigation_msgs/NodeEdgeMap.h"
+class LocalGoalCreator
+{
+public:
+ LocalGoalCreator();
+ void process();
+
+private:
+ //method
+ void global_path_callback(const nav_msgs::Path::ConstPtr&);
+ void current_pose_callback(const geometry_msgs::PoseStamped::ConstPtr&);
+ void path_callback(const std_msgs::Int32MultiArray::ConstPtr&);
+ void select_next_goal();
+ //parameter
+ int hz;
+ double border_distance;
+
+ //member
+ ros::NodeHandle nh;
+ ros::NodeHandle private_nh;
+ ros::Publisher pub_local_goal;
+ ros::Publisher pub_estimated_edge;
+ ros::Subscriber sub_global_path;
+ ros::Subscriber sub_current_pose;
+ ros::Subscriber sub_path;
+ nav_msgs::Path global_path;
+ geometry_msgs::PoseStamped current_pose;
+ geometry_msgs::PoseStamped local_goal;
+ std_msgs::Int32MultiArray global_path_num;
+ amsl_navigation_msgs::Edge estimated_edge;
+ unsigned int goal_number;
+ bool have_recieved_path = false;
+ bool have_recieved_multi_array = false;
+ bool have_recieved_pose = false;
+};
+
+LocalGoalCreator::LocalGoalCreator():private_nh("~")
+{
+ //parameter
+ private_nh.param("hz",hz,{10});
+ private_nh.param("border_distance",border_distance,{2.0});
+ //subscriber
+ sub_global_path = nh.subscribe("/global_path/path_path",10,&LocalGoalCreator::global_path_callback,this, ros::TransportHints().tcpNoDelay());
+ sub_current_pose = nh.subscribe("/ekf_pose",10,&LocalGoalCreator::current_pose_callback,this, ros::TransportHints().tcpNoDelay());
+ sub_path = nh.subscribe("/global_path/path",10,&LocalGoalCreator::path_callback,this, ros::TransportHints().tcpNoDelay());
+ //publisher
+ pub_local_goal = nh.advertise("/local_goal",1);
+ pub_estimated_edge = nh.advertise("/estimated_pose/edge", 1);
+}
+
+void LocalGoalCreator::path_callback(const std_msgs::Int32MultiArray::ConstPtr& msg_path)
+{
+ global_path_num = *msg_path;
+ have_recieved_multi_array = true;
+}
+void LocalGoalCreator::global_path_callback(const nav_msgs::Path::ConstPtr& msg)
+{
+ if(have_recieved_path) return;
+ global_path=*msg;
+ goal_number = 0;
+ local_goal = global_path.poses[goal_number]; // goal_number番目の位置
+ have_recieved_path = true;
+}
+void LocalGoalCreator::current_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+ current_pose = *msg;
+ have_recieved_pose = true;
+}
+void LocalGoalCreator::select_next_goal()
+{
+ double delta_x = local_goal.pose.position.x - current_pose.pose.position.x;
+ double delta_y = local_goal.pose.position.y - current_pose.pose.position.y;
+ double measure_distance = sqrt(delta_x*delta_x + delta_y*delta_y); // 自分の位置と次の通過ポイントの距離
+ std::cout<<"distance: "<< measure_distance< goal_number) local_goal = global_path.poses[goal_number];
+ else local_goal = global_path.poses[global_path.poses.size()-1];
+ if(goal_number == 0) goal_number ++;
+ std::cout<<"goal_number: "<< goal_number <