DuongTD/move_base2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

optimal & fix file cmake

Browse Source
This commit is contained in:
duongtd
2026-08-03 22:41:32 +07:00
parent d8babff20b
commit 701d25f952
70 changed files with 5572 additions and 1146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
test/navigation_server_test.cpp
View File

@@ -74,8 +74,6 @@ ControlLoopConfig baseConfig()
config.position.global_planner_name = "FakeGlobalPlanner";
config.position.local_planner_name = "FakeLocalPlanner";
config.position.default_xy_tolerance = 0.15; // [m]
config.position.default_yaw_tolerance = 0.10; // [rad]
config.docking = config.position;
config.go_straight = config.position;
@@ -217,7 +215,8 @@ TEST(NavigationServerTwist, ReturnsArbiterCommandNotOdometryVelocity)
ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist trả vận tốc đo được thay vì lệnh đã phát";
EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist returned the measured velocity instead of "
"the command that was published";
EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
}
@@ -266,7 +265,8 @@ TEST(NavigationServerTwist, StampFreezesWhenIdleSoTeleopOwnsCmdVel)
fixture.spin(3);
EXPECT_TRUE(fixture.server_.getTwist().header.stamp.isZero())
<< "chưa từng có yêu cầu mà stamp đã tươi — host sẽ phát 0 đè teleop";
<< "no request has ever arrived yet the stamp is fresh — the host would publish 0 over "
"teleop";
}
TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
@@ -288,14 +288,16 @@ TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
const double stamp_in_grace = fixture.server_.getTwist().header.stamp.toSec();
fixture.spin(1);
EXPECT_GT(fixture.server_.getTwist().header.stamp.toSec(), stamp_in_grace)
<< "stamp đóng băng ngay khi kết thúc — lệnh dừng cuối không bao giờ được publish";
<< "the stamp freezes as soon as the leg ends — the final stop command would never be "
"published";
// Chạy qua hết cửa ân hạn (0.5 s = 10 cycle) rồi thêm vài cycle: stamp phải đứng yên.
fixture.spin(12);
const double stamp_frozen = fixture.server_.getTwist().header.stamp.toSec();
fixture.spin(3);
EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_frozen, 1e-9)
<< "hết ân hạn mà stamp vẫn tươi — teleop không bao giờ lấy lại được /cmd_vel";
<< "the grace period is over yet the stamp is still fresh — teleop would never get /cmd_vel "
"back";
}
TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
@@ -311,7 +313,8 @@ TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.0));
EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_after_first, 1e-9)
<< "dấu thời gian tự tươi lại dù control loop không chạy — host sẽ tưởng lệnh còn hiệu lực";
<< "the timestamp refreshes itself although the control loop is not running — the host would "
"think the command is still valid";
}
TEST(NavigationServerTwist, IsStampedWithTheConfiguredRobotBaseFrame)
@@ -355,7 +358,7 @@ TEST(NavigationServerSensors, SamplesReachTheCostmapLayersOnceAttached)
fixture.server_.addPointCloud2("/camera/depth/points_proc", robot_sensor_msgs::PointCloud2());
EXPECT_EQ(static_layer->count(), 1U);
EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 phải cùng tới VoxelLayer";
EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 must both reach the VoxelLayer";
EXPECT_EQ(local_voxel->records()[0].topic, "/b_scan");
EXPECT_EQ(local_voxel->records()[1].topic, "/camera/depth/points_proc");
}
@@ -388,7 +391,8 @@ TEST(NavigationServerSensors, StaticMapReceivedBeforeAttachIsReplayed)
SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
fixture.attachCostmaps();
ASSERT_EQ(static_layer->count(), 1U) << "static map nhận trước khi gắn costmap không được phát lại";
ASSERT_EQ(static_layer->count(), 1U) << "a static map received before a costmap was attached "
"must not be replayed";
EXPECT_EQ(static_layer->records()[0].topic, "/map");
}
@@ -421,7 +425,7 @@ TEST(NavigationServerSensors, ReplayDoesNotDuplicateAMapAlreadyReceivedThroughTh
SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
fixture.attachCostmaps();
EXPECT_EQ(static_layer->count(), 1U) << "cùng một map bị phát lại hai lần";
EXPECT_EQ(static_layer->count(), 1U) << "the same map was replayed twice";
}
TEST(NavigationServerSensors, StaleLaserScansAreNotReplayedOnAttach)
@@ -475,11 +479,11 @@ TEST(NavigationServerSensors, StoredLaserScanIsTheSameOneHandedToTheCostmap)
{
if (std::isnan(stored[i]))
{
EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "lệch tại tia " << i;
EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "mismatch at ray " << i;
}
else
{
EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "lệch tại tia " << i;
EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "mismatch at ray " << i;
}
}
}
@@ -625,7 +629,7 @@ TEST(NavigationServerLifecycle, PauseTakesEffectOnTheNextCycleNotImmediately)
fixture.server_.pause();
EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling)
<< "pause() đi thẳng vào lõi từ thread host";
<< "pause() goes straight into the core from the host thread";
fixture.spin(1);
EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
@@ -686,7 +690,7 @@ TEST(NavigationServerLifecycle, CancelWinsOverAPauseRequestedInTheSameCycle)
fixture.server_.cancel();
fixture.spin(1);
EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause thắng cancel";
EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause won over cancel";
}
TEST(NavigationServerLifecycle, LifecycleRequestIsConsumedExactlyOnce)
@@ -738,7 +742,8 @@ TEST(NavigationServerControlThread, RunsCyclesWithoutAnyoneCallingSpinOnce)
}
fixture.server_.stopControlThread();
EXPECT_TRUE(left_idle) << "goal được nhận nhưng không cycle nào chạy — thiếu control thread";
EXPECT_TRUE(left_idle) << "the goal was accepted but no cycle ran — the control thread is "
"missing";
}
TEST(NavigationServerControlThread, RefusesToStartBeforeTheLoopIsConfigured)
@@ -763,7 +768,7 @@ TEST(NavigationServerControlThread, SecondStartIsRefusedAndStopIsIdempotent)
fixture.configure();
ASSERT_TRUE(fixture.server_.startControlThread(100.0));
EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "khởi động thread thứ hai";
EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "started a second thread";
fixture.server_.stopControlThread();
fixture.server_.stopControlThread(); // không được treo hay sập
@@ -797,7 +802,7 @@ TEST(NavigationServerPlannerData, GettersDoNotShareMutableState)
a.plan.poses.clear();
EXPECT_TRUE(b.plan.poses.empty() || !a.plan.poses.empty())
<< "hai lần gọi trả về cùng một vùng nhớ";
<< "two calls returned the same memory";
EXPECT_NO_THROW({ (void)fixture.server_.getLocalData(); });
}
@@ -851,7 +856,8 @@ TEST(NavigationServerPlannerData, PlanIsStampedWithTheControlLoopClock)
fixture.spin(1);
EXPECT_NEAR(fixture.server_.getGlobalData().plan.header.stamp.toSec(), kClockStart + 7.0, 1e-9)
<< "plan mang dấu thời gian khác đồng hồ control loop — host sẽ coi là quá hạn và bỏ qua";
<< "the plan carries a timestamp from another clock than the control loop — the host would "
"treat it as stale and drop it";
}
int main(int argc, char** argv)