From 0d7b9fa2d0caf7c9dc1c88db7d8a9a1ce4984c06 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 5 Nov 2020 11:16:16 +0100
Subject: [PATCH] Added a new parameter to set the minimum length to take into
account a path segment when splitting.
---
cfg/AckermannLocalPlanner.cfg | 1 +
include/ackermann_planner_limits.h | 3 +++
src/ackermann_planner_ros.cpp | 1 +
src/ackermann_planner_util.cpp | 2 +-
4 files changed, 6 insertions(+), 1 deletion(-)
diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg
index ba17b2f..0748ebc 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 21039e7..1c3f9ff 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 18a29fe..ca3bc68 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 e162d43..80f1159 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);
--
GitLab