diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
index 92529f9c7d3ccafc25ca3ab3b48ac0a6ea3cb59d..b30042fa6752f389666e70d17c36c625b96d0058 100644
--- a/src/ackermann_spline_generator.cpp
+++ b/src/ackermann_spline_generator.cpp
@@ -84,53 +84,57 @@ void AckermannSplineGenerator::initialise(
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;
- next_sample_index_=0;
- min_n1=0.0;
- min_n2=0.0;
- min_n3=0.0;
- max_n3=0.0;
- min_n4=0.0;
- max_n4=0.0;
- samples_.clear();
- 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)
- {
- num_samples=(current_plan.size()-current_index)/current_config_.min_segment_points;
- if(num_samples==0)
+ try{
+ this->forward=forward;
+ this->current_plan=current_plan;
+ pos_=pos;
+ ackermann_=ackermann;
+ goal_pos_=goal;
+ next_sample_index_=0;
+ min_n1=0.0;
+ min_n2=0.0;
+ min_n3=0.0;
+ max_n3=0.0;
+ min_n4=0.0;
+ max_n4=0.0;
+ samples_.clear();
+ 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)
{
- num_samples=1;
- step=(current_plan.size()-current_index);
+ 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
- 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;
- max_n2=2.0*diff_y;
- for(double current_n1=min_n1;current_n1<=max_n1;current_n1+=(max_n1-min_n1)/(current_config_.n1_samples-1))
- for(double current_n2=min_n2;current_n2<=max_n2;current_n2+=(max_n2-min_n2)/(current_config_.n2_samples-1))
- for(double current_n3=min_n3;current_n3<=max_n3;current_n3+=(max_n3-min_n3)/(current_config_.n3_samples-1))
- 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[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);
- }
+ 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;
+ max_n2=2.0*diff_y;
+ for(double current_n1=min_n1;current_n1<=max_n1;current_n1+=(max_n1-min_n1)/(current_config_.n1_samples-1))
+ for(double current_n2=min_n2;current_n2<=max_n2;current_n2+=(max_n2-min_n2)/(current_config_.n2_samples-1))
+ for(double current_n3=min_n3;current_n3<=max_n3;current_n3+=(max_n3-min_n3)/(current_config_.n3_samples-1))
+ 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[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);
+ }
+ }
+ }catch(std::exception &e){
+ std::cout << e.what() << std::endl;
}
}
@@ -157,109 +161,113 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
std::vector<double> x,y,curvature,heading;
try{
- start.x=pos_(0);
- start.y=pos_(1);
- if(this->forward)
- start.heading=pos_(2);
- else
- start.heading=pos_(2)+3.14159;
- if(fabs(ackermann_(1))<0.001)
- start.curvature=0.0;
- 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;
- end.x=samples_[next_sample_index_].end_point.x;
- end.y=samples_[next_sample_index_].end_point.y;
- if(this->forward)
- end.heading=samples_[next_sample_index_].end_point.heading;
- else
- end.heading=samples_[next_sample_index_].end_point.heading+3.14159;
- end.curvature=0.0;
- spline_.set_end_point(end);
- //std::cout << "end position: " << end.x << "," << end.y << "," << end.heading << "," << end.curvature << std::endl;
- resolution=current_config_.sim_granularity;
- spline_.generate(resolution,samples_[next_sample_index_].params(0),samples_[next_sample_index_].params(1),samples_[next_sample_index_].params(2),samples_[next_sample_index_].params(3));
- spline_.evaluate_all(x,y,curvature,heading);
-// std::cout << "generate: " << samples_[next_sample_index_].params(0) << "," << samples_[next_sample_index_].params(1) << "," << samples_[next_sample_index_].params(2) << "," << samples_[next_sample_index_].params(3) << std::endl;
- // discard trajectories with two much curvature
- k_max=-std::numeric_limits<double>::max();
- for(unsigned int i=0;i<curvature.size();i++)
- if(curvature[i]>k_max)
- k_max=curvature[i];
- max_curvature=tan(std::max(current_config_.max_steer_angle,fabs(current_config_.min_steer_angle)))/fabs(current_config_.axis_distance);
- next_sample_index_++;
- /* fill the ouput trajectory object */
- double k_max_speed,min_max_speed,new_start_vel,new_max_vel;
- double acceleration,length,steer_angle,path_x,path_y;
- CVelTrapezoid vel_profile;
- TPoint point;
-
- if(k_max==0)
- k_max_speed=std::numeric_limits<double>::quiet_NaN();
- else
- k_max_speed=sqrt(current_config_.comfort_lat_acc/fabs(k_max));
- min_max_speed=std::min(current_config_.max_trans_vel,k_max_speed);
- if(vel_profile.generate(fabs(ackermann_(0)),0.0,min_max_speed,current_config_.max_trans_acc,spline_.get_length(),new_start_vel,new_max_vel))
- {
- try{
- vel_profile.evaluate_at_time(current_config_.temp_horizon,comp_traj.xv_,acceleration,length);
- }catch(CException &e){
- length=0.0;
- comp_traj.xv_=vel_profile.get_max_vel();
- }
- if(!this->forward)
- comp_traj.xv_*=-1.0;
- comp_traj.yv_=0.0;
- point=spline_.evaluate(length);
- comp_traj.thetav_=-(point.heading-start.heading);
- if(comp_traj.thetav_>3.14159)
- comp_traj.thetav_-=2*3.14159;
- else if(comp_traj.thetav_<-3.14159)
- comp_traj.thetav_+=2*3.14159;
- this->get_path_closest_pose(path_x,path_y);
- double y_offset=(-(path_x-pos_(0))*sin(pos_(2))+(path_y-pos_(1))*cos(pos_(2)));
- if(forward)
- comp_traj.thetav_-=atan2(current_config_.lateral_offset_gain*y_offset,comp_traj.xv_);
+ start.x=pos_(0);
+ start.y=pos_(1);
+ if(this->forward)
+ start.heading=pos_(2);
+ else
+ start.heading=pos_(2)+3.14159;
+ if(fabs(ackermann_(1))<0.001)
+ start.curvature=0.0;
else
- comp_traj.thetav_+=atan2(current_config_.lateral_offset_gain*y_offset,-comp_traj.xv_);
- if(forward)
- comp_traj.thetav_*=-1.0;
- if(comp_traj.thetav_>current_config_.max_steer_angle)
- comp_traj.thetav_=current_config_.max_steer_angle;
- else if(comp_traj.thetav_<current_config_.min_steer_angle)
- comp_traj.thetav_=current_config_.min_steer_angle;
- /*
- point=spline_.evaluate(length);
- if(fabs(point.curvature)<0.001)
- steer_angle=0.0;
+ 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;
+ end.x=samples_[next_sample_index_].end_point.x;
+ end.y=samples_[next_sample_index_].end_point.y;
+ if(this->forward)
+ end.heading=samples_[next_sample_index_].end_point.heading;
else
- steer_angle=atan2(current_config_.axis_distance,1.0/point.curvature);
- if(steer_angle>1.5707)
- steer_angle-=3.14159;
- if(current_config_.use_steer_angle_cmd)
+ end.heading=samples_[next_sample_index_].end_point.heading+3.14159;
+ end.curvature=0.0;
+ spline_.set_end_point(end);
+ //std::cout << "end position: " << end.x << "," << end.y << "," << end.heading << "," << end.curvature << std::endl;
+ resolution=current_config_.sim_granularity;
+ spline_.generate(resolution,samples_[next_sample_index_].params(0),samples_[next_sample_index_].params(1),samples_[next_sample_index_].params(2),samples_[next_sample_index_].params(3));
+ spline_.evaluate_all(x,y,curvature,heading);
+ // std::cout << "generate: " << samples_[next_sample_index_].params(0) << "," << samples_[next_sample_index_].params(1) << "," << samples_[next_sample_index_].params(2) << "," << samples_[next_sample_index_].params(3) << std::endl;
+ // discard trajectories with two much curvature
+ k_max=0.0;
+ for(unsigned int i=0;i<curvature.size();i++)
+ if(fabs(curvature[i])>k_max)
+ k_max=fabs(curvature[i]);
+ max_curvature=tan(std::max(current_config_.max_steer_angle,fabs(current_config_.min_steer_angle)))/fabs(current_config_.axis_distance);
+ next_sample_index_++;
+ /* fill the ouput trajectory object */
+ double k_max_speed,min_max_speed,new_start_vel,new_max_vel;
+ double acceleration,length,steer_angle,path_x,path_y;
+ CVelTrapezoid vel_profile;
+ TPoint point;
+
+ if(k_max==0)
+ k_max_speed=std::numeric_limits<double>::max();
+ else
+ k_max_speed=sqrt(current_config_.comfort_lat_acc/fabs(k_max));
+ min_max_speed=std::min(current_config_.max_trans_vel,k_max_speed);
+ if(vel_profile.generate(fabs(ackermann_(0)),0.0,min_max_speed,current_config_.max_trans_acc,spline_.get_length(),new_start_vel,new_max_vel))
{
+ try{
+ vel_profile.evaluate_at_time(current_config_.temp_horizon,comp_traj.xv_,acceleration,length);
+ }catch(CException &e){
+ length=0.0;
+ comp_traj.xv_=vel_profile.get_max_vel();
+ }
+ if(!this->forward)
+ comp_traj.xv_*=-1.0;
+ comp_traj.yv_=0.0;
+ point=spline_.evaluate(length);
+ comp_traj.thetav_=-(point.heading-start.heading);
+ if(comp_traj.thetav_>3.14159)
+ comp_traj.thetav_-=2*3.14159;
+ else if(comp_traj.thetav_<-3.14159)
+ comp_traj.thetav_+=2*3.14159;
+ this->get_path_closest_pose(path_x,path_y);
+ double y_offset=(-(path_x-pos_(0))*sin(pos_(2))+(path_y-pos_(1))*cos(pos_(2)));
if(forward)
- comp_traj.thetav_ = steer_angle;
+ comp_traj.thetav_-=atan2(current_config_.lateral_offset_gain*y_offset,comp_traj.xv_);
else
- comp_traj.thetav_ = -steer_angle;
+ comp_traj.thetav_+=atan2(current_config_.lateral_offset_gain*y_offset,-comp_traj.xv_);
+ if(forward)
+ comp_traj.thetav_*=-1.0;
if(comp_traj.thetav_>current_config_.max_steer_angle)
comp_traj.thetav_=current_config_.max_steer_angle;
else if(comp_traj.thetav_<current_config_.min_steer_angle)
comp_traj.thetav_=current_config_.min_steer_angle;
+ /*
+ point=spline_.evaluate(length);
+ if(fabs(point.curvature)<0.001)
+ steer_angle=0.0;
+ else
+ steer_angle=atan2(current_config_.axis_distance,1.0/point.curvature);
+ if(steer_angle>1.5707)
+ steer_angle-=3.14159;
+ if(current_config_.use_steer_angle_cmd)
+ {
+ if(forward)
+ comp_traj.thetav_ = steer_angle;
+ else
+ comp_traj.thetav_ = -steer_angle;
+ if(comp_traj.thetav_>current_config_.max_steer_angle)
+ comp_traj.thetav_=current_config_.max_steer_angle;
+ else if(comp_traj.thetav_<current_config_.min_steer_angle)
+ comp_traj.thetav_=current_config_.min_steer_angle;
+ }
+ else
+ comp_traj.thetav_ = comp_traj.xv_*point.curvature;
+ */
+ comp_traj.resetPoints();
+ for(unsigned int i=0;i<x.size();i++)
+ comp_traj.addPoint(x[i],y[i],heading[i]);
+ return true;
}
else
- comp_traj.thetav_ = comp_traj.xv_*point.curvature;
- */
- comp_traj.resetPoints();
- for(unsigned int i=0;i<x.size();i++)
- comp_traj.addPoint(x[i],y[i],heading[i]);
- return true;
- }
- else
- return false;
+ return false;
}catch(CException &e){
std::cout << e.what() << std::endl;
+ return false;
+ }catch(std::exception &e){
+ std::cout << e.what() << std::endl;
+ return false;
}
}