73 lines
2.5 KiB
C++
Executable File
73 lines
2.5 KiB
C++
Executable File
#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();
|
|
} |