DuongTD/recovery_core
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:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

216
test/rotate_safety_test.cpp Normal file
View File

@@ -0,0 +1,216 @@
/*********************************************************************
*
* Kiểm quét cung và đo góc bằng pose thật của RotateRecovery.
*
* Bản trước không dùng `ctx()` một lần nào trong toàn file: quay mù, và đếm góc bằng
* `angular_speed * control_period`.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kTwoPi = 2.0 * M_PI;
struct RotateFixture
{
RotateFixture()
{
rig.pose.setPose(0.0, 0.0, 0.0);
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
rotate = recovery_test::findBehavior(registry, "rotate");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* rotate = nullptr;
};
TEST(RotateSafety, RefusesToStartWhenArcIsBlocked)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
// Footprint 0.6 x 0.4 quanh gốc: khi quay 90 độ, mũi robot quét tới y ~ +/-0.3.
// Đặt vật cản ở đó -> cung quay bị chặn dù vị trí hiện tại vẫn trống.
fixture.rig.costmap.setLethalCircle(0.0, 0.32, 0.06);
RecoveryGoal goal;
goal.angle = kTwoPi;
EXPECT_FALSE(fixture.rotate->start(goal, robot::Time(1000.0)));
}
TEST(RotateSafety, StartsWhenArcIsClear)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = kTwoPi;
EXPECT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
}
TEST(RotateSafety, PartialArcAvoidsBlockedSector)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
// Vật cản chỉ chặn khi robot đã quay đáng kể; cung nhỏ vẫn phải đi được.
fixture.rig.costmap.setLethalCircle(0.0, 0.32, 0.06);
RecoveryGoal small_arc;
small_arc.angle = 0.05;
EXPECT_TRUE(fixture.rotate->start(small_arc, robot::Time(1000.0)));
}
TEST(RotateSafety, StopsWithZeroCommandWhenPoseIsLost)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
ASSERT_TRUE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.rotate->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
fixture.rig.pose.setAvailable(false);
const auto result = fixture.rotate->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
TEST(RotateSafety, RefusesToStartWhenPoseIsUnavailable)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
fixture.rig.pose.setAvailable(false);
EXPECT_FALSE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
}
TEST(RotateSafety, FullRotationCountsPastPiCorrectly)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = kTwoPi;
ASSERT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
// Cộng dồn góc quay THẬT do test tự đo, độc lập với con số plugin báo.
double swept = 0.0;
double previous_yaw = fixture.rig.pose.rawPose().theta;
robot::Time now(1000.0);
bool finished = false;
for (int i = 0; i < 2000; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.rotate->update(now);
if (result.terminal())
{
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
finished = true;
break;
}
fixture.rig.applyCommand(result.command, 0.1);
const double yaw = fixture.rig.pose.rawPose().theta;
swept += std::abs(recovery_core::normalizeAngle(yaw - previous_yaw));
previous_yaw = yaw;
}
ASSERT_TRUE(finished);
// Quay đủ vòng: phép chuẩn hoá từng bước phải đếm đúng qua mốc pi, không bị wrap về 0.
EXPECT_NEAR(swept, kTwoPi, 0.05 * kTwoPi);
}
TEST(RotateSafety, SlowLoopDoesNotOvershoot)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = 1.0;
ASSERT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
double swept = 0.0;
double previous_yaw = fixture.rig.pose.rawPose().theta;
// dt gấp 5 lần nhịp thường.
robot::Time now(1000.0);
for (int i = 0; i < 200; ++i)
{
now = robot::Time(now.toSec() + 0.5);
const auto result = fixture.rotate->update(now);
if (result.terminal())
{
break;
}
fixture.rig.applyCommand(result.command, 0.5);
const double yaw = fixture.rig.pose.rawPose().theta;
swept += std::abs(recovery_core::normalizeAngle(yaw - previous_yaw));
previous_yaw = yaw;
}
EXPECT_NEAR(swept, 1.0, 0.05);
}
TEST(RotateSafety, CancelEmitsZeroCommand)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
ASSERT_TRUE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.rotate->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
fixture.rotate->cancel();
const auto result = fixture.rotate->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}