diff --git a/cfg/OpendriveGlobalPlanner.cfg b/cfg/OpendriveGlobalPlanner.cfg
index c17e7228c45a0dcb2e7c19956e35da18f001c1d3..0daa06cef3713644b8454e0c5d6c7cb3cc7959a7 100755
--- a/cfg/OpendriveGlobalPlanner.cfg
+++ b/cfg/OpendriveGlobalPlanner.cfg
@@ -16,6 +16,8 @@ gen.add("angle_tol", double_t, 0, "Angle tolerance to find start and end positio
gen.add("dist_tol", double_t, 0, "Distance tolerance to find start and end positions on the road map", 3.0, 0.5, 10.0)
gen.add("multi_hyp", bool_t, 0, "Use multi hypothesis path search", False)
gen.add("resolution", double_t, 0, "Resolution of the generated path", 0.1, 0.01, 1.0)
+gen.add("scale_factor", double_t, 0, "Scale factor of the input road description", 1.0, 0.01, 10.0)
+gen.add("min_road_length", double_t, 0, "Minimum road length to take it into account", 0.1, 0.01, 1.0)
gen.add("cost_type", int_t, 0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))
diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index ed3847ab53310ce1be3483b22b438ce47a92e8e4..620432447b4d75a4cb3759fb256548bf8dec6ada 100644
--- a/src/opendrive_planner.cpp
+++ b/src/opendrive_planner.cpp
@@ -75,6 +75,8 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DR
void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id)
{
+ double value;
+
if (!initialized_)
{
ros::NodeHandle private_nh("~/" + name);
@@ -83,15 +85,19 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
try{
std::string opendrive_file;
+ private_nh.param("resolution", value,0.0);
+ this->roadmap.set_resolution(value);
+ private_nh.param("scale_factor", value,0.0);
+ this->roadmap.set_scale_factor(value);
+ private_nh.param("min_road_length", value,0.0);
+ this->roadmap.set_min_road_length(value);
private_nh.param("opendrive_file", opendrive_file,std::string(""));
if(opendrive_file!="")
this->roadmap.load(opendrive_file);
- double resolution;
- private_nh.param("resolution", resolution,0.0);
- this->roadmap.set_resolution(resolution);
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);
+ std::cout << "multi hypothessis: " << this->multi_hyp << std::endl;
int cost_type;
private_nh.param("cost_type",cost_type,(int)COST_DIST);
this->roadmap.set_cost_type((cost_t)cost_type);
@@ -114,13 +120,15 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig& new_config, uint32_t level)
{
try{
- if(new_config.opendrive_file!=this->config.opendrive_file)
+ this->roadmap.set_resolution(new_config.resolution);
+ this->roadmap.set_scale_factor(new_config.scale_factor);
+ this->roadmap.set_min_road_length(new_config.min_road_length);
+ if(new_config.opendrive_file!=this->config.opendrive_file || new_config.scale_factor!=this->config.scale_factor || new_config.min_road_length!=this->config.min_road_length)
this->roadmap.load(new_config.opendrive_file);
this->angle_tol=new_config.angle_tol;
this->dist_tol=new_config.dist_tol;
this->multi_hyp=new_config.multi_hyp;
this->config=new_config;
- this->roadmap.set_resolution(new_config.resolution);
this->roadmap.set_cost_type((cost_t)new_config.cost_type);
}catch(CException &e){
ROS_ERROR_STREAM(e.what());
@@ -179,10 +187,13 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)
{
- double yaw,cost;
+ double yaw,best_cost;
unsigned int best_path_index;
- std::vector<unsigned int> path;
+ std::vector<unsigned int> best_path;
std::vector<double> x,y,heading;
+ std::vector< std::vector<unsigned int> > paths;
+ std::vector<double> costs;
+ std::vector<TMapPose> start_candidates,end_candidates;
boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
@@ -232,14 +243,38 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
ROS_WARN("Make Plan");
yaw=tf2::getYaw(start.pose.orientation);
this->roadmap.set_start_pose(start.pose.position.x,start.pose.position.y,yaw,this->dist_tol,this->angle_tol);
+ this->roadmap.get_start_pose_candidates(start_candidates);
+ std::cout << "Start pose candidates: " << std::endl;
+ for(unsigned int i=0;i<start_candidates.size();i++)
+ {
+ std::cout << i << ". x: " << start_candidates[i].pose.x << ", y:" << start_candidates[i].pose.y << ", heading: " << start_candidates[i].pose.heading << std::endl;
+ std::cout << " road map node: " << start_candidates[i].node_index << " child: " << start_candidates[i].child_index << std::endl;
+ std::cout << " distance from desired pose: " << start_candidates[i].dist << std::endl;
+ }
yaw=tf2::getYaw(goal.pose.orientation);
this->roadmap.set_end_pose(goal.pose.position.x,goal.pose.position.y,yaw,this->dist_tol,this->angle_tol);
+ this->roadmap.get_end_pose_candidates(end_candidates);
+ std::cout << "End pose candidates: " << std::endl;
+ for(unsigned int i=0;i<end_candidates.size();i++)
+ {
+ std::cout << i << ". x: " << end_candidates[i].pose.x << ", y:" << end_candidates[i].pose.y << ", heading: " << end_candidates[i].pose.heading << std::endl;
+ std::cout << " road map node: " << end_candidates[i].node_index << " child: " << end_candidates[i].child_index << std::endl;
+ std::cout << " distance from desired pose: " << end_candidates[i].dist << std::endl;
+ }
this->roadmap.find_shortest_path(this->multi_hyp);
- best_path_index=this->roadmap.get_best_path(cost,path);
-// std::cout << "best path index: " << best_path_index << std::endl;
-// for(unsigned int i=0;i<path.size();i++)
-// std::cout << path[i] << ",";
-// std::cout << std::endl;
+ this->roadmap.get_paths(costs,paths);
+ for(unsigned int i=0;i<paths.size();i++)
+ {
+ std::cout << "path " << i << ":" << std::endl;
+ for(unsigned int j=0;j<paths[i].size();j++)
+ std::cout << paths[i][j] << ",";
+ std::cout << std::endl;
+ }
+ best_path_index=this->roadmap.get_best_path(best_cost,best_path);
+ std::cout << "best path index: " << best_path_index << std::endl;
+ for(unsigned int i=0;i<best_path.size();i++)
+ std::cout << best_path[i] << ",";
+ std::cout << std::endl;
this->roadmap.get_trajectory(best_path_index,x,y,heading);
plan.resize(x.size());
ros::Time plan_time=ros::Time::now();