diff --git a/cfg/OpendriveGlobalPlanner.cfg b/cfg/OpendriveGlobalPlanner.cfg
index 0453c040a230019aa379e675c6f6b59dc445cb8f..c12e6bf230de9cbb86d6150e34cdfb58278106e4 100755
--- a/cfg/OpendriveGlobalPlanner.cfg
+++ b/cfg/OpendriveGlobalPlanner.cfg
@@ -12,8 +12,10 @@ gen.const("time", int_t, 1, "Use distance and speed to compute costs"),
gen.add("opendrive_file", str_t, 0, "Opendrive map file", "")
gen.add("opendrive_frame", str_t, 0, "Opendrive frame ID", "")
-gen.add("angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 10.0)
-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("start_angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 10.0)
+gen.add("start_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("end_angle_tol", double_t, 0, "Angle tolerance to find start and end positions on the road map", 0.5, 0.1, 10.0)
+gen.add("end_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)
diff --git a/include/iri_opendrive_global_planner/opendrive_planner.h b/include/iri_opendrive_global_planner/opendrive_planner.h
index 2ea9ae5ee9665761db9f19fc2741458019e8ca9d..786f38bf2d8859d5ceffd94c6cc441ae7639917f 100644
--- a/include/iri_opendrive_global_planner/opendrive_planner.h
+++ b/include/iri_opendrive_global_planner/opendrive_planner.h
@@ -137,8 +137,10 @@ class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner
COpendriveRoadMap roadmap;
std::vector<unsigned int> best_path;
ros::Time best_path_stamp;
- double angle_tol;
- double dist_tol;
+ double start_angle_tol;
+ double start_dist_tol;
+ double end_angle_tol;
+ double end_dist_tol;
bool multi_hyp;
private:
diff --git a/src/opendrive_planner.cpp b/src/opendrive_planner.cpp
index 7d7cd9def21efc7d9493c74e46d73b9716019fee..0ac42a4cf5bbacc18798c2d3a1cee19954507a21 100644
--- a/src/opendrive_planner.cpp
+++ b/src/opendrive_planner.cpp
@@ -51,8 +51,10 @@ namespace iri_opendrive_global_planner {
OpendriveGlobalPlanner::OpendriveGlobalPlanner() :
costmap_(NULL), initialized_(false), tf2_listener(tf2_buffer) {
- this->angle_tol=DEFAULT_ANGLE_THRESHOLD;
- this->dist_tol=DEFAULT_DIST_THRESHOLD;
+ this->start_angle_tol=DEFAULT_ANGLE_THRESHOLD;
+ this->start_dist_tol=DEFAULT_DIST_THRESHOLD;
+ this->end_angle_tol=DEFAULT_ANGLE_THRESHOLD;
+ this->end_dist_tol=DEFAULT_DIST_THRESHOLD;
this->multi_hyp=false;
}
@@ -115,10 +117,14 @@ void OpendriveGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D*
ROS_ERROR("OpendriveGlobalPlanner: Opendrive map file not defined");
if(!private_nh.getParam("opendrive_frame", this->opendrive_frame_id_))
ROS_ERROR("OpendriveGlobalPlanner: Opendrive frame ID not defined");
- if(!private_nh.getParam("angle_tol", this->angle_tol))
- ROS_ERROR("OpendriveGlobalPlanner: Angle tolerance for the goals not defined");
- if(!private_nh.getParam("dist_tol", this->dist_tol))
- ROS_ERROR("OpendriveGlobalPlanner: Distance tolerance for the goals not defined");
+ if(!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))
+ ROS_ERROR("OpendriveGlobalPlanner: Distance tolerance for the start position not defined");
+ if(!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))
+ ROS_ERROR("OpendriveGlobalPlanner: Distance tolerance for the end position not defined");
if(!private_nh.getParam("multi_hyp", this->multi_hyp))
ROS_ERROR("OpendriveGlobalPlanner: Multihypotheis feature not defined");
if(!private_nh.getParam("cost_type",cost_type))
@@ -235,8 +241,10 @@ void OpendriveGlobalPlanner::reconfigureCB(iri_opendrive_global_planner::Opendri
this->roadmap.set_scale_factor(new_config.scale_factor);
this->roadmap.set_min_road_length(new_config.min_road_length);
this->opendrive_frame_id_=new_config.opendrive_frame;
- this->angle_tol=new_config.angle_tol;
- this->dist_tol=new_config.dist_tol;
+ this->start_angle_tol=new_config.start_angle_tol;
+ this->start_dist_tol=new_config.start_dist_tol;
+ 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 || new_config.scale_factor!=this->config.scale_factor || new_config.min_road_length!=this->config.min_road_length)
{
@@ -369,7 +377,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
{
//ROS_WARN("Make Plan");
yaw=tf2::getYaw(start_out.pose.orientation);
- this->roadmap.set_start_pose(start_out.pose.position.x,start_out.pose.position.y,yaw,this->dist_tol,this->angle_tol);
+ this->roadmap.set_start_pose(start_out.pose.position.x,start_out.pose.position.y,yaw,this->start_dist_tol,this->start_angle_tol);
this->roadmap.get_start_pose_candidates(start_candidates);
if(this->config.debug)
{
@@ -381,7 +389,7 @@ bool OpendriveGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, c
}
}
yaw=tf2::getYaw(goal_out.pose.orientation);
- this->roadmap.set_end_pose(goal_out.pose.position.x,goal_out.pose.position.y,yaw,this->dist_tol,this->angle_tol);
+ this->roadmap.set_end_pose(goal_out.pose.position.x,goal_out.pose.position.y,yaw,this->end_dist_tol,this->end_angle_tol);
this->roadmap.get_end_pose_candidates(end_candidates);
if(this->config.debug)
{