diff --git a/src/ackermann_trajectory_search.cpp b/src/ackermann_trajectory_search.cpp index 498989342ad31aad3ffb71a348b7025e8f6fc431..ceb10bfcc010babf594736516ebe0d08d0a6bed0 100644 --- a/src/ackermann_trajectory_search.cpp +++ b/src/ackermann_trajectory_search.cpp @@ -125,7 +125,10 @@ bool AckermannTrajectorySearch::findBestTrajectory(base_local_planner::Trajector } count++; if (max_samples_ > 0 && count >= max_samples_) + { + std::cout << "Max sample limit" << std::endl; break; + } } if (best_traj_cost >= 0) { diff --git a/src/heading_cost_function.cpp b/src/heading_cost_function.cpp index ad19be0c03c8f0e24d81ae61dfecd5ae5809b2a1..d7b1846cb51f9670db0e85d2ecdc7092dbb25b01 100644 --- a/src/heading_cost_function.cpp +++ b/src/heading_cost_function.cpp @@ -53,10 +53,20 @@ bool HeadingCostFunction::prepare() double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj) { double dist,x,y,theta,near_dist=DBL_MAX,heading_diff=0.0,diff=0.0; - int interval=traj.getPointsSize()/this->num_points; + int interval,samples; unsigned int near_index=0,i,j; - for(j=0;j<this->num_points;j++) + if(this->num_points>traj.getPointsSize()) + { + interval=1; + samples=traj.getPointsSize(); + } + else + { + interval=traj.getPointsSize()/this->num_points; + samples=this->num_points; + } + for(j=0;j<samples;j++) { traj.getPoint((j+1)*interval-1,x,y,theta); // find the nearrest point on the path @@ -66,8 +76,8 @@ double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj (this->global_plan[i].pose.position.y-y)*(this->global_plan[i].pose.position.y-y)); if(dist<near_dist) { - near_dist=dist; - near_index=i; + near_dist=dist; + near_index=i; } } double v1_x,v1_y;