diff --git a/include/iri_opendrive_global_planner/opendrive_planner.h b/include/iri_opendrive_global_planner/opendrive_planner.h
index 0eeec65710a3bd81e6e57cad96543d416d1cdd88..ade42e9dea22734cdd7d96b01819ae1775d32e5b 100644
--- a/include/iri_opendrive_global_planner/opendrive_planner.h
+++ b/include/iri_opendrive_global_planner/opendrive_planner.h
@@ -135,6 +135,7 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
bool initialized_;
CRoadMapPlanner roadmap;
+ bool load_osm_paths(std::vector<TOSMPathData> &osm_paths);
std::vector<unsigned int> best_path;
ros::Time best_path_stamp;
double start_angle_tol;
@@ -143,6 +144,8 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
double end_dist_tol;
bool multi_hyp;
+ ros::NodeHandle private_nh;
+
private:
void mapToWorld(double mx, double my, double& wx, double& wy);
bool worldToMap(double wx, double wy, double& mx, double& my);
diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index daa8b8d43b0c525b9578bd7ced64aaa957e15164..3cc125573abf1072f4b0d74dcc38f8224d78c48c 100644
--- a/src/opendrive_planner.cpp
+++ b/src/opendrive_planner.cpp
@@ -78,9 +78,11 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DR
void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id)
{
+ std::vector<TOSMPathData> osm_paths;
+
if (!initialized_)
{
- ros::NodeHandle private_nh("~/" + name);
+ this->private_nh=ros::NodeHandle("~/" + name);
costmap_ = costmap;
frame_id_ = frame_id;
@@ -90,12 +92,12 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
int cost_type;
int utm_zone;
- if(private_nh.getParam("resolution", resolution))
+ if(this->private_nh.getParam("resolution", resolution))
this->roadmap.set_resolution(resolution);
else
ROS_ERROR("OpendriveGlobalPlanner: Map resolution not defined");
- if(private_nh.getParam("utm_zone", utm_zone))
+ if(this->private_nh.getParam("utm_zone", utm_zone))
{
//ROS_INFO("utm_zone=%d",utm_zone);
this->roadmap.set_utm_zone(utm_zone);
@@ -103,7 +105,7 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
else
ROS_ERROR("OpendriveGlobalPlanner: utm zone not defined");
- if(private_nh.getParam("opendrive_file", opendrive_file))
+ if(this->private_nh.getParam("opendrive_file", opendrive_file))
{
if(opendrive_file!="")
{
@@ -111,7 +113,12 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
if(opendrive_file.find("xodr")!=std::string::npos)
this->roadmap.load_opendrive(opendrive_file);
else if(opendrive_file.find("osm")!=std::string::npos)
- this->roadmap.load_osm(opendrive_file);
+ {
+ if(this->load_osm_paths(osm_paths))
+ this->roadmap.load_osm(opendrive_file,osm_paths);
+ else
+ ROS_ERROR("Invalid OSM categories information");
+ }
else
ROS_ERROR("Unknown file type");
this->create_opendrive_map(opendrive_file,resolution, utm_zone);
@@ -122,33 +129,33 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
}
else
ROS_ERROR("OpendriveGlobalPlanner: Opendrive map file not defined");
- if(!private_nh.getParam("opendrive_frame", this->opendrive_frame_id_))
+ if(!this->private_nh.getParam("opendrive_frame", this->opendrive_frame_id_))
ROS_ERROR("OpendriveGlobalPlanner: Opendrive frame ID not defined");
- if(!private_nh.getParam("start_angle_tol", this->start_angle_tol))
+ if(!this->private_nh.getParam("start_angle_tol", this->start_angle_tol))
ROS_ERROR("OpendriveGlobalPlanner: Angle tolerance for the start position not defined");
- if(!private_nh.getParam("start_dist_tol", this->start_dist_tol))
+ if(!this->private_nh.getParam("start_dist_tol", this->start_dist_tol))
ROS_ERROR("OpendriveGlobalPlanner: Distance tolerance for the start position not defined");
- if(!private_nh.getParam("end_angle_tol", this->end_angle_tol))
+ if(!this->private_nh.getParam("end_angle_tol", this->end_angle_tol))
ROS_ERROR("OpendriveGlobalPlanner: Angle tolerance for the end position not defined");
- if(!private_nh.getParam("end_dist_tol", this->end_dist_tol))
+ if(!this->private_nh.getParam("end_dist_tol", this->end_dist_tol))
ROS_ERROR("OpendriveGlobalPlanner: Distance tolerance for the end position not defined");
- if(!private_nh.getParam("multi_hyp", this->multi_hyp))
+ if(!this->private_nh.getParam("multi_hyp", this->multi_hyp))
ROS_ERROR("OpendriveGlobalPlanner: Multihypotheis feature not defined");
- if(!private_nh.getParam("cost_type",cost_type))
+ if(!this->private_nh.getParam("cost_type",cost_type))
ROS_ERROR("OpendriveGlobalPlanner: Desired cost type not defined");
this->roadmap.set_cost_type((cost_t)cost_type);
}catch(CException &e){
ROS_ERROR_STREAM(e.what());
}
- plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
- make_plan_srv_ = private_nh.advertiseService("make_plan", &OpendriveGlobalPlanner::makePlanService, this);
+ plan_pub_ = this->private_nh.advertise<nav_msgs::Path>("plan", 1);
+ make_plan_srv_ = this->private_nh.advertiseService("make_plan", &OpendriveGlobalPlanner::makePlanService, this);
- this->opendrive_map_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("opendrive_map", 1,boost::bind(&OpendriveGlobalPlanner::map_connect_callback, this, _1));
+ this->opendrive_map_pub_ = this->private_nh.advertise<nav_msgs::OccupancyGrid>("opendrive_map", 1,boost::bind(&OpendriveGlobalPlanner::map_connect_callback, this, _1));
- this->opendrive_map_service=private_nh.advertiseService("get_opendrive_map", &OpendriveGlobalPlanner::get_opendrive_map,this);
+ this->opendrive_map_service=this->private_nh.advertiseService("get_opendrive_map", &OpendriveGlobalPlanner::get_opendrive_map,this);
- this->opendrive_nodes_service=private_nh.advertiseService("get_opendrive_nodes", &OpendriveGlobalPlanner::get_opendrive_nodes,this);
+ this->opendrive_nodes_service=this->private_nh.advertiseService("get_opendrive_nodes", &OpendriveGlobalPlanner::get_opendrive_nodes,this);
dsrv_ = new dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig>(ros::NodeHandle("~/" + name));
dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig>::CallbackType cb = boost::bind(&OpendriveGlobalPlanner::reconfigureCB, this, _1, _2);
@@ -171,6 +178,7 @@ void OpendriveGlobalPlanner::create_opendrive_map(std::string &filename,double r
{
std::vector<double> x,partial_x,y,partial_y,heading;
double max_x=std::numeric_limits<double>::min(),min_x=std::numeric_limits<double>::max(),max_y=std::numeric_limits<double>::min(),min_y=std::numeric_limits<double>::max();
+ std::vector<TOSMPathData> osm_paths;
CRoadMap road;
this->path_access.enter();
@@ -178,7 +186,12 @@ void OpendriveGlobalPlanner::create_opendrive_map(std::string &filename,double r
if(filename.find("xodr")!=std::string::npos)
road.load_opendrive(filename);
else if(filename.find("osm")!=std::string::npos)
- road.load_osm(filename);
+ {
+ if(this->load_osm_paths(osm_paths))
+ road.load_osm(filename,osm_paths);
+ else
+ return;
+ }
else
return;
road.set_resolution(resolution);
@@ -237,6 +250,8 @@ bool OpendriveGlobalPlanner::get_opendrive_nodes(iri_nav_module::get_opendrive_n
void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig& new_config, uint32_t level)
{
+ std::vector<TOSMPathData> osm_paths;
+
try{
boost::mutex::scoped_lock lock(mutex_);
this->roadmap.set_resolution(new_config.resolution);
@@ -251,7 +266,12 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
if(new_config.opendrive_file.find("xodr")!=std::string::npos)
roadmap.load_opendrive(new_config.opendrive_file);
else if(new_config.opendrive_file.find("osm")!=std::string::npos)
- roadmap.load_osm(new_config.opendrive_file);
+ {
+ if(this->load_osm_paths(osm_paths))
+ roadmap.load_osm(new_config.opendrive_file,osm_paths);
+ else
+ ROS_ERROR("Invalid OSM categories information");
+ }
else
ROS_ERROR("Unknown file type");
this->create_opendrive_map(new_config.opendrive_file,new_config.resolution, new_config.utm_zone);
@@ -496,4 +516,39 @@ void OpendriveGlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseSt
plan_pub_.publish(gui_path);
}
+bool OpendriveGlobalPlanner::load_osm_paths(std::vector<TOSMPathData> &osm_paths)
+{
+ std::vector<std::string> categories;
+ std::string name_space;
+
+ /* get the list of categories */
+ if(!this->private_nh.getParam("highway_categories", categories))
+ return false;
+ osm_paths.resize(categories.size());
+ for(unsigned int i=0;i<categories.size();i++)
+ {
+ osm_paths[i].name=categories[i];
+ name_space=categories[i]+"/highways";
+ if(!this->private_nh.getParam(name_space,osm_paths[i].highways))
+ return false;
+ name_space=categories[i]+"/keys";
+ if(!this->private_nh.getParam(name_space,osm_paths[i].keys))
+ return false;
+ name_space=categories[i]+"/min_turn_radius";
+ if(!this->private_nh.getParam(name_space,osm_paths[i].min_turn_radius))
+ return false;
+ name_space=categories[i]+"/min_road_length";
+ if(!this->private_nh.getParam(name_space,osm_paths[i].min_road_length))
+ return false;
+ 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";
+ if(!this->private_nh.getParam(name_space,osm_paths[i].force_two_ways))
+ return false;
+ }
+
+ return true;
+}
+
} //end namespace iri_opendrive_global_planner