1296.91 액션 노드의 단위 테스트
1. 개요
액션 노드의 단위 테스트(unit test)는 개별 액션 노드의 동작을 독립적으로 검증하는 소프트웨어 테스트 기법이다. 행동 트리를 구성하는 액션 노드가 정확한 반환 상태를 보고하고, 입출력 포트를 올바르게 처리하며, halt에 적절히 대응하는지를 자동화된 테스트를 통해 검증한다.
단위 테스트는 통합 테스트(integration test)에 비해 실행 속도가 빠르고, 오류의 원인을 정확히 특정할 수 있으며, 외부 의존성(ROS2 노드, 액션 서버, 센서 등) 없이 실행할 수 있다는 장점이 있다.
2. 테스트 프레임워크
2.1 Google Test (gtest)
ROS2 생태계에서 C++ 단위 테스트의 표준 프레임워크는 Google Test(gtest)이다. ament_cmake의 테스트 매크로를 통해 빌드 시스템과 통합된다.
# CMakeLists.txt
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(test_action_nodes
test/test_action_nodes.cpp)
target_link_libraries(test_action_nodes
${PROJECT_NAME}
${behaviortree_cpp_LIBRARIES})
2.2 테스트 구조
#include <gtest/gtest.h>
#include <behaviortree_cpp/bt_factory.h>
class ActionNodeTest : public ::testing::Test
{
protected:
void SetUp() override
{
factory_ =
std::make_shared<BT::BehaviorTreeFactory>();
// 노드 등록
registerTestNodes();
}
void TearDown() override
{
factory_.reset();
}
std::shared_ptr<BT::BehaviorTreeFactory> factory_;
BT::Blackboard::Ptr blackboard_ =
BT::Blackboard::create();
};
3. 테스트 범주
3.1 반환 상태 테스트
액션 노드가 주어진 입력에 대해 올바른 반환 상태(SUCCESS, FAILURE, RUNNING)를 보고하는지 검증한다.
TEST_F(ActionNodeTest, SuccessOnValidInput)
{
auto tree = factory_->createTreeFromText(R"(
<root BTCPP_format="4">
<BehaviorTree>
<TestAction input="valid_value"
result="{output}" />
</BehaviorTree>
</root>
)", blackboard_);
auto status = tree.tickWhileRunning();
EXPECT_EQ(status, BT::NodeStatus::SUCCESS);
}
TEST_F(ActionNodeTest, FailureOnInvalidInput)
{
blackboard_->set("bad_input", -1.0);
auto tree = factory_->createTreeFromText(R"(
<root BTCPP_format="4">
<BehaviorTree>
<TestAction input="{bad_input}" />
</BehaviorTree>
</root>
)", blackboard_);
auto status = tree.tickWhileRunning();
EXPECT_EQ(status, BT::NodeStatus::FAILURE);
}
3.2 포트 입출력 테스트
입력 포트의 값이 올바르게 읽히고, 출력 포트에 결과가 올바르게 기록되는지 검증한다.
TEST_F(ActionNodeTest, OutputPortWritten)
{
blackboard_->set("altitude", 20.0);
auto tree = factory_->createTreeFromText(R"(
<root BTCPP_format="4">
<BehaviorTree>
<MockTakeoff target_altitude="{altitude}"
reached_altitude="{result}" />
</BehaviorTree>
</root>
)", blackboard_);
tree.tickWhileRunning();
double reached;
EXPECT_TRUE(blackboard_->get("result", reached));
EXPECT_NEAR(reached, 20.0, 0.5);
}
3.3 Halt 테스트
RUNNING 상태에서 halt가 호출되었을 때, 노드가 자원을 올바르게 해제하고 재시작이 가능한지 검증한다.
TEST_F(ActionNodeTest, HaltAndRestart)
{
auto tree = factory_->createTreeFromText(R"(
<root BTCPP_format="4">
<BehaviorTree>
<LongRunningAction duration="10.0" />
</BehaviorTree>
</root>
)", blackboard_);
// 첫 번째 tick: RUNNING 반환
auto status = tree.tickOnce();
EXPECT_EQ(status, BT::NodeStatus::RUNNING);
// Halt
tree.haltTree();
// 재시작: 다시 RUNNING 반환
status = tree.tickOnce();
EXPECT_EQ(status, BT::NodeStatus::RUNNING);
}
3.4 타임아웃 테스트
지정된 시간 내에 작업이 완료되지 않으면 FAILURE를 반환하는지 검증한다.
TEST_F(ActionNodeTest, TimeoutFailure)
{
auto tree = factory_->createTreeFromText(R"(
<root BTCPP_format="4">
<BehaviorTree>
<Timeout msec="1000">
<NeverCompletingAction />
</Timeout>
</BehaviorTree>
</root>
)", blackboard_);
auto status = tree.tickWhileRunning();
EXPECT_EQ(status, BT::NodeStatus::FAILURE);
}
4. 모의 객체 (Mock Object)
4.1 모의 액션 서버
실제 ROS2 액션 서버 없이 테스트하기 위해, 모의 액션 서버를 구현한다.
template <typename ActionT>
class MockActionServer
{
public:
MockActionServer(
rclcpp::Node::SharedPtr node,
const std::string& action_name)
{
server_ =
rclcpp_action::create_server<ActionT>(
node, action_name,
std::bind(&MockActionServer::handleGoal,
this, std::placeholders::_1,
std::placeholders::_2),
std::bind(&MockActionServer::handleCancel,
this, std::placeholders::_1),
std::bind(&MockActionServer::handleAccepted,
this, std::placeholders::_1));
}
void setResult(bool success)
{
mock_success_ = success;
}
void setDelay(std::chrono::milliseconds delay)
{
mock_delay_ = delay;
}
private:
rclcpp_action::GoalResponse handleGoal(
const rclcpp_action::GoalUUID&,
std::shared_ptr<const typename ActionT::Goal>)
{
return rclcpp_action::GoalResponse::
ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handleCancel(
const std::shared_ptr<
rclcpp_action::ServerGoalHandle<ActionT>>)
{
return rclcpp_action::CancelResponse::ACCEPT;
}
void handleAccepted(
const std::shared_ptr<
rclcpp_action::ServerGoalHandle<ActionT>>
goal_handle)
{
std::thread([this, goal_handle]()
{
std::this_thread::sleep_for(mock_delay_);
auto result =
std::make_shared<typename ActionT::Result>();
result->success = mock_success_;
goal_handle->succeed(result);
}).detach();
}
typename rclcpp_action::Server<ActionT>::SharedPtr
server_;
bool mock_success_{true};
std::chrono::milliseconds mock_delay_{100};
};
4.2 모의 센서 토픽
센서 데이터를 시뮬레이션하는 모의 발행자를 구현한다.
class MockCamera
{
public:
MockCamera(rclcpp::Node::SharedPtr node,
const std::string& topic)
{
publisher_ =
node->create_publisher<sensor_msgs::msg::Image>(
topic, rclcpp::SensorDataQoS());
}
void publishImage(int width, int height)
{
auto msg = sensor_msgs::msg::Image();
msg.header.stamp =
rclcpp::Clock().now();
msg.width = width;
msg.height = height;
msg.encoding = "bgr8";
msg.data.resize(width * height * 3, 128);
publisher_->publish(msg);
}
private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr
publisher_;
};
5. 테스트 실행
5.1 colcon을 통한 테스트 실행
colcon test --packages-select my_bt_nodes
colcon test-result --verbose
5.2 특정 테스트만 실행
colcon test --packages-select my_bt_nodes \
--ctest-args -R "ActionNodeTest"
6. 테스트 커버리지
단위 테스트가 커버해야 하는 주요 시나리오는 다음과 같다.
- 정상 입력에 대한
SUCCESS반환 - 잘못된 입력에 대한
FAILURE반환 - 서버 미응답 시 타임아웃
FAILURE RUNNING중 halt 호출 후 재시작- 블랙보드 입출력 포트의 올바른 동작
- 예외 발생 시
FAILURE반환 및 자원 해제 - 연속 실행의 독립성(이전 실행이 다음 실행에 영향 없음)
7. 참고 문헌
- Colledanchise, M. and Ögren, P., “Behavior Trees in Robotics and AI: An Introduction,” CRC Press, 2018.
- Faconti, D. and Contributors, “BehaviorTree.CPP: A C++ library to build Behavior Trees,” GitHub Repository, https://github.com/BehaviorTree/BehaviorTree.CPP.
- Google, “Google Test User’s Guide,” https://google.github.io/googletest/.
- Open Robotics, “ament_cmake_gtest — ROS 2 Documentation,” https://docs.ros.org/en/rolling/p/ament_cmake_gtest/.
버전: 2026-04-04