diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp index 46a7164bdbd62ae18376c43ab12c123c5bf8d756..2c51eb045cf6d5e9f2dc475d73fd68d389a8cf8e 100644 --- a/src/opendrive_planner.cpp +++ b/src/opendrive_planner.cpp @@ -87,23 +87,42 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* double resolution,scale_factor,min_road_length; int cost_type; - private_nh.param("resolution", resolution,0.0); - this->roadmap.set_resolution(resolution); - private_nh.param("scale_factor", scale_factor,0.0); - this->roadmap.set_scale_factor(scale_factor); - private_nh.param("min_road_length", min_road_length,0.0); - this->roadmap.set_min_road_length(min_road_length); - private_nh.param("opendrive_file", opendrive_file,std::string("")); - if(opendrive_file!="") + if(private_nh.getParam("resolution", resolution)) + this->roadmap.set_resolution(resolution); + else + ROS_ERROR("Map resolution not defined"); + if(private_nh.getParam("scale_factor", scale_factor)) + this->roadmap.set_scale_factor(scale_factor); + else + ROS_ERROR("Map scale factor not defined"); + if(private_nh.getParam("min_road_length", min_road_length)) + this->roadmap.set_min_road_length(min_road_length); + else + ROS_ERROR("Map minimum road length not defined"); + if(private_nh.getParam("opendrive_file", opendrive_file)) { - this->roadmap.load(opendrive_file); - this->create_opendrive_map(opendrive_file,resolution,scale_factor,min_road_length); + if(opendrive_file!="") + { + try{ + this->roadmap.load(opendrive_file); + this->create_opendrive_map(opendrive_file,resolution,scale_factor,min_road_length); + }catch(CException &e){ + ROS_ERROR_STREAM(e.what()); + } + } } - private_nh.param("opendrive_frame", this->opendrive_frame_id_,std::string("")); - 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); - private_nh.param("cost_type",cost_type,(int)COST_DIST); + else + ROS_ERROR("Opendrive map file not defined"); + if(!private_nh.getParam("opendrive_frame", this->opendrive_frame_id_)) + ROS_ERROR("Opendrive frame ID not defined"); + if(!private_nh.getParam("angle_tol", this->angle_tol)) + ROS_ERROR("Angle tolerance for the goals not defined"); + if(!private_nh.getParam("dist_tol", this->dist_tol)) + ROS_ERROR("Distance tolerance for the goals not defined"); + if(!private_nh.getParam("multi_hyp", this->multi_hyp)) + ROS_ERROR("Multihypotheis feature not defined"); + if(!private_nh.getParam("cost_type",cost_type)) + ROS_ERROR("Desired cost type not defined"); this->roadmap.set_cost_type((cost_t)cost_type); }catch(CException &e){ ROS_ERROR_STREAM(e.what());