diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg index ba17b2fd9f6d49b510b49bae4dfe3bf6bd4c9824..0748ebc70179f2978069afcc7cb16f95a354b565 100755 --- a/cfg/AckermannLocalPlanner.cfg +++ b/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) diff --git a/include/ackermann_planner_limits.h b/include/ackermann_planner_limits.h index 21039e76e7867143a814b31265a2ce0385cff1fb..1c3f9ff0518df893856bca5cb670fe7e4f784343 100644 --- a/include/ackermann_planner_limits.h +++ b/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), diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp index 18a29fed0162e5d124114872b71de0db07e17142..ca3bc68cdfb09fc5f6956a4026702d92e46e97ae 100644 --- a/src/ackermann_planner_ros.cpp +++ b/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; diff --git a/src/ackermann_planner_util.cpp b/src/ackermann_planner_util.cpp index e162d43fd3e4487b837b1ecec5f54e3842da45ab..80f11597c47110bbd11fbd1e7f48c25076771202 100644 --- a/src/ackermann_planner_util.cpp +++ b/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);