Fixed TEB autoResize if last TimeDiff is small

* fix the removal of a small last TimeDiff
* add a small set of tests for autoResize

CMakeLists.txt

@@ -267,6 +267,13 @@ install(DIRECTORY
 ## Testing ##
 #############
 
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(test_teb_basics test/teb_basics.cpp)
+  if(TARGET test_teb_basics)
+     target_link_libraries(test_teb_basics teb_local_planner)
+  endif()
+endif()
+
 ## Add gtest based cpp test target and link libraries
 # catkin_add_gtest(${PROJECT_NAME}-test test/test_teb_local_planner.cpp)
 # if(TARGET ${PROJECT_NAME}-test)

src/timed_elastic_band.cpp

@@ -225,8 +225,8 @@ void TimedElasticBand::setTimeDiffVertexFixed(int index, bool status)
 
 void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, int min_samples, int max_samples, bool fast_mode)
 {  
+  ROS_ASSERT(sizeTimeDiffs() == 0 || sizeTimeDiffs() + 1 == sizePoses());
   /// iterate through all TEB states and add/remove states!
-
   bool modified = true;
 
   for (int rep = 0; rep < 100 && modified; ++rep) // actually it should be while(), but we want to make sure to not get stuck in some oscillation, hence max 100 repitions.
@@ -257,6 +257,12 @@ void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, int min_s
           deleteTimeDiff(i);
           deletePose(i+1);
         }
+        else
+        { // last motion should be adjusted, shift time to the interval before
+          TimeDiff(i-1) += TimeDiff(i);
+          deleteTimeDiff(i);
+          deletePose(i);
+        }
 
         modified = true;
       }

test/teb_basics.cpp

@@ -0,0 +1,73 @@
+#include <gtest/gtest.h>
+
+#include <teb_local_planner/timed_elastic_band.h>
+
+TEST(TEBBasic, autoResizeLargeValueAtEnd)
+{
+  double dt = 0.1;
+  double dt_hysteresis = dt/3.;
+  teb_local_planner::TimedElasticBand teb;
+
+  teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
+  for (int i = 1; i < 10; ++i) {
+    teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
+  }
+  // add a pose with a large timediff as the last one
+  teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt + 2*dt_hysteresis);
+
+  // auto resize + test of the result
+  teb.autoResize(dt, dt_hysteresis, 3, 100, false);
+  for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
+    ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
+    ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
+  }
+}
+
+TEST(TEBBasic, autoResizeSmallValueAtEnd)
+{
+  double dt = 0.1;
+  double dt_hysteresis = dt/3.;
+  teb_local_planner::TimedElasticBand teb;
+
+  teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
+  for (int i = 1; i < 10; ++i) {
+    teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
+  }
+  // add a pose with a small timediff as the last one
+  teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis);
+
+  // auto resize + test of the result
+  teb.autoResize(dt, dt_hysteresis, 3, 100, false);
+  for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
+    ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
+    ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
+  }
+}
+
+TEST(TEBBasic, autoResize)
+{
+  double dt = 0.1;
+  double dt_hysteresis = dt/3.;
+  teb_local_planner::TimedElasticBand teb;
+
+  teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
+  for (int i = 1; i < 10; ++i) {
+    teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
+  }
+  // modify the timediff in the middle and add a pose with a smaller timediff as the last one
+  teb.TimeDiff(5) = dt + 2*dt_hysteresis;
+  teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis);
+
+  // auto resize
+  teb.autoResize(dt, dt_hysteresis, 3, 100, false);
+  for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
+    ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
+    ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
+  }
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}