diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg
index e0d5c422274c7959fa43437dab3af26b8961b645..3be8ff0a917d8abbf481b2ec26b5a48c48d697dd 100755
--- a/cfg/AckermannLocalPlanner.cfg
+++ b/cfg/AckermannLocalPlanner.cfg
@@ -104,6 +104,7 @@ splines.add("n2_samples", int_t, 0, "Number of samples for th
splines.add("n3_samples", int_t, 0, "Number of samples for the N3 parameter of the spline", 1,1,100)
splines.add("n4_samples", int_t, 0, "Number of samples for the N4 parameter of the spline", 1,1,100)
splines.add("dist_samples", int_t, 0, "Number of distance samples for the current segment", 1,1,10)
+splines.add("min_segment_points", int_t, 0, "Minimum number of points of each segment", 10,1,100)
splines.add("lateral_offset_gain", double_t, 0, "Gain for the lateral offset steering correction", 0.1,0.001,10)
splines.add("temp_horizon", double_t, 0, "Future time to compute the control command", 0.2,0.01,10)
diff --git a/include/ackermann_spline_generator.h b/include/ackermann_spline_generator.h
index c1099daea59e593275e138ce8469334bf4bd157d..f9992d37015edf3c7a84e7c39f64ad2b7dd99fbe 100644
--- a/include/ackermann_spline_generator.h
+++ b/include/ackermann_spline_generator.h
@@ -93,7 +93,7 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
bool forward;
double sim_period_;
- void get_path_closest_pose(double &path_x,double &path_y);
+ unsigned int get_path_closest_pose(double &path_x,double &path_y);
};
#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */
diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
index aa727713004bfe2507fa253481221c3f96793ce7..92529f9c7d3ccafc25ca3ab3b48ac0a6ea3cb59d 100644
--- a/src/ackermann_spline_generator.cpp
+++ b/src/ackermann_spline_generator.cpp
@@ -51,7 +51,7 @@ AckermannSplineGenerator::AckermannSplineGenerator()
{
}
-void AckermannSplineGenerator::get_path_closest_pose(double &path_x,double &path_y)
+unsigned int AckermannSplineGenerator::get_path_closest_pose(double &path_x,double &path_y)
{
double dist,min_dist=std::numeric_limits<double>::max();
unsigned int min_index=0;
@@ -68,6 +68,8 @@ void AckermannSplineGenerator::get_path_closest_pose(double &path_x,double &path
}
path_x=this->current_plan[min_index].pose.position.x;
path_y=this->current_plan[min_index].pose.position.y;
+
+ return min_index;
}
void AckermannSplineGenerator::initialise(
@@ -77,11 +79,13 @@ void AckermannSplineGenerator::initialise(
bool forward,
std::vector<geometry_msgs::PoseStamped>& current_plan)
{
- double diff_x,diff_y;
+ double diff_x,diff_y,current_x,current_y,step;
double min_n1,max_n1,min_n2,max_n2,min_n3,max_n3,min_n4,max_n4;
- unsigned int step=current_plan.size()/current_config_.dist_samples;
+ unsigned int int_step,current_index,num_samples;
TSplineSample new_sample;
+ this->forward=forward;
+ this->current_plan=current_plan;
pos_=pos;
ackermann_=ackermann;
goal_pos_=goal;
@@ -93,9 +97,25 @@ void AckermannSplineGenerator::initialise(
min_n4=0.0;
max_n4=0.0;
samples_.clear();
- for(unsigned int i=0;i<current_config_.dist_samples;i++)
+ current_index=this->get_path_closest_pose(current_x,current_y);
+ step=(current_plan.size()-current_index)/current_config_.dist_samples;
+ if(step<current_config_.min_segment_points)
{
- Eigen::Vector3f tmp_goal=Eigen::Vector3f(current_plan[step*(i+1)-1].pose.position.x,current_plan[step*(i+1)-1].pose.position.y,tf::getYaw(current_plan[step*(i+1)-1].pose.orientation));
+ num_samples=(current_plan.size()-current_index)/current_config_.min_segment_points;
+ if(num_samples==0)
+ {
+ num_samples=1;
+ step=(current_plan.size()-current_index);
+ }
+ else
+ step=(current_plan.size()-current_index)/num_samples;
+ }
+ else
+ num_samples=current_config_.dist_samples;
+ for(unsigned int i=0;i<num_samples;i++)
+ {
+ int_step=current_index+(unsigned int)(step*(i+1));
+ Eigen::Vector3f tmp_goal=Eigen::Vector3f(current_plan[int_step-1].pose.position.x,current_plan[int_step-1].pose.position.y,tf::getYaw(current_plan[int_step-1].pose.orientation));
diff_x=sqrt(pow(tmp_goal(0)-pos(0),2));
diff_y=sqrt(pow(tmp_goal(1)-pos(1),2));
max_n1=2.0*diff_x;
@@ -106,14 +126,12 @@ void AckermannSplineGenerator::initialise(
for(double current_n4=min_n4;current_n4<=max_n4;current_n4+=(max_n4-min_n4)/(current_config_.n4_samples-1))
{
new_sample.params=Eigen::Vector4f(current_n1,current_n2,current_n3,current_n4);
- new_sample.end_point.x=current_plan[step*(i+1)-1].pose.position.x;
- new_sample.end_point.y=current_plan[step*(i+1)-1].pose.position.y;
- new_sample.end_point.heading=tf::getYaw(current_plan[step*(i+1)-1].pose.orientation);
+ new_sample.end_point.x=current_plan[int_step-1].pose.position.x;
+ new_sample.end_point.y=current_plan[int_step-1].pose.position.y;
+ new_sample.end_point.heading=tf::getYaw(current_plan[int_step-1].pose.orientation);
samples_.push_back(new_sample);
}
}
- this->forward=forward;
- this->current_plan=current_plan;
}
void AckermannSplineGenerator::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg)
@@ -150,7 +168,7 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
else
start.curvature=tan(ackermann_(1))/current_config_.axis_distance;
spline_.set_start_point(start);
-// std::cout << "start position: " << pos_(0) << "," << pos_(1) << "," << pos_(2) << "," << start.curvature << std::endl;
+ //std::cout << "start position: " << pos_(0) << "," << pos_(1) << "," << pos_(2) << "," << start.curvature << std::endl;
end.x=samples_[next_sample_index_].end_point.x;
end.y=samples_[next_sample_index_].end_point.y;
if(this->forward)