Library changes updates

include/iri_opendrive_global_planner/opendrive_planner.h

@@ -46,7 +46,7 @@
 #include <dynamic_reconfigure/server.h>
 #include <iri_opendrive_global_planner/OpendriveGlobalPlannerConfig.h>
 
-#include "road_map.h"
+#include "opendrive_road_map.h"
 
 namespace iri_opendrive_global_planner {
 
@@ -118,7 +118,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
     ros::Publisher plan_pub_;
     bool initialized_;
 
-    CRoadMap roadmap;        
+    COpendriveRoadMap roadmap;        
     double angle_tol;
     double dist_tol;
     bool multi_hyp;

src/opendrive_planner.cpp

@@ -248,7 +248,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
     for(unsigned int i=0;i<start_candidates.size();i++)
     {
       std::cout << i << ". x: " << start_candidates[i].pose.x << ", y:" << start_candidates[i].pose.y << ", heading: " << start_candidates[i].pose.heading << std::endl;
-      std::cout << "   road map node: " << start_candidates[i].node_index << " child: " << start_candidates[i].child_index << std::endl;
+      //std::cout << "   road map node: " << start_candidates[i].node_index << " child: " << start_candidates[i].child_index << std::endl;
       std::cout << "   distance from desired pose: " << start_candidates[i].dist << std::endl;
     }
     yaw=tf2::getYaw(goal.pose.orientation);
@@ -258,7 +258,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
     for(unsigned int i=0;i<end_candidates.size();i++)
     {
       std::cout << i << ". x: " << end_candidates[i].pose.x << ", y:" << end_candidates[i].pose.y << ", heading: " << end_candidates[i].pose.heading << std::endl;
-      std::cout << "   road map node: " << end_candidates[i].node_index << " child: " << end_candidates[i].child_index << std::endl;
+      //std::cout << "   road map node: " << end_candidates[i].node_index << " child: " << end_candidates[i].child_index << std::endl;
       std::cout << "   distance from desired pose: " << end_candidates[i].dist << std::endl;
     }
     this->roadmap.find_shortest_path(this->multi_hyp);