optimal & fix file cmake
This commit is contained in:
190
test/output_kind_test.cpp
Normal file
190
test/output_kind_test.cpp
Normal file
@@ -0,0 +1,190 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Kiểm base CƯỠNG CHẾ bất biến họ output.
|
||||
*
|
||||
* Bản trước để plugin tự đặt `output_type` mỗi tick, nên nó không dùng được để route: một plugin họ
|
||||
* path trả `kVelocity` ở tick đầu, còn base thì tự sinh `Velocity(zero)` cho mọi họ ở nhánh
|
||||
* terminal. Test này khoá lại hành vi đúng.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <limits>
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
|
||||
#include "recovery_test_utils.h"
|
||||
|
||||
namespace
|
||||
{
|
||||
using recovery_core::RecoveryGoal;
|
||||
using recovery_core::RecoveryOutputType;
|
||||
using recovery_core::RecoveryStatus;
|
||||
using recovery_test::MockBehavior;
|
||||
using recovery_test::VelocityRig;
|
||||
|
||||
/// Dựng một MockBehavior đã configure + start, sẵn sàng nhận update().
|
||||
struct Fixture
|
||||
{
|
||||
explicit Fixture(RecoveryOutputType kind) : behavior(kind)
|
||||
{
|
||||
ctx = rig.ctx;
|
||||
EXPECT_TRUE(behavior.configure("mock", ctx, nh));
|
||||
EXPECT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
|
||||
}
|
||||
|
||||
VelocityRig rig;
|
||||
robot::NodeHandle nh;
|
||||
recovery_core::RecoveryContext ctx;
|
||||
MockBehavior behavior;
|
||||
};
|
||||
|
||||
TEST(OutputKind, NoneFamilyNeverReportsVelocity)
|
||||
{
|
||||
Fixture fixture(RecoveryOutputType::kNone);
|
||||
|
||||
// Plugin cố tình trả output vận tốc dù nó khai họ kNone.
|
||||
robot_geometry_msgs::Twist rogue;
|
||||
rogue.linear.x = 0.5;
|
||||
fixture.behavior.next_result =
|
||||
recovery_core::RecoveryResult::Velocity(rogue, RecoveryStatus::kRunning);
|
||||
|
||||
const auto result = fixture.behavior.update(robot::Time(1000.1));
|
||||
|
||||
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
|
||||
EXPECT_EQ(result.velocity(), nullptr);
|
||||
// Quan trọng nhất: giá trị rogue không được rò ra ngoài dưới bất kỳ dạng nào.
|
||||
EXPECT_DOUBLE_EQ(result.command.linear.x, 0.0);
|
||||
}
|
||||
|
||||
TEST(OutputKind, PathFamilyNeverReportsVelocity)
|
||||
{
|
||||
Fixture fixture(RecoveryOutputType::kPath);
|
||||
|
||||
robot_geometry_msgs::Twist rogue;
|
||||
rogue.angular.z = 1.0;
|
||||
fixture.behavior.next_result =
|
||||
recovery_core::RecoveryResult::Velocity(rogue, RecoveryStatus::kRunning);
|
||||
|
||||
const auto result = fixture.behavior.update(robot::Time(1000.1));
|
||||
|
||||
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
|
||||
EXPECT_EQ(result.velocity(), nullptr);
|
||||
EXPECT_EQ(result.pathOut(), nullptr);
|
||||
}
|
||||
|
||||
TEST(OutputKind, VelocityFamilyNeverReportsPath)
|
||||
{
|
||||
Fixture fixture(RecoveryOutputType::kVelocity);
|
||||
|
||||
robot_nav_msgs::Path rogue;
|
||||
rogue.poses.resize(2);
|
||||
fixture.behavior.next_result =
|
||||
recovery_core::RecoveryResult::PathOut(rogue, RecoveryStatus::kRunning);
|
||||
|
||||
const auto result = fixture.behavior.update(robot::Time(1000.1));
|
||||
|
||||
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
|
||||
EXPECT_EQ(result.pathOut(), nullptr);
|
||||
EXPECT_TRUE(result.path.poses.empty());
|
||||
}
|
||||
|
||||
TEST(OutputKind, MatchingKindPassesThrough)
|
||||
{
|
||||
Fixture fixture(RecoveryOutputType::kVelocity);
|
||||
|
||||
robot_geometry_msgs::Twist cmd;
|
||||
cmd.linear.x = -0.1;
|
||||
fixture.behavior.next_result =
|
||||
recovery_core::RecoveryResult::Velocity(cmd, RecoveryStatus::kRunning);
|
||||
|
||||
const auto result = fixture.behavior.update(robot::Time(1000.1));
|
||||
|
||||
ASSERT_NE(result.velocity(), nullptr);
|
||||
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, -0.1);
|
||||
}
|
||||
|
||||
TEST(OutputKind, NoneIsAlwaysAllowed)
|
||||
{
|
||||
// Một behavior họ velocity vẫn được phép nói "tick này không có output".
|
||||
Fixture fixture(RecoveryOutputType::kVelocity);
|
||||
fixture.behavior.next_result = recovery_core::RecoveryResult::Running();
|
||||
|
||||
const auto result = fixture.behavior.update(robot::Time(1000.1));
|
||||
|
||||
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
|
||||
EXPECT_EQ(result.status, RecoveryStatus::kRunning);
|
||||
}
|
||||
|
||||
TEST(OutputKind, TerminalStopOutputMatchesFamily)
|
||||
{
|
||||
{
|
||||
Fixture none_family(RecoveryOutputType::kNone);
|
||||
none_family.behavior.next_result = recovery_core::RecoveryResult::Succeeded();
|
||||
ASSERT_EQ(none_family.behavior.update(robot::Time(1000.1)).status, RecoveryStatus::kSucceeded);
|
||||
|
||||
// Tick sau khi đã kết thúc: họ kNone KHÔNG được báo cáo mình phát vận tốc.
|
||||
const auto after = none_family.behavior.update(robot::Time(1000.2));
|
||||
EXPECT_EQ(after.output_type, RecoveryOutputType::kNone);
|
||||
}
|
||||
{
|
||||
Fixture velocity_family(RecoveryOutputType::kVelocity);
|
||||
velocity_family.behavior.next_result = recovery_core::RecoveryResult::Succeeded();
|
||||
ASSERT_EQ(velocity_family.behavior.update(robot::Time(1000.1)).status,
|
||||
RecoveryStatus::kSucceeded);
|
||||
|
||||
// Họ velocity thì ngược lại: phải có lệnh dừng tường minh để caller publish.
|
||||
const auto after = velocity_family.behavior.update(robot::Time(1000.2));
|
||||
ASSERT_NE(after.velocity(), nullptr);
|
||||
EXPECT_DOUBLE_EQ(after.velocity()->linear.x, 0.0);
|
||||
EXPECT_DOUBLE_EQ(after.velocity()->angular.z, 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(OutputKind, NonFiniteVelocityIsForcedToStop)
|
||||
{
|
||||
Fixture fixture(RecoveryOutputType::kVelocity);
|
||||
|
||||
robot_geometry_msgs::Twist bad;
|
||||
bad.linear.x = std::numeric_limits<double>::quiet_NaN();
|
||||
fixture.behavior.next_result =
|
||||
recovery_core::RecoveryResult::Velocity(bad, RecoveryStatus::kRunning);
|
||||
|
||||
const auto result = fixture.behavior.update(robot::Time(1000.1));
|
||||
|
||||
// NaN lọt ra cmd_vel là không được phép đi tiếp.
|
||||
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
|
||||
ASSERT_NE(result.velocity(), nullptr);
|
||||
EXPECT_TRUE(std::isfinite(result.velocity()->linear.x));
|
||||
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
|
||||
}
|
||||
|
||||
TEST(OutputKind, InfiniteAngularVelocityIsForcedToStop)
|
||||
{
|
||||
Fixture fixture(RecoveryOutputType::kVelocity);
|
||||
|
||||
robot_geometry_msgs::Twist bad;
|
||||
bad.angular.z = std::numeric_limits<double>::infinity();
|
||||
fixture.behavior.next_result =
|
||||
recovery_core::RecoveryResult::Velocity(bad, RecoveryStatus::kRunning);
|
||||
|
||||
const auto result = fixture.behavior.update(robot::Time(1000.1));
|
||||
|
||||
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
|
||||
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
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user