RAPP Platform  v0.6.0
RAPP Platform is a collection of ROS nodes and back-end processes that aim to deliver ready-to-use generic services to robots
 All Classes Namespaces Files Functions Variables Macros
path_planner.cpp
Go to the documentation of this file.
1 #include <path_planning/path_planning.h>
2 
3 
4 PathPlanner::PathPlanner(void)
5 {
6 }
7 
8 // set next planning sequence ID
9 std::string PathPlanner::setSequenceNR(ros::NodeHandle &nh_, int pathPlanningThreads_){
10  int seq_nr_int=1;
11  std::string seq_nr_str;
12  if (nh_.hasParam("/rapp/rapp_path_planning/last_seq")){
13  nh_.getParam("/rapp/rapp_path_planning/last_seq", seq_nr_int);
14 
15  seq_nr_int++;
16  if (seq_nr_int > pathPlanningThreads_)
17  seq_nr_int = 1;
18  }
19 
20  seq_nr_str = boost::lexical_cast<std::string>(seq_nr_int);
21  nh_.setParam("/rapp/rapp_path_planning/last_seq", seq_nr_int);
22 
23  return seq_nr_str;
24 }
25 // configure sequence -> load .yaml files and load proper map
26 bool PathPlanner::configureSequence(std::string seq_nr, std::string map_path, std::string robot_type, std::string algorithm, ros::NodeHandle &nh_){
27 
28  nh_.setParam("/map_server"+seq_nr+"/setMap", map_path);
29  ros::ServiceClient get_map_client = nh_.serviceClient<rapp_platform_ros_communications::MapServerGetMapRosSrv>("map_server"+seq_nr+"/get_map");
30  ros::ServiceClient set_costmap_client = nh_.serviceClient<rapp_platform_ros_communications::Costmap2dRosSrv>("/global_planner"+seq_nr+"/costmap_map_update");
31 
32  rapp_platform_ros_communications::MapServerGetMapRosSrv get_map_srv;
33  rapp_platform_ros_communications::Costmap2dRosSrv set_costmap_srv;
34  get_map_srv.request.map_path = map_path;
35  uint32_t serv_port;
36  std::string serv_node_name, serv_name;
37  ros::ServiceManager srv_manager;
38  bool serv_active = srv_manager.lookupService("map_server"+seq_nr+"/get_map",serv_node_name,serv_port);
39 
40  while (!serv_active){
41  ROS_DEBUG_STREAM("Waiting for:"<<" map_server"<<seq_nr<<"/get_map" << " service ");
42  serv_active = srv_manager.lookupService("map_server"+seq_nr+"/get_map",serv_node_name,serv_port);
43  ros::Duration(1).sleep();
44  }
45 
46 
47  if (get_map_client.call(get_map_srv))
48  {
49  set_costmap_srv.request.map = get_map_srv.response.map;
50 
51  if (set_costmap_client.call(set_costmap_srv))
52  {
53  ROS_INFO_STREAM("Costmap update for SEQ: " << seq_nr);
54 
55  }else{
56  ROS_ERROR_STREAM("Costmap update error for SEQ: " << seq_nr << "\n path planner cannot set new costmap");
57 
58  }
59  }else{
60 
61  ROS_ERROR_STREAM("Costmap update error for SEQ: " << seq_nr << "\n path planner cannot get map from map server");
62 
63  }
64  const char* execute_command = "/opt/ros/indigo/bin/rosparam";
65  pid_t load_configs_costmap_pID = fork();
66  if (load_configs_costmap_pID == 0)
67  {
68  ROS_DEBUG_STREAM("starting load_configs_costmap_pID for sequence: "<< seq_nr);
69 
70  //robot_type = "NAO";
71  std::string costmap_file = robot_type+".yaml";
72  std::string load_configs_pkg_path = ros::package::getPath("rapp_path_planning");
73  // set yaml file path
74  std:: string execute_param_1_string = load_configs_pkg_path+"/cfg/costmap/"+costmap_file;
75  const char* execute_param_1 = execute_param_1_string.c_str();
76  ROS_DEBUG_STREAM("costmap file path:\n"<< execute_param_1_string);
77 
78  // set params namespace
79  std:: string execute_param_2_string = "/global_planner"+seq_nr+"/costmap/";
80  const char* execute_param_2 = execute_param_2_string.c_str();
81  nh_.setParam("/global_planner"+seq_nr+"/costmap/map_service", "/map_server"+seq_nr+"/getMap");
82 
83  execl(execute_command,execute_command,"load", execute_param_1,execute_param_2 , (char *)0);
84  }
85  else if (load_configs_costmap_pID < 0)
86  {
87  std::cout << "Failed to fork load_configs" << std::endl;
88  return false;
89  exit(1);
90 
91  }
92  else{
93  pid_t load_configs_planner_pID = fork();
94  if (load_configs_planner_pID == 0)
95  {
96  ROS_DEBUG_STREAM("starting load_configs_planner_pID for sequence: "<< seq_nr);
97 
98  //algorithm = "dijkstra";
99  std::string algorithm_file = algorithm+".yaml";
100  std::string load_configs_pkg_path = ros::package::getPath("rapp_path_planning");
101  // set yaml file path
102  std:: string execute_param_1_string = load_configs_pkg_path+"/cfg/planner/"+algorithm_file;
103  const char* execute_param_1 = execute_param_1_string.c_str();
104  // set params namespace
105  std:: string execute_param_2_string = "/global_planner"+seq_nr+"/planner/";
106  const char* execute_param_2 = execute_param_2_string.c_str();
107  execl(execute_command,execute_command,"load", execute_param_1,execute_param_2 , (char *)0);
108  }
109  else if (load_configs_planner_pID < 0)
110  {
111  ROS_ERROR("Failed to fork load_configs");
112  return false;
113  exit(1);
114 
115  }
116  else{
117 
118  std:: string costmap_param_name = "/global_planner"+seq_nr+"/costmap/map_topic";
119 
120  std:: string planner_param_name = "/global_planner"+seq_nr+"/planner/use_dijkstra";
121  ros::Time time_start = ros::Time::now();
122  ros::Time time_now;
123 
124  while (!nh_.hasParam(costmap_param_name) && !nh_.hasParam(planner_param_name)){
125  ros::Duration(0.1).sleep();
126  time_now = ros::Time::now();
127  if (time_now > time_start + ros::Duration(5))
128  return false;
129  }
130  return true;
131  }
132 
133  }
134  return false;
135 }
136 // send request to approprate global_planner and return MakeNavPlanResponse
137 navfn::MakeNavPlanResponse PathPlanner::startSequence(std::string seq_nr, geometry_msgs::PoseStamped request_start, geometry_msgs::PoseStamped request_goal, ros::NodeHandle &nh_){
138  navfn::MakeNavPlanResponse planned_path;
139  uint32_t serv_port;
140  std::string serv_node_name, serv_name;
141  serv_name = "/global_planner"+seq_nr+"/make_plan";
142  ros::ServiceManager srv_manager;
143  bool serv_active = srv_manager.lookupService(serv_name,serv_node_name,serv_port);
144 
145  ros::Duration(1.5).sleep();
146 
147  while (!serv_active){
148  ROS_ERROR("Can't find service make_plan");
149  serv_active = srv_manager.lookupService(serv_name,serv_node_name,serv_port);
150  ros::Duration(1).sleep();
151  }
152  ROS_DEBUG_STREAM("HOST name:\n" << serv_node_name << "service port:\n" << serv_port);
153 
154  ros::ServiceClient client = nh_.serviceClient<navfn::MakeNavPlan>("/global_planner"+seq_nr+"/make_plan");
155  navfn::MakeNavPlan srv;
156  srv.request.start = request_start;
157  srv.request.goal = request_goal;
158 
159  if (client.call(srv))
160  {
161  navfn::MakeNavPlanResponse planned_path;
162  planned_path = srv.response;
163  ROS_DEBUG("Path planning service ended");
164  return planned_path;
165 
166  }
167 
168  else
169  {
170  ROS_ERROR("Failed to call service make_plan");
171  return planned_path;
172  }
173 
174 }
175 
176 
178 //
179 // OLD APPROACH
180 //
182 // bool deleteParameters(std::string &new_nodes_string_id, ros::NodeHandle &nh_){
183 // ROS_ERROR("deleting params\n");
184 // ROS_DEBUG_STREAM(new_nodes_string_id);
185 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/allow_unknown");
186 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/default_tolerance");
187 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/visualize_potential");
188 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/use_dijkstra");
189 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/use_quadratic");
190 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/use_grid_path");
191 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/old_navfn_behavior");
192 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/cost_factor");
193 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/lethal_cost");
194 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/neutral_cost");
195 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/orientation_mode");
196 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/planner/publish_potential");
197 
198 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/global_frame");
199 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/robot_base_frame");
200 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/update_frequency");
201 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/publish_frequency");
202 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/plugins");
203 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/publish_voxel_map");
204 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/static_map");
205 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/map_topic");
206 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/unknown_cost_value");
207 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/lethal_cost_threshold");
208 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/footprint");
209 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/footprint_padding");
210 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/cost_scaling_factor");
211 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/height");
212 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/origin_x");
213 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/origin_y");
214 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/resolution");
215 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/robot_radius");
216 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/transform_tolerance");
217 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/width");
218 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/inflater/cost_scaling_factor");
219 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/inflater/enabled");
220 // nh_.deleteParam("/global_planner"+new_nodes_string_id+"/costmap/inflater/inflation_radius");
221 // return true;
222 
223 // }
224 // navfn::MakeNavPlanResponse PathPlanner::plannPath(std::string map_path, std::string robot, std::string algorithm, geometry_msgs::PoseStamped request_start, geometry_msgs::PoseStamped request_goal, ros::NodeHandle &nh_)
225 // {
226 
227 
228 // ros::package::V_string nodes;
229 // ros::master::getNodes(nodes);
230 // std::string pathPlanningTopic_;
231 // int pathPlanningThreads_;
232 // nh_.getParam("/rapp_path_planning_threads", pathPlanningThreads_);
233 // // initialize examplary command for execl instructions
234 // const char* execute_command = "";
235 // const char* execute_pkg_name = "";
236 // const char* execute_pkg_exe_name = "";
237 
238 // // determine individual new_nodes_ID
239 // int new_nodes_ID = 1;
240 // std::string new_node_name = "/map_server";
241 // std::string new_nodes_string_id = "empty";
242 // ROS_DEBUG_STREAM( "looking for launched nodes\n Number of avaliable threads:\n"<<pathPlanningThreads_);
243 // for (int i=0; i < nodes.size();i++){
244 // ROS_DEBUG_STREAM("\nNodes: " << nodes.at(i));
245 // }
246 // for (int i=0; i < pathPlanningThreads_;i++){
247 // new_nodes_string_id = boost::lexical_cast<std::string>(new_nodes_ID);//std::to_string(new_nodes_ID);
248 // new_node_name = new_node_name + new_nodes_string_id;
249 // // old method -> check if /map_server node exist
250 // // if (std::find(nodes.begin(), nodes.end(), new_node_name) != nodes.end() ){
251 // // new_nodes_ID++;
252 // // ROS_DEBUG_STREAM("Node: " << new_node_name << "exist");
253 // // }else{
254 // // ROS_DEBUG("New nodes names ID found!");
255 // // break;
256 // // }
257 // // check if param exist. if param does not exist but node is sill alive, the old node can be replaced by new one.
258 // if (nh_.hasParam("/global_planner"+new_nodes_string_id+"/costmap/inflater/inflation_radius"))
259 // new_nodes_ID++;
260 // }
261 
262 // if (new_nodes_string_id =="empty"){
263 
264 // ROS_DEBUG("NO THREADS AVALIABLE FOR Path planner module!");
265 
266 // }else{
267 // // set node names
268 // ROS_DEBUG("setting nodes names");
269 // std::string map_server_pkg_name = "map_server";
270 // map_server_pkg_name = map_server_pkg_name + new_nodes_string_id;
271 // std::string tf_broadcaster_pkg_name = "static_transform_publisher";
272 // tf_broadcaster_pkg_name = tf_broadcaster_pkg_name + new_nodes_string_id;
273 // std::string load_configs_pkg_name = "load_configs";
274 // load_configs_pkg_name = load_configs_pkg_name + new_nodes_string_id;
275 // std::string global_planner_pkg_name = "global_planner";
276 // global_planner_pkg_name = global_planner_pkg_name + new_nodes_string_id;
277 
278 // pid_t map_server_pID = fork();
279 // if (map_server_pID == 0)
280 // {
281 // ROS_DEBUG("starting map_server");
282 
283 
284 // execute_command = "/opt/ros/indigo/bin/rosrun";
285 // execute_pkg_name = "map_server";
286 // execute_pkg_exe_name = "map_server";
287 // std:: string execute_param_1_string = "__name:="+map_server_pkg_name;
288 // const char* execute_param_1 = execute_param_1_string.c_str();
289 // ROS_DEBUG_STREAM("map_server name:\n" << map_server_pkg_name );
290 
291 // // set map path
292 // std:: string execute_param_2_string = map_path;
293 // const char* execute_param_2 = execute_param_2_string.c_str();
294 // // remap map publication topic
295 // std:: string execute_param_3_string = "/map:=/"+map_server_pkg_name+"/map";
296 // const char* execute_param_3 = execute_param_3_string.c_str();
297 // execl(execute_command, execute_command,execute_pkg_name, execute_pkg_exe_name, execute_param_1, execute_param_2, execute_param_3, (char *)0);
298 // }
299 // else if (map_server_pID < 0)
300 // {
301 // std::cout << "Failed to fork map_server" << std::endl;
302 // exit(1);
303 // }
304 // else{
305 // pid_t tf_broadcaster_pID = fork();
306 // if (tf_broadcaster_pID == 0)
307 // {
308 // ROS_DEBUG("starting tf_broadcaster_pID");
309 
310 // std::string tf_broadcaster_pkg_path = ros::package::getPath("tf");
311 // execute_command = "/opt/ros/indigo/bin/rosrun";
312 // execute_pkg_name = "tf";
313 // execute_pkg_exe_name = "static_transform_publisher";
314 // std:: string execute_param_1_string = "__name:="+tf_broadcaster_pkg_name;
315 // const char* execute_param_1 = execute_param_1_string.c_str();
316 // execl(execute_command,execute_command,execute_pkg_name, execute_pkg_exe_name, execute_param_1, "0.066", "1.7", "0.54", "0.49999984", "0.49960184", "0.49999984","0.50039816","map","base_link","100", (char *)0);
317 // }
318 // else if (tf_broadcaster_pID < 0)
319 // {
320 // std::cout << "Failed to fork global_planner" << std::endl;
321 // exit(1);
322 // }
323 // else{
324 // pid_t load_configs_costmap_pID = fork();
325 // if (load_configs_costmap_pID == 0)
326 // {
327 // ROS_DEBUG("starting load_configs_costmap_pID");
328 
329 // execute_command = "/opt/ros/indigo/bin/rosparam";
330 
331 // std::string costmap_file = "costmap_NAO.yaml";
332 // std::string load_configs_pkg_path = ros::package::getPath("rapp_path_planning");
333 // // set yaml file path
334 // std:: string execute_param_1_string = load_configs_pkg_path+"/cfg/costmap/"+costmap_file;
335 // const char* execute_param_1 = execute_param_1_string.c_str();
336 // ROS_DEBUG_STREAM("costmap file path:\n"<< execute_param_1_string);
337 
338 // // set params namespace
339 // std:: string execute_param_2_string = "/"+global_planner_pkg_name+"/costmap/";
340 // const char* execute_param_2 = execute_param_2_string.c_str();
341 
342 // execl(execute_command,execute_command,"load", execute_param_1,execute_param_2 , (char *)0);
343 // }
344 // else if (load_configs_costmap_pID < 0)
345 // {
346 // std::cout << "Failed to fork load_configs" << std::endl;
347 // exit(1);
348 // }
349 // else{
350 // pid_t load_configs_planner_pID = fork();
351 // if (load_configs_planner_pID == 0)
352 // {
353 // ROS_DEBUG("starting load_configs_planner_pID");
354 
355 // execute_command = "/opt/ros/indigo/bin/rosparam";
356 
357 // std::string algorithm_file = "dijkstra.yaml";
358 // std::string load_configs_pkg_path = ros::package::getPath("rapp_path_planning");
359 // // set yaml file path
360 // std:: string execute_param_1_string = load_configs_pkg_path+"/cfg/planner/"+algorithm_file;
361 // const char* execute_param_1 = execute_param_1_string.c_str();
362 // // set params namespace
363 // std:: string execute_param_2_string = "/"+global_planner_pkg_name+"/planner/";
364 // const char* execute_param_2 = execute_param_2_string.c_str();
365 // execl(execute_command,execute_command,"load", execute_param_1,execute_param_2 , (char *)0);
366 // }
367 // else if (load_configs_planner_pID < 0)
368 // {
369 // ROS_ERROR("Failed to fork load_configs");
370 // exit(1);
371 
372 // }
373 // else{
374 // pid_t global_planner_pID = fork();
375 // if (global_planner_pID == 0)
376 // {
377 // ROS_DEBUG("starting global_planner_pID");
378 
379 // execute_command = "/opt/ros/indigo/bin/rosrun";
380 
381 // std::string global_planner_pkg_path = ros::package::getPath("global_planner");
382 // execute_pkg_name = "global_planner";
383 // execute_pkg_exe_name = "planner";
384 // // ROS node name
385 // std:: string execute_param_1_string = "__name:="+global_planner_pkg_name;
386 // const char* execute_param_1 = execute_param_1_string.c_str();
387 // // remap map subscribtion topic
388 // std:: string execute_param_2_string = "/map:=/"+map_server_pkg_name+"/map";
389 // const char* execute_param_2 = execute_param_2_string.c_str();
390 // std::string map_topic_test;
391 // bool use_dijkstra_test;
392 // while (!nh_.getParam("/"+global_planner_pkg_name+"/costmap/map_topic", map_topic_test) && !nh_.getParam("/"+global_planner_pkg_name+"/planner/use_dijkstra",use_dijkstra_test) ){
393 // ROS_DEBUG("Global planner is waiting for configuration");
394 // }
395 // execl(execute_command,execute_command,execute_pkg_name, execute_pkg_exe_name, execute_param_1, execute_param_2, (char *)0);
396 
397 // }
398 // else if (global_planner_pID < 0)
399 // {
400 // ROS_ERROR("Failed to fork global_planner");
401 // exit(1);
402 
403 // }
404 // else{
405 // if(!nh_.getParam("/rapp_path_planning_plan_path_topic", pathPlanningTopic_))
406 // {
407 // ROS_ERROR("Path planning topic param does not exist");
408 // }
409 // ROS_DEBUG_STREAM("GP name:\n" << global_planner_pkg_name << "service name call:\n" << "/"+global_planner_pkg_name+"/make_plan");
410 // uint32_t serv_port;
411 // std::string serv_node_name, serv_name;
412 // serv_name = "/"+global_planner_pkg_name+"/make_plan";
413 // ros::ServiceManager srv_manager;
414 // bool serv_active = srv_manager.lookupService(serv_name,serv_node_name,serv_port);
415 
416 
417 // while (!serv_active){
418 // ROS_ERROR("Can't find service make_plan");
419 // serv_active = srv_manager.lookupService(serv_name,serv_node_name,serv_port);
420 // ros::Duration(1).sleep();
421 // }
422 // ROS_DEBUG_STREAM("HOST name:\n" << serv_node_name << "service port:\n" << serv_port);
423 
424 // ros::ServiceClient client = nh_.serviceClient<navfn::MakeNavPlan>("/"+global_planner_pkg_name+"/make_plan");
425 // navfn::MakeNavPlan srv;
426 // srv.request.start = request_start;
427 // srv.request.goal = request_goal;
428 // if (client.call(srv))
429 // {
430 // navfn::MakeNavPlanResponse planned_path;
431 // planned_path = srv.response;
432 // ROS_DEBUG("Path planning service ended");
433 // ROS_DEBUG_STREAM("\n nodes_ID:\n"<<"map_server: "<<map_server_pID <<"\ntf_broadcaster_pID: "<<tf_broadcaster_pID <<"\nload_configs_costmap_pID: "<<load_configs_costmap_pID <<"\nload_configs_planner_pID: "<<load_configs_planner_pID <<"\nglobal_planner_pID: "<<global_planner_pID);
434 // pid_t killer_ID = fork();
435 // if (killer_ID == 0)
436 // {
437 // deleteParameters(new_nodes_string_id,nh_);
438 // ROS_DEBUG("starting killer_ID");
439 
440 // execute_command = "/opt/ros/indigo/bin/rosnode";
441 
442 // // ROS node name
443 // std:: string execute_param_1_string = global_planner_pkg_name;
444 // const char* execute_param_1 = execute_param_1_string.c_str();
445 // // remap map subscribtion topic
446 // std:: string execute_param_2_string = map_server_pkg_name;
447 // const char* execute_param_2 = execute_param_2_string.c_str();
448 // std:: string execute_param_3_string = tf_broadcaster_pkg_name;
449 // const char* execute_param_3 = execute_param_3_string.c_str();
450 
451 // execl(execute_command,execute_command,"kill", execute_param_1, execute_param_2,execute_param_3, (char *)0);
452 
453 // }
454 // else if (killer_ID < 0)
455 // {
456 // ROS_ERROR("Failed to fork global_planner");
457 // exit(1);
458 
459 // }
460 // else{
461 // return planned_path;
462 
463 // }
464 // }
465 // else
466 // {
467 // navfn::MakeNavPlanResponse planned_path;
468 // ROS_ERROR("Failed to call service make_plan");
469 // kill(map_server_pID,SIGINT);
470 // kill(tf_broadcaster_pID,SIGINT);
471 // kill(load_configs_costmap_pID,SIGINT);
472 // kill(load_configs_planner_pID,SIGINT);
473 // kill(global_planner_pID,SIGINT);
474 
475 
476 // return planned_path;
477 // }
478 
479 // }
480 
481 // }
482 // }
483 // }
484 
485 // }
486 
487 // }
488 // }
489 
PathPlanner::setSequenceNR
std::string setSequenceNR(ros::NodeHandle &nh_, int pathPlanningThreads_)
Determines next planning seqence (which global_planner and map_server should be used) ...
Definition: path_planner.cpp:9
PathPlanner::configureSequence
bool configureSequence(std::string seq_nr, std::string map_path, std::string robot_type, std::string algorithm, ros::NodeHandle &nh_)
Configures sequence. Sets map, approprate costmap parameters for specified robot type, sets global_planner to use detemined algorithm.
Definition: path_planner.cpp:26
PathPlanner::PathPlanner
PathPlanner(void)
Default constructor.
Definition: path_planner.cpp:4
PathPlanner::startSequence
navfn::MakeNavPlanResponse startSequence(std::string seq_nr, geometry_msgs::PoseStamped request_start, geometry_msgs::PoseStamped request_goal, ros::NodeHandle &nh_)
Starts planning service of determined planning sequence.
Definition: path_planner.cpp:137
path_planning.h