recovery_core/test/goal_semantics_test.cpp

/*********************************************************************
 *
 * Kiểm ngữ nghĩa RecoveryGoal sau khi bỏ sentinel "0 = dùng default".
 *
 * Bản trước dùng `std::abs(goal.angle) > 0.0` để quyết định "caller có đặt góc không", nên một góc
 * tính từ hình học ra đúng 0 bị âm thầm thay bằng pi/2 — robot quay 90 độ mà không log gì. Test này
 * khoá lại: `std::optional` phân biệt được "không đặt" với "đặt bằng 0".
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <cmath>
#include <cstdlib>
#include <limits>

#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;

/// Nạp bộ behavior test qua đúng đường Boost.DLL mà runtime dùng.
struct PluginFixture
{
  PluginFixture()
  {
    loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
  }

  recovery_core::RecoveryBehavior* behavior(const std::string& name)
  {
    return recovery_test::findBehavior(registry, name);
  }

  VelocityRig rig;
  robot::NodeHandle nh;
  recovery_core::RecoveryRegistry registry;
  bool loaded = false;
};

TEST(GoalSemantics, ExplicitZeroAngleDoesNotRotate)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* rotate = fixture.behavior("rotate");
  ASSERT_NE(rotate, nullptr);

  RecoveryGoal goal;
  goal.angle = 0.0;  // "đừng quay" — một yêu cầu hợp lệ

  ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
  const auto result = rotate->update(robot::Time(1000.1));

  EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
  ASSERT_NE(result.velocity(), nullptr);
  EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}

TEST(GoalSemantics, UnsetAngleUsesFullRotationDefault)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* rotate = fixture.behavior("rotate");
  ASSERT_NE(rotate, nullptr);

  RecoveryGoal goal;  // angle không đặt

  ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
  const auto result = rotate->update(robot::Time(1000.1));

  ASSERT_EQ(result.status, RecoveryStatus::kRunning);
  EXPECT_NEAR(result.remaining, kTwoPi, 1e-3);
}

TEST(GoalSemantics, TinyAngleIsRespectedNotReplaced)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* rotate = fixture.behavior("rotate");
  ASSERT_NE(rotate, nullptr);

  RecoveryGoal goal;
  goal.angle = 0.05;  // nhỏ nhưng khác 0

  ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
  const auto result = rotate->update(robot::Time(1000.1));

  ASSERT_EQ(result.status, RecoveryStatus::kRunning);
  EXPECT_NEAR(result.remaining, 0.05, 1e-3);
}

TEST(GoalSemantics, NegativeAngleRotatesClockwise)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* rotate = fixture.behavior("rotate");
  ASSERT_NE(rotate, nullptr);

  RecoveryGoal goal;
  goal.angle = -1.0;

  ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
  fixture.rig.clock.advance(0.1);
  const auto result = rotate->update(fixture.rig.clock.now());

  ASSERT_NE(result.velocity(), nullptr);
  EXPECT_LT(result.velocity()->angular.z, 0.0);
}

TEST(GoalSemantics, AngleBeyondTwoPiIsClamped)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* rotate = fixture.behavior("rotate");
  ASSERT_NE(rotate, nullptr);

  RecoveryGoal goal;
  goal.angle = 100.0;  // ~16 vòng — bản cũ nhận nguyên

  ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
  const auto result = rotate->update(robot::Time(1000.1));

  ASSERT_EQ(result.status, RecoveryStatus::kRunning);
  EXPECT_NEAR(result.remaining, kTwoPi, 1e-3);
}

TEST(GoalSemantics, UnsetDistanceUsesConfiguredDefault)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* back_up = fixture.behavior("back_up");
  ASSERT_NE(back_up, nullptr);

  RecoveryGoal goal;  // distance không đặt -> backup_distance: 0.28 trong config test

  ASSERT_TRUE(back_up->start(goal, robot::Time(1000.0)));
  const auto result = back_up->update(robot::Time(1000.1));

  ASSERT_EQ(result.status, RecoveryStatus::kRunning);
  EXPECT_NEAR(result.remaining, 0.28, 1e-3);
}

TEST(GoalSemantics, DistanceBeyondMaxIsClamped)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* back_up = fixture.behavior("back_up");
  ASSERT_NE(back_up, nullptr);

  RecoveryGoal goal;
  goal.distance = 50.0;  // backup_distance_max: 1.0

  ASSERT_TRUE(back_up->start(goal, robot::Time(1000.0)));
  const auto result = back_up->update(robot::Time(1000.1));

  ASSERT_EQ(result.status, RecoveryStatus::kRunning);
  EXPECT_NEAR(result.remaining, 1.0, 1e-3);
}

TEST(GoalSemantics, NonPositiveDistanceIsRejectedByBase)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* back_up = fixture.behavior("back_up");
  ASSERT_NE(back_up, nullptr);

  RecoveryGoal goal;
  goal.distance = -0.1;

  // distance có giá trị thì phải > 0. Base bắt trước khi plugin nhìn thấy goal.
  EXPECT_FALSE(back_up->start(goal, robot::Time(1000.0)));
}

TEST(GoalSemantics, NonFiniteGoalIsRejectedByBase)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* rotate = fixture.behavior("rotate");
  ASSERT_NE(rotate, nullptr);

  RecoveryGoal nan_angle;
  nan_angle.angle = std::numeric_limits<double>::quiet_NaN();
  EXPECT_FALSE(rotate->start(nan_angle, robot::Time(1000.0)));

  RecoveryGoal nan_param;
  nan_param.params["angular_speed"] = std::numeric_limits<double>::infinity();
  EXPECT_FALSE(rotate->start(nan_param, robot::Time(1000.0)));
}

TEST(GoalSemantics, PerRunSpeedOverrideIsApplied)
{
  PluginFixture fixture;
  ASSERT_TRUE(fixture.loaded);
  auto* rotate = fixture.behavior("rotate");
  ASSERT_NE(rotate, nullptr);

  RecoveryGoal goal;
  goal.angle = kTwoPi;
  goal.params["angular_speed"] = 0.2;

  ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));

  // Đi đủ lâu để ramp gia tốc đạt trần tốc độ yêu cầu.
  robot::Time now(1000.0);
  double commanded = 0.0;
  for (int i = 0; i < 20; ++i)
  {
    now = robot::Time(now.toSec() + 0.1);
    const auto result = rotate->update(now);
    ASSERT_NE(result.velocity(), nullptr);
    commanded = result.velocity()->angular.z;
    fixture.rig.applyCommand(result.command, 0.1);
  }

  EXPECT_NEAR(commanded, 0.2, 1e-6);
}

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