HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
navigations/teb_local_planner/test/teb_basics.cpp Executable file
View File

@@ -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();
}