Added a new parameter to set the minimum length to take into account a path segment when splitting.

cfg/AckermannLocalPlanner.cfg

@@ -81,6 +81,7 @@ gen.add("max_steer_acc",           double_t,  0,                               "
 gen.add("axis_distance",           double_t,  0,                               "distance between axes",          1.65,     -2.0,    2)
 gen.add("wheel_distance",          double_t,  0,                               "distance between wheels",        1.2,      0,    2)
 gen.add("wheel_radius",            double_t,  0,                               "Wheel diameter",                 0.436,    0,    2)
+gen.add("split_ignore_length",     double_t,  0,                               "Paths segments under this length will be ignored",1.0,0.0,2.0)
 gen.add("cmd_vel_avg",             int_t,     0,                               "Number of cmd_vel to average",   1,        1,    20)
 gen.add("odom_avg",                int_t,     0,                               "Number of odom to average",      1,        1,    20)
 

include/ackermann_planner_limits.h

@@ -56,6 +56,7 @@ class AckermannPlannerLimits
     double wheel_distance;
     double wheel_radius; 
     // general planner limits
+    double split_ignore_length;
     bool prune_plan;
     double xy_goal_tolerance;
     double yaw_goal_tolerance;
@@ -81,6 +82,7 @@ class AckermannPlannerLimits
       double wheel_distance,
       double wheel_radius, 
       // general planner limits
+      double split_ignore_length,
       double xy_goal_tolerance,
       double yaw_goal_tolerance,
       bool prune_plan=true,
@@ -97,6 +99,7 @@ class AckermannPlannerLimits
       axis_distance(axis_distance),
       wheel_distance(wheel_distance),
       wheel_radius(wheel_radius),
+      split_ignore_length(split_ignore_length),
       prune_plan(prune_plan),
       xy_goal_tolerance(xy_goal_tolerance),
       yaw_goal_tolerance(yaw_goal_tolerance),

src/ackermann_planner_ros.cpp

@@ -87,6 +87,7 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke
   limits.wheel_distance = config.wheel_distance;
   limits.wheel_radius = config.wheel_radius;
   // general planner limits
+  limits.split_ignore_length = config.split_ignore_length;
   limits.xy_goal_tolerance = config.xy_goal_tolerance;
   limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
   limits.prune_plan = config.prune_plan;

src/ackermann_planner_util.cpp

@@ -137,7 +137,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
         angle=std::acos(angle);
         if(fabs(angle)>1.57)
         {
-          if(pathLength>1.0)
+          if(pathLength>limits_.split_ignore_length)
           {
             ROS_INFO("TrajectoryPlannerROS::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
             subPathList.push_back(subPath);