diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index 06351f95b2cd8ff7f43a0cde703ee8b96cd87df4..75bde7fe6501d9d9128ae4ddf4a890f2dbf473a1 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -458,8 +458,8 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
double diff = fabs(ackermann.steering_angle-ang_z);
if(diff > current_config_.stopped_steering_angle_threshold)
{
- //ROS_INFO("AckermannPlannerROS: steering without moving")
- //ROS_INFO("AckermannPlannerROS: stopped steering. Angle current - target = %f - %f = %f, vs threshold %f )",ackermann.steering_angle, ang_z, diff, current_config_.stopped_steering_angle_threshold);
+ ROS_INFO("AckermannPlannerROS: steering without moving");
+ ROS_INFO("AckermannPlannerROS: stopped steering. Angle current - target = %f - %f = %f, vs threshold %f )",ackermann.steering_angle, ang_z, diff, current_config_.stopped_steering_angle_threshold);
cmd_vel.linear.x=0.0;
}
else
diff --git a/src/ackermann_planner_util.cpp b/src/ackermann_planner_util.cpp
index b2943b4d6f95bf13cda18525d20f2ca0ce12aa00..e916a49575c008ab624be8bdc3f8ff1b311bfacb 100644
--- a/src/ackermann_planner_util.cpp
+++ b/src/ackermann_planner_util.cpp
@@ -103,79 +103,84 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
ROS_ERROR("AckermannPlannerUtil: planner utils have not been initialized, please call initialize() first");
return false;
}
- if(orig_global_plan.size()<5)
- {
- ROS_ERROR("AckermannPlannerUtil: A valid path at least needs 5 points");
- return false;
- }
- ////divide plan by manuveurs
subPathList.clear();
subPath.clear();
forward_segment.clear();
subPathIndex = 0;
double pathLength = 0, yaw;
for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
+ std::cout << orig_global_plan[i].pose.position.x << "," << orig_global_plan[i].pose.position.y << "," << tf::getYaw(orig_global_plan[i].pose.orientation) << std::endl;
+ if(orig_global_plan.size()<5)
{
- if(i>2 && i<(orig_global_plan.size()-1))
+ for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
+ subPath.push_back(orig_global_plan[i]);
+ }
+ else
+ {
+ ////divide plan by manuveurs
+ for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
{
- double x_1 = orig_global_plan[i+1].pose.position.x;
- double x0 = orig_global_plan[i ].pose.position.x;
- double x1 = orig_global_plan[i-1].pose.position.x;
- double x2 = orig_global_plan[i-2].pose.position.x;
- double x3 = orig_global_plan[i-3].pose.position.x;
- double y0 = orig_global_plan[i ].pose.position.y;
- double y_1 = orig_global_plan[i+1].pose.position.y;
- double y1 = orig_global_plan[i-1].pose.position.y;
- double y2 = orig_global_plan[i-2].pose.position.y;
- double y3 = orig_global_plan[i-3].pose.position.y;
- double angle=((x0-x1)*(x1-x2)+(y0-y1)*(y1-y2))/(std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2))*std::sqrt(std::pow(x1-x2,2)+std::pow(y1-y2,2)));
- double curvature;
- curvature=((x_1-x0)*(y0-2*y1+y2)-(x0-2*x1+x2)*(y_1-y0))/sqrt(pow(pow(x_1-x0,2.0)+pow(y_1-y0,2.0),3.0));
-// std::cout << curvature << std::endl;
- if(angle<-1.0)
- angle=-1.0;
- else if(angle>1.0)
- angle=1.0;
- angle=std::acos(angle);
- pathLength+= std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2));
- if(fabs(angle)>1.57) //if changes of direction detected
+ if(i>2 && i<(orig_global_plan.size()-1))
{
- angle=((x_1-x0)*(x2-x3)+(y_1-y0)*(y2-y3))/(std::sqrt(std::pow(x_1-x0,2)+std::pow(y_1-y0,2))*std::sqrt(std::pow(x2-x3,2)+std::pow(y2-y3,2)));
+ double x_1 = orig_global_plan[i+1].pose.position.x;
+ double x0 = orig_global_plan[i ].pose.position.x;
+ double x1 = orig_global_plan[i-1].pose.position.x;
+ double x2 = orig_global_plan[i-2].pose.position.x;
+ double x3 = orig_global_plan[i-3].pose.position.x;
+ double y0 = orig_global_plan[i ].pose.position.y;
+ double y_1 = orig_global_plan[i+1].pose.position.y;
+ double y1 = orig_global_plan[i-1].pose.position.y;
+ double y2 = orig_global_plan[i-2].pose.position.y;
+ double y3 = orig_global_plan[i-3].pose.position.y;
+ double angle=((x0-x1)*(x1-x2)+(y0-y1)*(y1-y2))/(std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2))*std::sqrt(std::pow(x1-x2,2)+std::pow(y1-y2,2)));
+ double curvature;
+ curvature=((x_1-x0)*(y0-2*y1+y2)-(x0-2*x1+x2)*(y_1-y0))/sqrt(pow(pow(x_1-x0,2.0)+pow(y_1-y0,2.0),3.0));
+// std::cout << curvature << std::endl;
if(angle<-1.0)
angle=-1.0;
else if(angle>1.0)
angle=1.0;
angle=std::acos(angle);
- if(fabs(angle)>1.57)
+ pathLength+= std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2));
+ if(fabs(angle)>1.57) //if changes of direction detected
{
- if(pathLength>current_config_.split_ignore_length)
+ angle=((x_1-x0)*(x2-x3)+(y_1-y0)*(y2-y3))/(std::sqrt(std::pow(x_1-x0,2)+std::pow(y_1-y0,2))*std::sqrt(std::pow(x2-x3,2)+std::pow(y2-y3,2)));
+ if(angle<-1.0)
+ angle=-1.0;
+ else if(angle>1.0)
+ angle=1.0;
+ angle=std::acos(angle);
+ if(fabs(angle)>1.57)
{
- //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
- yaw=tf::getYaw(subPath[0].pose.orientation);
- if(yaw<0.0)
- yaw+=3.14159*2.0;
- if(((subPath[subPath.size()-1].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[subPath.size()-1].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
+ if(pathLength>current_config_.split_ignore_length)
{
- std::cout << "backward segment" << std::endl;
- this->forward_segment.push_back(false);
+ //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
+ yaw=tf::getYaw(subPath[0].pose.orientation);
+ if(yaw<0.0)
+ yaw+=3.14159*2.0;
+ if(((subPath[subPath.size()-1].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[subPath.size()-1].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
+ {
+ std::cout << "backward segment" << std::endl;
+ this->forward_segment.push_back(false);
+ }
+ else
+ {
+ std::cout << "forward segment" << std::endl;
+ this->forward_segment.push_back(true);
+ }
+ subPathList.push_back(subPath);
}
- else
+ else //ignored subpath, too short
{
- std::cout << "forward segment" << std::endl;
- this->forward_segment.push_back(true);
+ //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, Ignored by length=%f", i, angle, pathLength);
}
- subPathList.push_back(subPath);
- }
- else //ignored subpath, too short
- {
- //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, Ignored by length=%f", i, angle, pathLength);
+ subPath.clear();
+ pathLength=0.0;
}
- subPath.clear();
- pathLength=0.0;
}
}
+ subPath.push_back(orig_global_plan[i]);
}
- subPath.push_back(orig_global_plan[i]);
}
yaw=tf::getYaw(subPath[0].pose.orientation);
if(yaw<0.0)
diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
index bfbdd87a6cfd7b59f5cd7464190e5ed0e8c210e6..668ba9d43986490bbd8257c2cc1a64d633c1bb6e 100644
--- a/src/ackermann_spline_generator.cpp
+++ b/src/ackermann_spline_generator.cpp
@@ -157,7 +157,7 @@ bool AckermannSplineGenerator::hasMoreTrajectories()
bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &comp_traj)
{
TPoint start,end;
- double resolution,k_max,max_curvature;
+ double resolution,k_max;
std::vector<double> x,y,curvature,heading;
try{
@@ -191,8 +191,9 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
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_++;
+ if(k_max>2.0)
+ return false;
/* 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;