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)