Added parameters to configure the planner.

Used the new version of the C++ library.

cfg/OpendriveGlobalPlanner.cfg

@@ -4,6 +4,18 @@ PACKAGE = "iri_opendrive_global_planner"
 from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
+
+enum_cost_type = gen.enum([ 
+gen.const("dist", int_t, 0, "Use only distance to compute costs"),
+gen.const("time",  int_t, 1, "Use distance and speed to compute costs"),
+], "Possible costs types.")
+
+
 gen.add("opendrive_file", str_t, 0, "Opendrive map file", "")
+gen.add("angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 1.5707)
+gen.add("dist_tol", double_t, 0, "Distance tolerance to find start and end positions on the road map", 3.0, 0.5, 10.0)
+gen.add("multi_hyp", bool_t, 0, "Use multi hypothesis path search", False)
+gen.add("resolution", double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0)
+gen.add("cost_type", int_t, 0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
 
 exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))

include/iri_opendrive_global_planner/opendrive_planner.h

@@ -119,6 +119,9 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
     bool initialized_;
 
     CRoadMap roadmap;        
+    double angle_tol;
+    double dist_tol;
+    bool multi_hyp;
 
   private:
     void mapToWorld(double mx, double my, double& wx, double& wy);

src/opendrive_planner.cpp

@@ -50,6 +50,9 @@ namespace iri_opendrive_global_planner {
 
 OpendriveGlobalPlanner::OpendriveGlobalPlanner() :
   costmap_(NULL), initialized_(false) {
+  this->angle_tol=DEFAULT_ANGLE_THRESHOLD;
+  this->dist_tol=DEFAULT_DIST_THRESHOLD;
+  this->multi_hyp=false;
 }
 
 OpendriveGlobalPlanner::OpendriveGlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
@@ -83,6 +86,15 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
       private_nh.param("opendrive_file", opendrive_file,std::string(""));
       if(opendrive_file!="")
         this->roadmap.load(opendrive_file);
+      double resolution;
+      private_nh.param("resolution", resolution,0.0);
+      this->roadmap.set_resolution(resolution);
+      private_nh.param("angle_tol", this->angle_tol,DEFAULT_ANGLE_THRESHOLD);
+      private_nh.param("dist_tol", this->dist_tol,DEFAULT_DIST_THRESHOLD);
+      private_nh.param("multi_hyp", this->multi_hyp,false);
+      int cost_type;
+      private_nh.param("cost_type",cost_type,(int)COST_DIST);
+      this->roadmap.set_cost_type((cost_t)cost_type);
     }catch(CException &e){
       ROS_ERROR_STREAM(e.what());
     }
@@ -104,7 +116,12 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
   try{
     if(new_config.opendrive_file!=this->config.opendrive_file)
       this->roadmap.load(new_config.opendrive_file);
+    this->angle_tol=new_config.angle_tol;
+    this->dist_tol=new_config.dist_tol;
+    this->multi_hyp=new_config.multi_hyp;
     this->config=new_config;
+    this->roadmap.set_resolution(new_config.resolution);
+    this->roadmap.set_cost_type((cost_t)new_config.cost_type);
   }catch(CException &e){
     ROS_ERROR_STREAM(e.what());
   }
@@ -162,10 +179,10 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
 
 bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) 
 {
-  double yaw;
+  double yaw,cost;
+  unsigned int best_path_index;
   std::vector<unsigned int> path;
   std::vector<double> x,y,heading;
-  TPoint real_goal;
 
   boost::mutex::scoped_lock lock(mutex_);
   if (!initialized_) {
@@ -214,15 +231,16 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
   try{
     ROS_WARN("Make Plan");
     yaw=tf2::getYaw(start.pose.orientation);
-    this->roadmap.set_start_point(start.pose.position.x,start.pose.position.y,yaw);
+    this->roadmap.set_start_pose(start.pose.position.x,start.pose.position.y,yaw,this->dist_tol,this->angle_tol);
     yaw=tf2::getYaw(goal.pose.orientation);
-    real_goal=this->roadmap.set_target_point(goal.pose.position.x,goal.pose.position.y,yaw);
-    this->roadmap.find_shortest_path();
-    this->roadmap.get_path(path);
+    this->roadmap.set_end_pose(goal.pose.position.x,goal.pose.position.y,yaw,this->dist_tol,this->angle_tol);
+    this->roadmap.find_shortest_path(this->multi_hyp);
+    best_path_index=this->roadmap.get_best_path(cost,path);
+//    std::cout << "best path index: " << best_path_index << std::endl;
 //    for(unsigned int i=0;i<path.size();i++)
 //      std::cout << path[i] << ",";
 //    std::cout << std::endl;
-    this->roadmap.get_trajectory(x,y,heading);
+    this->roadmap.get_trajectory(best_path_index,x,y,heading);
     plan.resize(x.size());
     ros::Time plan_time=ros::Time::now();
     for(unsigned int i=0;i<x.size();i++)