Merge branch 'devel' into 'master'

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

See merge request !4

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

@@ -88,14 +88,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;
-
-      if(this->private_nh.getParam("resolution", resolution))
-        this->roadmap.set_resolution(resolution);
-      else
-        ROS_ERROR("OpendriveGlobalPlanner: Map resolution not defined");
+      bool lane_mode=false;
 
       if(this->private_nh.getParam("utm_zone", utm_zone))
       {
@@ -105,6 +100,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!="")
@@ -121,7 +122,7 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
             }
             else
               ROS_ERROR("Unknown file type");
-            this->create_opendrive_map(opendrive_file,resolution, utm_zone);
+            this->create_opendrive_map(opendrive_file,this->roadmap.get_resolution(), utm_zone);
           }catch(CException &e){
             ROS_ERROR_STREAM(e.what());
           }
@@ -242,8 +243,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 +261,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 +342,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 +439,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 +450,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 +540,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;
   }