diff --git a/cfg/OpendriveGlobalPlanner.cfg b/cfg/OpendriveGlobalPlanner.cfg
index fdc842008583ac02c339de60ca5316427f528968..5278b8d9b2be68d0015e387b026a19dc902c40d7 100755
--- a/cfg/OpendriveGlobalPlanner.cfg
+++ b/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"))
diff --git a/include/iri_opendrive_global_planner/opendrive_planner.h b/include/iri_opendrive_global_planner/opendrive_planner.h
index ade42e9dea22734cdd7d96b01819ae1775d32e5b..889c9378d3367c118dd34d8860cafc69d5ad5e1d 100644
--- a/include/iri_opendrive_global_planner/opendrive_planner.h
+++ b/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;
diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index 3cc125573abf1072f4b0d74dcc38f8224d78c48c..bd60d1dbee88a1e2da37f4c7665e3fe56ad0152e 100644
--- a/src/opendrive_planner.cpp
+++ b/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;
}