Updated the ROS node to the latest version of the autonomous_driving_tools and...

Updated the ROS node to the latest version of the autonomous_driving_tools and road_map_planner libraries.

cfg/OpendriveGlobalPlanner.cfg

@@ -20,6 +20,7 @@ gen.add("multi_hyp",       bool_t,   0, "Use multi hypothesis path search", Fals
 gen.add("resolution",      double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0)
 gen.add("utm_zone",        int_t,    0, "UTM zone for geodesy lat/lon to UTM conversions", 31, 1, 60)
 gen.add("cost_type",       int_t,    0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
-gen.add("debug",  bool_t,    0, "Show debug messages", False)
+gen.add("lane_mode",       bool_t,   0, "Whether to use segment or lane planning", False)
+gen.add("debug",           bool_t,   0, "Show debug messages", False)
 
 exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))

include/iri_opendrive_global_planner/opendrive_planner.h

@@ -136,7 +136,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
 
     CRoadMapPlanner roadmap;        
     bool load_osm_paths(std::vector<TOSMPathData> &osm_paths);
-    std::vector<unsigned int> best_path;
+    std::vector<TLaneID> best_path;
     ros::Time best_path_stamp;
     double start_angle_tol;
     double start_dist_tol;

src/opendrive_planner.cpp

@@ -89,8 +89,9 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
     try{
       std::string opendrive_file;
       double resolution;
-      int cost_type;
+      int cost_type=COST_DIST;
       int utm_zone;
+      bool lane_mode=false;
 
       if(this->private_nh.getParam("resolution", resolution))
         this->roadmap.set_resolution(resolution);
@@ -105,6 +106,12 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
       else
         ROS_ERROR("OpendriveGlobalPlanner: utm zone not defined");
 
+      if(!this->private_nh.getParam("lane_mode",lane_mode))
+        ROS_ERROR("OpendriveGlobalPlanner: Desired planning mode not defined");
+      if(lane_mode)
+        this->roadmap.set_lane_mode();
+      else
+        this->roadmap.set_segment_mode();
       if(this->private_nh.getParam("opendrive_file", opendrive_file))
       {
         if(opendrive_file!="")
@@ -242,8 +249,7 @@ bool OpendriveGlobalPlanner::get_opendrive_nodes(iri_nav_module::get_opendrive_n
   res.opendrive_nodes.header.stamp=this->best_path_stamp;
   res.opendrive_nodes.nodes.resize(this->best_path.size());
   for(unsigned int i=0;i<this->best_path.size();i++)
-    res.opendrive_nodes.nodes[i]=this->best_path[i];
-  res.opendrive_nodes.nodes=this->best_path;
+    res.opendrive_nodes.nodes[i]=this->best_path[i].segment_id;
 
   return true;
 }
@@ -261,7 +267,11 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
     this->end_angle_tol=new_config.end_angle_tol;
     this->end_dist_tol=new_config.end_dist_tol;
     this->multi_hyp=new_config.multi_hyp;
-    if(new_config.opendrive_file!=this->config.opendrive_file)
+    if(new_config.lane_mode)
+      this->roadmap.set_lane_mode();
+    else
+      this->roadmap.set_segment_mode();
+    if(new_config.opendrive_file!=this->config.opendrive_file || new_config.lane_mode!=this->config.lane_mode)
     {
       if(new_config.opendrive_file.find("xodr")!=std::string::npos)
         roadmap.load_opendrive(new_config.opendrive_file);
@@ -338,7 +348,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
   double yaw,best_cost;
   unsigned int best_path_index;
   std::vector<double> x,y,heading;
-  std::vector< std::vector<unsigned int> > paths;
+  std::vector< std::vector<TLaneID> > paths;
   std::vector<double> costs;
   std::vector<TMapPose> start_candidates,end_candidates;
   geometry_msgs::PoseStamped start_out,goal_out,point;
@@ -435,7 +445,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
       {
         std::cout << "--- path " << i << ":" << std::endl;
         for(unsigned int j=0;j<paths[i].size();j++)
-          std::cout <<"--- " <<paths[i][j] << ",";
+          std::cout <<"--- (" << paths[i][j].segment_id << "," << paths[i][j].lane_id << "),";
         std::cout << std::endl;
       }
     }
@@ -446,7 +456,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
     {
       std::cout << "--- best path index: " << best_path_index << std::endl;
       for(unsigned int i=0;i<this->best_path.size();i++)
-        std::cout <<"--- "<< this->best_path[i] << ",";
+        std::cout <<"--- (" << this->best_path[i].segment_id << "," << this->best_path[i].lane_id << "),";
       std::cout << std::endl;
     }
     this->best_path_stamp=ros::Time::now();
@@ -536,14 +546,17 @@ bool OpendriveGlobalPlanner::load_osm_paths(std::vector<TOSMPathData> &osm_paths
       return false;
     name_space=categories[i]+"/min_turn_radius";
     if(!this->private_nh.getParam(name_space,osm_paths[i].min_turn_radius))
-      return false;
+      osm_paths[i].min_turn_radius=5.0;
     name_space=categories[i]+"/min_road_length";
     if(!this->private_nh.getParam(name_space,osm_paths[i].min_road_length))
-      return false;
+      osm_paths[i].min_road_length=0.1;
     name_space=categories[i]+"/use_default_lane_width";
     if(!this->private_nh.getParam(name_space,osm_paths[i].use_default_lane_width))
-      return false;
-    name_space=categories[i]+"/force_twoways";
+      osm_paths[i].use_default_lane_width=true;
+    name_space=categories[i]+"/default_lane_width";
+    if(!this->private_nh.getParam(name_space,osm_paths[i].default_lane_width))
+      osm_paths[i].default_lane_width=4.0;
+    name_space=categories[i]+"/force_twoway";
     if(!this->private_nh.getParam(name_space,osm_paths[i].force_two_ways))
       return false;
   }