diff --git a/include/ackermann_spline_generator.h b/include/ackermann_spline_generator.h
index 46d1c3e744136e47fd7aafe1de70115a4f1ffaf2..46763e50114c4daad293229475b6157684ef2630 100644
--- a/include/ackermann_spline_generator.h
+++ b/include/ackermann_spline_generator.h
@@ -39,6 +39,7 @@
#define ACKERMANN_SPLINE_GENERATOR_H_
#include <base_local_planner/trajectory_sample_generator.h>
+#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Core>
#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
@@ -69,7 +70,7 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
*/
bool nextTrajectory(base_local_planner::Trajectory &traj);
- void initialise(const Eigen::Vector3f& pos,const Eigen::Vector3f& ackermann,const Eigen::Vector3f& goal,bool forward);
+ void initialise(const Eigen::Vector3f& pos,const Eigen::Vector3f& ackermann,const Eigen::Vector3f& goal,bool forward,std::vector<geometry_msgs::PoseStamped>& current_plan);
void set_sim_period(double period);
@@ -82,9 +83,11 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
Eigen::Vector3f pos_;
Eigen::Vector3f goal_pos_;
Eigen::Vector3f ackermann_;
+ std::vector<geometry_msgs::PoseStamped> current_plan;
bool forward;
-
double sim_period_;
+
+ void get_path_closest_pose(double &path_x,double &path_y);
};
#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */
diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index 42cad18af3e37aa2ca0217c816f761cd2d804e69..5562dd92edaba78b38f7aea2075d249d58abe382 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -228,7 +228,7 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
// prepare cost functions and generators for this run
generator_.initialise(pos,ack_state,goal);
- spline_generator_.initialise(pos,ack_state,goal,plan_forward);
+ spline_generator_.initialise(pos,ack_state,goal,plan_forward,global_plan_);
result_traj_.cost_ = -7;
// find best trajectory by sampling and scoring the samples
diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
index 674467d8a4d4e40a53f9c62cd08c8efa6c8849bc..1e15304cdc285b2bb0dfd06e74a97d4196cf28d5 100644
--- a/src/ackermann_spline_generator.cpp
+++ b/src/ackermann_spline_generator.cpp
@@ -50,11 +50,31 @@ AckermannSplineGenerator::AckermannSplineGenerator()
{
}
+void 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;
+
+ /* find the path point closest to the robot */
+ for(unsigned int i=0;i<this->current_plan.size();i++)
+ {
+ dist=sqrt(pow(pos_(0)-this->current_plan[i].pose.position.x,2.0)+pow(pos_(1)-this->current_plan[i].pose.position.x,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ min_index=i;
+ }
+ }
+ path_x=this->current_plan[min_index].pose.position.x;
+ path_y=this->current_plan[min_index].pose.position.y;
+}
+
void AckermannSplineGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& ackermann,//[0] -> trans_vel, [1]-> steer_angle, [2]-> steer_vel
const Eigen::Vector3f& goal,
- bool forward)
+ bool forward,
+ std::vector<geometry_msgs::PoseStamped>& current_plan)
{
double diff_x,diff_y;
double min_n1,max_n1,min_n2,max_n2,min_n3,max_n3,min_n4,max_n4;
@@ -81,6 +101,7 @@ void AckermannSplineGenerator::initialise(
for(double current_n4=min_n4;current_n4<=max_n4;current_n4+=(max_n4-min_n4)/(current_config_.n4_samples-1))
samples_.push_back(Eigen::Vector4f(current_n1,current_n2,current_n3,current_n4));
this->forward=forward;
+ this->current_plan=current_plan;
}
void AckermannSplineGenerator::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg)
@@ -137,80 +158,72 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
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_++;
-// if(k_max>max_curvature)
-// {
-// std::cout << "Too much curvature: " << k_max << "(" << max_curvature << ")" << std::endl;
-// return false;
-// }
-// else
-// {
- /* fill the ouput trajectory object */
- double k_max_speed,min_max_speed,new_start_vel,new_max_vel;
- double acceleration,length,steer_angle;
- 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);
+ /* 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{
- 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))
- {
- vel_profile.evaluate_at_time(sim_period_,comp_traj.xv_,acceleration,length);
- 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;
- 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;
-
- //double y_offset=(-(transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*sin(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*cos(yaw));
- //comp_traj.thetav_-=atan2(0.01*y_offset,comp_traj.xv_);
- /*
- 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
- return false;
+ vel_profile.evaluate_at_time(sim_period_,comp_traj.xv_,acceleration,length);
}catch(CException &e){
- //ROS_INFO_STREAM(e.what());
- return false;
+ comp_traj.xv_=vel_profile.get_max_vel();
}
-// }
+ std::cout << "x velocity: " << comp_traj.xv_ << std::endl;
+ if(!this->forward)
+ comp_traj.xv_*=-1.0;
+ comp_traj.yv_=0.0;
+ point=spline_.evaluate(0.0);
+ 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;
+ 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;
+ 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)));
+ comp_traj.thetav_-=atan2(0.01*y_offset,comp_traj.xv_);
+ /*
+ 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
+ return false;
}
void AckermannSplineGenerator::set_sim_period(double period)