Commit c45caabc authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added parameters to configure the planner.

Used the new version of the C++ library.
parent 7ca6636c
...@@ -4,6 +4,18 @@ PACKAGE = "iri_opendrive_global_planner" ...@@ -4,6 +4,18 @@ PACKAGE = "iri_opendrive_global_planner"
from dynamic_reconfigure.parameter_generator_catkin import * from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator() gen = ParameterGenerator()
enum_cost_type = gen.enum([
gen.const("dist", int_t, 0, "Use only distance to compute costs"),
gen.const("time", int_t, 1, "Use distance and speed to compute costs"),
], "Possible costs types.")
gen.add("opendrive_file", str_t, 0, "Opendrive map file", "") gen.add("opendrive_file", str_t, 0, "Opendrive map file", "")
gen.add("angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 1.5707)
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("cost_type", int_t, 0, "Cost type", 0, 0, 1, edit_method=enum_cost_type)
exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner")) exit(gen.generate(PACKAGE, "iri_opendrive_global_planner", "OpendriveGlobalPlanner"))
...@@ -119,6 +119,9 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner ...@@ -119,6 +119,9 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
bool initialized_; bool initialized_;
CRoadMap roadmap; CRoadMap roadmap;
double angle_tol;
double dist_tol;
bool multi_hyp;
private: private:
void mapToWorld(double mx, double my, double& wx, double& wy); void mapToWorld(double mx, double my, double& wx, double& wy);
......
...@@ -50,6 +50,9 @@ namespace iri_opendrive_global_planner { ...@@ -50,6 +50,9 @@ namespace iri_opendrive_global_planner {
OpendriveGlobalPlanner::OpendriveGlobalPlanner() : OpendriveGlobalPlanner::OpendriveGlobalPlanner() :
costmap_(NULL), initialized_(false) { costmap_(NULL), initialized_(false) {
this->angle_tol=DEFAULT_ANGLE_THRESHOLD;
this->dist_tol=DEFAULT_DIST_THRESHOLD;
this->multi_hyp=false;
} }
OpendriveGlobalPlanner::OpendriveGlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) : OpendriveGlobalPlanner::OpendriveGlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
...@@ -83,6 +86,15 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* ...@@ -83,6 +86,15 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
private_nh.param("opendrive_file", opendrive_file,std::string("")); private_nh.param("opendrive_file", opendrive_file,std::string(""));
if(opendrive_file!="") if(opendrive_file!="")
this->roadmap.load(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);
int cost_type;
private_nh.param("cost_type",cost_type,(int)COST_DIST);
this->roadmap.set_cost_type((cost_t)cost_type);
}catch(CException &e){ }catch(CException &e){
ROS_ERROR_STREAM(e.what()); ROS_ERROR_STREAM(e.what());
} }
...@@ -104,7 +116,12 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri ...@@ -104,7 +116,12 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
try{ try{
if(new_config.opendrive_file!=this->config.opendrive_file) if(new_config.opendrive_file!=this->config.opendrive_file)
this->roadmap.load(new_config.opendrive_file); 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->config=new_config;
this->roadmap.set_resolution(new_config.resolution);
this->roadmap.set_cost_type((cost_t)new_config.cost_type);
}catch(CException &e){ }catch(CException &e){
ROS_ERROR_STREAM(e.what()); ROS_ERROR_STREAM(e.what());
} }
...@@ -162,10 +179,10 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -162,10 +179,10 @@ 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) bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)
{ {
double yaw; double yaw,cost;
unsigned int best_path_index;
std::vector<unsigned int> path; std::vector<unsigned int> path;
std::vector<double> x,y,heading; std::vector<double> x,y,heading;
TPoint real_goal;
boost::mutex::scoped_lock lock(mutex_); boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) { if (!initialized_) {
...@@ -214,15 +231,16 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c ...@@ -214,15 +231,16 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
try{ try{
ROS_WARN("Make Plan"); ROS_WARN("Make Plan");
yaw=tf2::getYaw(start.pose.orientation); yaw=tf2::getYaw(start.pose.orientation);
this->roadmap.set_start_point(start.pose.position.x,start.pose.position.y,yaw); this->roadmap.set_start_pose(start.pose.position.x,start.pose.position.y,yaw,this->dist_tol,this->angle_tol);
yaw=tf2::getYaw(goal.pose.orientation); yaw=tf2::getYaw(goal.pose.orientation);
real_goal=this->roadmap.set_target_point(goal.pose.position.x,goal.pose.position.y,yaw); this->roadmap.set_end_pose(goal.pose.position.x,goal.pose.position.y,yaw,this->dist_tol,this->angle_tol);
this->roadmap.find_shortest_path(); this->roadmap.find_shortest_path(this->multi_hyp);
this->roadmap.get_path(path); 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++) // for(unsigned int i=0;i<path.size();i++)
// std::cout << path[i] << ","; // std::cout << path[i] << ",";
// std::cout << std::endl; // std::cout << std::endl;
this->roadmap.get_trajectory(x,y,heading); this->roadmap.get_trajectory(best_path_index,x,y,heading);
plan.resize(x.size()); plan.resize(x.size());
ros::Time plan_time=ros::Time::now(); ros::Time plan_time=ros::Time::now();
for(unsigned int i=0;i<x.size();i++) for(unsigned int i=0;i<x.size();i++)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment