add file test
This commit is contained in:
@@ -92,7 +92,7 @@ else()
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_utils
|
||||
LIBRARIES ${PROJECT_NAME}
|
||||
CATKIN_DEPENDS
|
||||
robot_costmap_2d
|
||||
robot_nav_core
|
||||
@@ -105,7 +105,7 @@ else()
|
||||
robot_visualization_msgs
|
||||
robot_nav_2d_utils
|
||||
data_convert
|
||||
DEPENDS PCL Boost Eigen
|
||||
DEPENDS PCL Boost
|
||||
)
|
||||
|
||||
include_directories(
|
||||
@@ -218,11 +218,11 @@ endif()
|
||||
# ========================================================
|
||||
if(BUILDING_WITH_CATKIN)
|
||||
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
# )
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
@@ -237,12 +237,12 @@ if(BUILDING_WITH_CATKIN)
|
||||
|
||||
else()
|
||||
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
|
||||
# EXPORT ${PROJECT_NAME}-targets
|
||||
# ARCHIVE DESTINATION lib
|
||||
# LIBRARY DESTINATION lib
|
||||
# RUNTIME DESTINATION bin
|
||||
# )
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
EXPORT ${PROJECT_NAME}-targets
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(EXPORT ${PROJECT_NAME}-targets
|
||||
FILE ${PROJECT_NAME}-targets.cmake
|
||||
@@ -290,24 +290,15 @@ if(CATKIN_ENABLE_TESTING)
|
||||
message(FATAL_ERROR "catkin_add_gtest NOT FOUND")
|
||||
endif()
|
||||
|
||||
find_package(GTest REQUIRED)
|
||||
|
||||
add_executable(test_mission_adapters
|
||||
src/test_mission_adapters.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(test_mission_adapters
|
||||
mission_adapters
|
||||
GTest::GTest
|
||||
GTest::Main
|
||||
pthread
|
||||
${catkin_LIBRARIES}
|
||||
catkin_add_gtest(test_mission_adapters
|
||||
test/mission_adapters_test.cpp
|
||||
)
|
||||
|
||||
if(TARGET test_mission_adapters)
|
||||
target_link_libraries(test_mission_adapters
|
||||
mission_adapters
|
||||
${catkin_LIBRARIES}
|
||||
pthread
|
||||
)
|
||||
|
||||
target_include_directories(test_mission_adapters PRIVATE
|
||||
|
||||
@@ -7,8 +7,10 @@
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <functional>
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
|
||||
#include <robot/robot.h>
|
||||
#include <robot_protocol_msgs/Order.h>
|
||||
@@ -68,17 +70,17 @@ namespace mission_adapters
|
||||
class Action
|
||||
{
|
||||
public:
|
||||
int sequenceId;
|
||||
ActionType type;
|
||||
int sequenceId = 0;
|
||||
ActionType type = ActionType::NODE_ACTION;
|
||||
robot_protocol_msgs::Action action;
|
||||
};
|
||||
|
||||
class Mission
|
||||
{
|
||||
public:
|
||||
int sequenceId;
|
||||
MissionType type;
|
||||
int priority;
|
||||
int sequenceId = 0;
|
||||
MissionType type = MissionType::SIMPLE_GOAL;
|
||||
int priority = 0;
|
||||
robot_geometry_msgs::PoseStamped start;
|
||||
robot_geometry_msgs::PoseStamped goal;
|
||||
std::vector<robot_protocol_msgs::Node> nodes;
|
||||
@@ -88,8 +90,9 @@ namespace mission_adapters
|
||||
|
||||
struct Event
|
||||
{
|
||||
EventType type;
|
||||
int priority;
|
||||
EventType type = EventType::SUBMIT_MISSIONS;
|
||||
int priority = PRIORITY_ORDER;
|
||||
uint64_t sequence = 0;
|
||||
std::vector<std::shared_ptr<Mission>> missions;
|
||||
};
|
||||
|
||||
@@ -97,7 +100,10 @@ namespace mission_adapters
|
||||
{
|
||||
bool operator()(const Event& lhs, const Event& rhs) const
|
||||
{
|
||||
return lhs.priority > rhs.priority;
|
||||
if (lhs.priority != rhs.priority)
|
||||
return lhs.priority > rhs.priority;
|
||||
|
||||
return lhs.sequence > rhs.sequence;
|
||||
}
|
||||
};
|
||||
|
||||
@@ -108,6 +114,7 @@ namespace mission_adapters
|
||||
std::priority_queue<Event, std::vector<Event>, EventCompare> queue_;
|
||||
std::condition_variable cv_;
|
||||
bool stop_ = false;
|
||||
uint64_t next_sequence_ = 0;
|
||||
|
||||
public:
|
||||
void push(const Event& event);
|
||||
|
||||
@@ -11,13 +11,16 @@ namespace mission_adapters
|
||||
void EventBus::push(const Event& event)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
queue_.push(event);
|
||||
Event queued_event = event;
|
||||
queued_event.sequence = next_sequence_++;
|
||||
queue_.push(std::move(queued_event));
|
||||
cv_.notify_one();
|
||||
}
|
||||
|
||||
void EventBus::push(Event&& event)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
event.sequence = next_sequence_++;
|
||||
queue_.push(std::move(event));
|
||||
cv_.notify_one();
|
||||
}
|
||||
@@ -47,6 +50,8 @@ namespace mission_adapters
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
stop_ = false;
|
||||
next_sequence_ = 0;
|
||||
while (!queue_.empty()) queue_.pop();
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────
|
||||
@@ -74,7 +79,13 @@ namespace mission_adapters
|
||||
std::vector<std::shared_ptr<Mission>> missions;
|
||||
std::vector<size_t> action_node_indices;
|
||||
|
||||
if(order.nodes.empty()) return missions;
|
||||
if (order.nodes.empty()) return missions;
|
||||
|
||||
if (order.nodes.size() > 1 && order.edges.size() < order.nodes.size() - 1)
|
||||
{
|
||||
robot::log_error("Invalid VDA5050 order: edge count is smaller than node_count - 1");
|
||||
return missions;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < order.nodes.size(); ++i)
|
||||
{
|
||||
@@ -169,6 +180,10 @@ namespace mission_adapters
|
||||
void MissionManager::submit(const std::vector<std::shared_ptr<Mission>>& missions)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
if (state_ == MissionState::EMERGENCY)
|
||||
return;
|
||||
|
||||
if (!mission_queue_.empty())
|
||||
{
|
||||
mission_queue_ = {};
|
||||
@@ -188,7 +203,6 @@ namespace mission_adapters
|
||||
case MissionState::COMPLETED:
|
||||
case MissionState::FAILED:
|
||||
case MissionState::CANCELLED:
|
||||
case MissionState::EMERGENCY:
|
||||
case MissionState::CLEAR_EMERGENCY:
|
||||
state_ = MissionState::QUEUED;
|
||||
break;
|
||||
@@ -230,6 +244,10 @@ namespace mission_adapters
|
||||
void MissionManager::onNavigationDone()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
if (state_ != MissionState::RUNNING)
|
||||
return;
|
||||
|
||||
current_mission_.reset();
|
||||
state_ = mission_queue_.empty() ? MissionState::IDLE : MissionState::QUEUED;
|
||||
}
|
||||
@@ -237,6 +255,10 @@ namespace mission_adapters
|
||||
void MissionManager::onNavigationFailed()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
if (state_ != MissionState::RUNNING)
|
||||
return;
|
||||
|
||||
current_mission_.reset();
|
||||
while (!mission_queue_.empty()) mission_queue_.pop();
|
||||
state_ = MissionState::FAILED;
|
||||
@@ -374,38 +396,59 @@ namespace mission_adapters
|
||||
|
||||
void EventProcessor::pauseEvent()
|
||||
{
|
||||
event_bus_.push({EventType::PAUSE, PRIORITY_PAUSE, {}});
|
||||
Event event;
|
||||
event.type = EventType::PAUSE;
|
||||
event.priority = PRIORITY_PAUSE;
|
||||
event_bus_.push(std::move(event));
|
||||
}
|
||||
|
||||
void EventProcessor::resumeEvent()
|
||||
{
|
||||
event_bus_.push({EventType::RESUME, PRIORITY_RESUME, {}});
|
||||
Event event;
|
||||
event.type = EventType::RESUME;
|
||||
event.priority = PRIORITY_RESUME;
|
||||
event_bus_.push(std::move(event));
|
||||
}
|
||||
|
||||
void EventProcessor::cancelEvent()
|
||||
{
|
||||
event_bus_.push({EventType::CANCEL, PRIORITY_CANCEL, {}});
|
||||
Event event;
|
||||
event.type = EventType::CANCEL;
|
||||
event.priority = PRIORITY_CANCEL;
|
||||
event_bus_.push(std::move(event));
|
||||
}
|
||||
|
||||
void EventProcessor::navDoneEvent()
|
||||
{
|
||||
event_bus_.push({EventType::NAV_DONE, PRIORITY_NAV_DONE, {}});
|
||||
Event event;
|
||||
event.type = EventType::NAV_DONE;
|
||||
event.priority = PRIORITY_NAV_DONE;
|
||||
event_bus_.push(std::move(event));
|
||||
}
|
||||
|
||||
// FIX #4: Use PRIORITY_NAV_FAILED (correct constant name).
|
||||
void EventProcessor::navFailedEvent()
|
||||
{
|
||||
event_bus_.push({EventType::NAV_FAILED, PRIORITY_NAV_FAILED, {}});
|
||||
Event event;
|
||||
event.type = EventType::NAV_FAILED;
|
||||
event.priority = PRIORITY_NAV_FAILED;
|
||||
event_bus_.push(std::move(event));
|
||||
}
|
||||
|
||||
void EventProcessor::emergencyEvent()
|
||||
{
|
||||
event_bus_.push({EventType::EMERGENCY, PRIORITY_EMERGENCY, {}});
|
||||
Event event;
|
||||
event.type = EventType::EMERGENCY;
|
||||
event.priority = PRIORITY_EMERGENCY;
|
||||
event_bus_.push(std::move(event));
|
||||
}
|
||||
|
||||
void EventProcessor::clearEmergencyEvent()
|
||||
{
|
||||
event_bus_.push({EventType::CLEAR_EMERGENCY, PRIORITY_EMERGENCY, {}});
|
||||
Event event;
|
||||
event.type = EventType::CLEAR_EMERGENCY;
|
||||
event.priority = PRIORITY_EMERGENCY;
|
||||
event_bus_.push(std::move(event));
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────────────
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
#include <mission_adapters/mission_adapters.h>
|
||||
#include <move_base_core/navigation.h>
|
||||
|
||||
#include <utility>
|
||||
|
||||
using namespace mission_adapters;
|
||||
|
||||
class RobotControlTest
|
||||
@@ -23,7 +25,7 @@ class RobotControlTest
|
||||
}
|
||||
|
||||
public:
|
||||
RobotControlTest();
|
||||
explicit RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base);
|
||||
~RobotControlTest();
|
||||
|
||||
void run();
|
||||
@@ -32,7 +34,8 @@ private:
|
||||
void executeMission(const Mission& mission);
|
||||
};
|
||||
|
||||
RobotControlTest::RobotControlTest()
|
||||
RobotControlTest::RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base)
|
||||
: move_base_ptr_(std::move(move_base))
|
||||
{}
|
||||
|
||||
RobotControlTest::~RobotControlTest()
|
||||
@@ -43,31 +46,43 @@ RobotControlTest::~RobotControlTest()
|
||||
|
||||
void RobotControlTest::run()
|
||||
{
|
||||
if (!move_base_ptr_)
|
||||
{
|
||||
robot::log_error("RobotControlTest requires a valid BaseNavigation pointer");
|
||||
return;
|
||||
}
|
||||
|
||||
robot::Rate rate(50);
|
||||
|
||||
// FIX #1: Set the callback ONCE before starting the executor, not inside the loop.
|
||||
mission_executor_.setMissionCallback(
|
||||
[this](const std::shared_ptr<Mission>& mission)
|
||||
{
|
||||
executeMission(*mission);
|
||||
});
|
||||
|
||||
event_processor_.start();
|
||||
mission_executor_.start();
|
||||
|
||||
|
||||
while (true)
|
||||
while (robot::ok())
|
||||
{
|
||||
auto feedback = move_base_ptr_->getFeedback();
|
||||
if (!feedback)
|
||||
{
|
||||
rate.sleep();
|
||||
continue;
|
||||
}
|
||||
|
||||
auto nav_state = feedback->navigation_state;
|
||||
|
||||
if (nav_state != prev_nav_done_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
|
||||
if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
|
||||
{
|
||||
event_processor_.navDoneEvent();
|
||||
}
|
||||
else if (nav_state == robot::move_base_core::State::ABORTED)
|
||||
else if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::ABORTED)
|
||||
{
|
||||
event_processor_.navFailedEvent();
|
||||
}
|
||||
prev_nav_state_ = nav_state;
|
||||
prev_nav_state_ = nav_state;
|
||||
|
||||
// Example: receive an order (replace condition with your real source)
|
||||
if (/* new order available */ false)
|
||||
@@ -86,10 +101,3 @@ void RobotControlTest::executeMission(const Mission& mission)
|
||||
// TODO: send mission goal to move_base_ptr_
|
||||
(void)mission;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
RobotControlTest test;
|
||||
test.run();
|
||||
return 0;
|
||||
}
|
||||
Binary file not shown.
@@ -1,578 +0,0 @@
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "mission_adapters/mission_adapters.h"
|
||||
|
||||
using namespace mission_adapters;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
robot_geometry_msgs::PoseStamped MakeGoal(double x, double y)
|
||||
{
|
||||
robot_geometry_msgs::PoseStamped g;
|
||||
g.pose.position.x = x;
|
||||
g.pose.position.y = y;
|
||||
return g;
|
||||
}
|
||||
|
||||
robot_protocol_msgs::Action MakeAction(
|
||||
const std::string &id)
|
||||
{
|
||||
robot_protocol_msgs::Action a;
|
||||
a.actionId = id;
|
||||
a.actionType = "TEST";
|
||||
return a;
|
||||
}
|
||||
|
||||
robot_protocol_msgs::Node MakeNode(
|
||||
int seq,
|
||||
bool add_action = false)
|
||||
{
|
||||
robot_protocol_msgs::Node n;
|
||||
n.sequenceId = seq;
|
||||
n.nodeId = "node_" + std::to_string(seq);
|
||||
|
||||
n.nodePosition.x = seq;
|
||||
n.nodePosition.y = seq;
|
||||
|
||||
if (add_action)
|
||||
n.actions.push_back(
|
||||
MakeAction("node_action_" + std::to_string(seq)));
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
robot_protocol_msgs::Edge MakeEdge(
|
||||
int seq,
|
||||
bool add_action = false)
|
||||
{
|
||||
robot_protocol_msgs::Edge e;
|
||||
|
||||
e.sequenceId = seq;
|
||||
e.edgeId = "edge_" + std::to_string(seq);
|
||||
|
||||
if (add_action)
|
||||
e.actions.push_back(
|
||||
MakeAction("edge_action_" + std::to_string(seq)));
|
||||
|
||||
return e;
|
||||
}
|
||||
|
||||
robot_protocol_msgs::Order MakeOrder(
|
||||
int node_count)
|
||||
{
|
||||
robot_protocol_msgs::Order order;
|
||||
|
||||
for (int i = 0; i < node_count; ++i)
|
||||
order.nodes.push_back(
|
||||
MakeNode(i));
|
||||
|
||||
for (int i = 0; i < node_count - 1; ++i)
|
||||
order.edges.push_back(
|
||||
MakeEdge(i));
|
||||
|
||||
return order;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
//--------------------------------------------------------------
|
||||
// EventBus
|
||||
//--------------------------------------------------------------
|
||||
|
||||
TEST(EventBus, PriorityOrdering)
|
||||
{
|
||||
EventBus bus;
|
||||
|
||||
bus.push({EventType::PAUSE,
|
||||
PRIORITY_PAUSE,
|
||||
{}});
|
||||
|
||||
bus.push({EventType::CANCEL,
|
||||
PRIORITY_CANCEL,
|
||||
{}});
|
||||
|
||||
bus.push({EventType::EMERGENCY,
|
||||
PRIORITY_EMERGENCY,
|
||||
{}});
|
||||
|
||||
Event e;
|
||||
|
||||
ASSERT_TRUE(bus.pop(e));
|
||||
EXPECT_EQ(e.priority,
|
||||
PRIORITY_EMERGENCY);
|
||||
|
||||
ASSERT_TRUE(bus.pop(e));
|
||||
EXPECT_EQ(e.priority,
|
||||
PRIORITY_CANCEL);
|
||||
|
||||
ASSERT_TRUE(bus.pop(e));
|
||||
EXPECT_EQ(e.priority,
|
||||
PRIORITY_PAUSE);
|
||||
}
|
||||
|
||||
TEST(EventBus, StopReturnsFalse)
|
||||
{
|
||||
EventBus bus;
|
||||
|
||||
std::thread t([&]()
|
||||
{ bus.stop(); });
|
||||
|
||||
Event e;
|
||||
|
||||
EXPECT_FALSE(bus.pop(e));
|
||||
|
||||
t.join();
|
||||
}
|
||||
|
||||
TEST(EventBus, ConcurrentPush)
|
||||
{
|
||||
EventBus bus;
|
||||
|
||||
constexpr int N = 100;
|
||||
|
||||
std::thread producer([&]()
|
||||
{
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
bus.push(
|
||||
{EventType::NAV_DONE,
|
||||
PRIORITY_NAV_DONE,
|
||||
{}});
|
||||
}
|
||||
});
|
||||
|
||||
int count = 0;
|
||||
|
||||
while (count < N)
|
||||
{
|
||||
Event e;
|
||||
|
||||
if (bus.pop(e))
|
||||
++count;
|
||||
}
|
||||
|
||||
producer.join();
|
||||
|
||||
EXPECT_EQ(count, N);
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------
|
||||
// GoalAdapter
|
||||
//--------------------------------------------------------------
|
||||
|
||||
TEST(GoalAdapter, ConvertSingleGoal)
|
||||
{
|
||||
GoalAdapter adapter;
|
||||
|
||||
auto missions =
|
||||
adapter.convert(
|
||||
MakeGoal(5.5, 9.1));
|
||||
|
||||
ASSERT_EQ(missions.size(), 1u);
|
||||
|
||||
EXPECT_EQ(
|
||||
missions[0]->type,
|
||||
MissionType::SIMPLE_GOAL);
|
||||
|
||||
EXPECT_DOUBLE_EQ(
|
||||
missions[0]->goal.pose.position.x,
|
||||
5.5);
|
||||
|
||||
EXPECT_DOUBLE_EQ(
|
||||
missions[0]->goal.pose.position.y,
|
||||
9.1);
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------
|
||||
// VDA5050Adapter
|
||||
//--------------------------------------------------------------
|
||||
|
||||
TEST(VDA5050Adapter, EmptyOrder)
|
||||
{
|
||||
VDA5050Adapter adapter;
|
||||
|
||||
robot_protocol_msgs::Order order;
|
||||
|
||||
auto missions =
|
||||
adapter.convert(order);
|
||||
|
||||
EXPECT_TRUE(
|
||||
missions.empty());
|
||||
}
|
||||
|
||||
TEST(VDA5050Adapter, OrderWithoutActionsCreatesTailMission)
|
||||
{
|
||||
VDA5050Adapter adapter;
|
||||
|
||||
auto order =
|
||||
MakeOrder(4);
|
||||
|
||||
auto missions =
|
||||
adapter.convert(order);
|
||||
|
||||
ASSERT_EQ(
|
||||
missions.size(),
|
||||
1u);
|
||||
|
||||
EXPECT_EQ(
|
||||
missions[0]->nodes.size(),
|
||||
4u);
|
||||
|
||||
EXPECT_EQ(
|
||||
missions[0]->edges.size(),
|
||||
3u);
|
||||
}
|
||||
|
||||
TEST(VDA5050Adapter, SplitAtNodeAction)
|
||||
{
|
||||
VDA5050Adapter adapter;
|
||||
|
||||
auto order =
|
||||
MakeOrder(5);
|
||||
|
||||
order.nodes[2].actions.push_back(
|
||||
MakeAction("dock"));
|
||||
|
||||
auto missions =
|
||||
adapter.convert(order);
|
||||
|
||||
ASSERT_EQ(
|
||||
missions.size(),
|
||||
2u);
|
||||
|
||||
EXPECT_EQ(
|
||||
missions[0]->nodes.size(),
|
||||
3u);
|
||||
|
||||
EXPECT_EQ(
|
||||
missions[1]->nodes.size(),
|
||||
3u);
|
||||
}
|
||||
|
||||
TEST(VDA5050Adapter, CollectEdgeAction)
|
||||
{
|
||||
VDA5050Adapter adapter;
|
||||
|
||||
auto order =
|
||||
MakeOrder(2);
|
||||
|
||||
order.nodes[1].actions.push_back(
|
||||
MakeAction("node"));
|
||||
|
||||
order.edges[0].actions.push_back(
|
||||
MakeAction("edge"));
|
||||
|
||||
auto missions =
|
||||
adapter.convert(order);
|
||||
|
||||
ASSERT_EQ(
|
||||
missions.size(),
|
||||
1u);
|
||||
|
||||
ASSERT_EQ(
|
||||
missions[0]->actions.size(),
|
||||
2u);
|
||||
|
||||
EXPECT_LT(
|
||||
missions[0]->actions[0].sequenceId,
|
||||
missions[0]->actions[1].sequenceId);
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------
|
||||
// MissionManager
|
||||
//--------------------------------------------------------------
|
||||
|
||||
TEST(MissionManager, SubmitChangesState)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(1, 2)));
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::QUEUED);
|
||||
|
||||
EXPECT_TRUE(
|
||||
mgr.hasMission());
|
||||
}
|
||||
|
||||
TEST(MissionManager, NextMission)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(1, 2)));
|
||||
|
||||
auto m =
|
||||
mgr.nextMission();
|
||||
|
||||
ASSERT_NE(
|
||||
m,
|
||||
nullptr);
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::RUNNING);
|
||||
}
|
||||
|
||||
TEST(MissionManager, NavigationDone)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(0, 0)));
|
||||
|
||||
mgr.nextMission();
|
||||
|
||||
mgr.onNavigationDone();
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::IDLE);
|
||||
|
||||
EXPECT_FALSE(
|
||||
mgr.hasMission());
|
||||
}
|
||||
|
||||
TEST(MissionManager, NavigationFailed)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(0, 0)));
|
||||
|
||||
mgr.nextMission();
|
||||
|
||||
mgr.onNavigationFailed();
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::FAILED);
|
||||
}
|
||||
|
||||
TEST(MissionManager, PauseResume)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(1, 1)));
|
||||
|
||||
mgr.pause();
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::PAUSED);
|
||||
|
||||
mgr.resume();
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::QUEUED);
|
||||
}
|
||||
|
||||
TEST(MissionManager, Cancel)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(1, 1)));
|
||||
|
||||
mgr.cancel();
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::CANCELLED);
|
||||
|
||||
EXPECT_FALSE(
|
||||
mgr.hasMission());
|
||||
}
|
||||
|
||||
TEST(MissionManager, Emergency)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(2, 2)));
|
||||
|
||||
mgr.emergency();
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::EMERGENCY);
|
||||
}
|
||||
|
||||
TEST(MissionManager, ClearEmergency)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
mgr.emergency();
|
||||
|
||||
mgr.clearEmergency();
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::CLEAR_EMERGENCY);
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------
|
||||
// EventProcessor
|
||||
//--------------------------------------------------------------
|
||||
|
||||
TEST(EventProcessor, GoalEvent)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
EventProcessor proc(mgr);
|
||||
|
||||
proc.start();
|
||||
|
||||
proc.goalEvent(
|
||||
MakeGoal(5, 6));
|
||||
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(100));
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::QUEUED);
|
||||
|
||||
proc.stop();
|
||||
}
|
||||
|
||||
TEST(EventProcessor, EmergencyHasHighestPriority)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
EventProcessor proc(mgr);
|
||||
|
||||
proc.start();
|
||||
|
||||
proc.goalEvent(
|
||||
MakeGoal(1, 1));
|
||||
|
||||
proc.emergencyEvent();
|
||||
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(100));
|
||||
|
||||
EXPECT_EQ(
|
||||
mgr.state(),
|
||||
MissionState::EMERGENCY);
|
||||
|
||||
proc.stop();
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------
|
||||
// MissionExecutor
|
||||
//--------------------------------------------------------------
|
||||
|
||||
TEST(MissionExecutor, CallbackInvokedOnce)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(10, 10)));
|
||||
|
||||
MissionExecutor exec(mgr);
|
||||
|
||||
std::atomic<int> count{0};
|
||||
|
||||
exec.setMissionCallback(
|
||||
[&](const std::shared_ptr<Mission> &)
|
||||
{
|
||||
count++;
|
||||
});
|
||||
|
||||
exec.start();
|
||||
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(200));
|
||||
|
||||
exec.stop();
|
||||
|
||||
EXPECT_EQ(
|
||||
count.load(),
|
||||
1);
|
||||
}
|
||||
|
||||
TEST(MissionExecutor, NewMissionAfterDone)
|
||||
{
|
||||
MissionManager mgr;
|
||||
|
||||
GoalAdapter adapter;
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(1, 1)));
|
||||
|
||||
MissionExecutor exec(mgr);
|
||||
|
||||
std::atomic<int> count{0};
|
||||
|
||||
exec.setMissionCallback(
|
||||
[&](const std::shared_ptr<Mission> &)
|
||||
{
|
||||
count++;
|
||||
});
|
||||
|
||||
exec.start();
|
||||
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(100));
|
||||
|
||||
mgr.onNavigationDone();
|
||||
|
||||
mgr.submit(
|
||||
adapter.convert(
|
||||
MakeGoal(2, 2)));
|
||||
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(150));
|
||||
|
||||
exec.stop();
|
||||
|
||||
EXPECT_EQ(
|
||||
count.load(),
|
||||
2);
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
::testing::InitGoogleTest(
|
||||
&argc,
|
||||
argv);
|
||||
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
360
test/mission_adapters_test.cpp
Normal file
360
test/mission_adapters_test.cpp
Normal file
@@ -0,0 +1,360 @@
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include <mission_adapters/mission_adapters.h>
|
||||
|
||||
namespace
|
||||
{
|
||||
using namespace mission_adapters;
|
||||
|
||||
robot_geometry_msgs::PoseStamped makeGoal(double x, double y)
|
||||
{
|
||||
robot_geometry_msgs::PoseStamped goal;
|
||||
goal.pose.position.x = x;
|
||||
goal.pose.position.y = y;
|
||||
return goal;
|
||||
}
|
||||
|
||||
robot_protocol_msgs::Action makeAction(const std::string& id)
|
||||
{
|
||||
robot_protocol_msgs::Action action;
|
||||
action.actionId = id;
|
||||
action.actionType = "TEST";
|
||||
return action;
|
||||
}
|
||||
|
||||
robot_protocol_msgs::Node makeNode(int sequence_id, bool add_action = false)
|
||||
{
|
||||
robot_protocol_msgs::Node node;
|
||||
node.sequenceId = sequence_id;
|
||||
node.nodeId = "node_" + std::to_string(sequence_id);
|
||||
node.nodePosition.x = sequence_id;
|
||||
node.nodePosition.y = sequence_id;
|
||||
|
||||
if (add_action)
|
||||
node.actions.push_back(makeAction("node_action_" + std::to_string(sequence_id)));
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
robot_protocol_msgs::Edge makeEdge(int sequence_id, bool add_action = false)
|
||||
{
|
||||
robot_protocol_msgs::Edge edge;
|
||||
edge.sequenceId = sequence_id;
|
||||
edge.edgeId = "edge_" + std::to_string(sequence_id);
|
||||
|
||||
if (add_action)
|
||||
edge.actions.push_back(makeAction("edge_action_" + std::to_string(sequence_id)));
|
||||
|
||||
return edge;
|
||||
}
|
||||
|
||||
robot_protocol_msgs::Order makeOrder(int node_count)
|
||||
{
|
||||
robot_protocol_msgs::Order order;
|
||||
|
||||
for (int i = 0; i < node_count; ++i)
|
||||
order.nodes.push_back(makeNode(i));
|
||||
|
||||
for (int i = 0; i < node_count - 1; ++i)
|
||||
order.edges.push_back(makeEdge(i));
|
||||
|
||||
return order;
|
||||
}
|
||||
|
||||
bool waitForState(MissionManager& manager, MissionState expected, std::chrono::milliseconds timeout)
|
||||
{
|
||||
const auto deadline = std::chrono::steady_clock::now() + timeout;
|
||||
while (std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
if (manager.state() == expected)
|
||||
return true;
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
}
|
||||
|
||||
return manager.state() == expected;
|
||||
}
|
||||
|
||||
class MissionAdaptersTest : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
GoalAdapter goal_adapter;
|
||||
VDA5050Adapter order_adapter;
|
||||
};
|
||||
|
||||
TEST(EventBusTest, PopsHighestPriorityFirst)
|
||||
{
|
||||
EventBus bus;
|
||||
|
||||
Event pause;
|
||||
pause.type = EventType::PAUSE;
|
||||
pause.priority = PRIORITY_PAUSE;
|
||||
bus.push(pause);
|
||||
|
||||
Event cancel;
|
||||
cancel.type = EventType::CANCEL;
|
||||
cancel.priority = PRIORITY_CANCEL;
|
||||
bus.push(cancel);
|
||||
|
||||
Event emergency;
|
||||
emergency.type = EventType::EMERGENCY;
|
||||
emergency.priority = PRIORITY_EMERGENCY;
|
||||
bus.push(emergency);
|
||||
|
||||
Event event;
|
||||
ASSERT_TRUE(bus.pop(event));
|
||||
EXPECT_EQ(event.type, EventType::EMERGENCY);
|
||||
|
||||
ASSERT_TRUE(bus.pop(event));
|
||||
EXPECT_EQ(event.type, EventType::CANCEL);
|
||||
|
||||
ASSERT_TRUE(bus.pop(event));
|
||||
EXPECT_EQ(event.type, EventType::PAUSE);
|
||||
}
|
||||
|
||||
TEST(EventBusTest, PreservesFifoOrderForSamePriority)
|
||||
{
|
||||
EventBus bus;
|
||||
|
||||
Event emergency;
|
||||
emergency.type = EventType::EMERGENCY;
|
||||
emergency.priority = PRIORITY_EMERGENCY;
|
||||
bus.push(emergency);
|
||||
|
||||
Event clear_emergency;
|
||||
clear_emergency.type = EventType::CLEAR_EMERGENCY;
|
||||
clear_emergency.priority = PRIORITY_EMERGENCY;
|
||||
bus.push(clear_emergency);
|
||||
|
||||
Event event;
|
||||
ASSERT_TRUE(bus.pop(event));
|
||||
EXPECT_EQ(event.type, EventType::EMERGENCY);
|
||||
|
||||
ASSERT_TRUE(bus.pop(event));
|
||||
EXPECT_EQ(event.type, EventType::CLEAR_EMERGENCY);
|
||||
}
|
||||
|
||||
TEST(EventBusTest, StopUnblocksPop)
|
||||
{
|
||||
EventBus bus;
|
||||
Event event;
|
||||
|
||||
std::thread stopper([&bus] { bus.stop(); });
|
||||
EXPECT_FALSE(bus.pop(event));
|
||||
stopper.join();
|
||||
}
|
||||
|
||||
TEST(EventBusTest, ResetDropsPendingEvents)
|
||||
{
|
||||
EventBus bus;
|
||||
|
||||
Event pause;
|
||||
pause.type = EventType::PAUSE;
|
||||
pause.priority = PRIORITY_PAUSE;
|
||||
bus.push(pause);
|
||||
|
||||
bus.reset();
|
||||
EXPECT_TRUE(bus.empty());
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, GoalAdapterCreatesSingleMission)
|
||||
{
|
||||
const auto missions = goal_adapter.convert(makeGoal(5.5, 9.1));
|
||||
|
||||
ASSERT_EQ(missions.size(), 1u);
|
||||
EXPECT_EQ(missions.front()->type, MissionType::SIMPLE_GOAL);
|
||||
EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 5.5);
|
||||
EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 9.1);
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, EmptyOrderCreatesNoMissions)
|
||||
{
|
||||
robot_protocol_msgs::Order order;
|
||||
EXPECT_TRUE(order_adapter.convert(order).empty());
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, OrderWithoutActionsCreatesOneTailMission)
|
||||
{
|
||||
const auto missions = order_adapter.convert(makeOrder(4));
|
||||
|
||||
ASSERT_EQ(missions.size(), 1u);
|
||||
EXPECT_EQ(missions.front()->nodes.size(), 4u);
|
||||
EXPECT_EQ(missions.front()->edges.size(), 3u);
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, InvalidOrderWithMissingEdgesCreatesNoMissions)
|
||||
{
|
||||
auto order = makeOrder(4);
|
||||
order.edges.pop_back();
|
||||
|
||||
EXPECT_TRUE(order_adapter.convert(order).empty());
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, OrderSplitsAtNodeAction)
|
||||
{
|
||||
auto order = makeOrder(5);
|
||||
order.nodes[2].actions.push_back(makeAction("dock"));
|
||||
|
||||
const auto missions = order_adapter.convert(order);
|
||||
|
||||
ASSERT_EQ(missions.size(), 2u);
|
||||
EXPECT_EQ(missions[0]->nodes.size(), 3u);
|
||||
EXPECT_EQ(missions[1]->nodes.size(), 3u);
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, OrderCollectsAndSortsActions)
|
||||
{
|
||||
auto order = makeOrder(2);
|
||||
order.edges[0].actions.push_back(makeAction("edge"));
|
||||
order.nodes[1].actions.push_back(makeAction("node"));
|
||||
|
||||
const auto missions = order_adapter.convert(order);
|
||||
|
||||
ASSERT_EQ(missions.size(), 1u);
|
||||
ASSERT_EQ(missions.front()->actions.size(), 2u);
|
||||
EXPECT_EQ(missions.front()->actions[0].type, ActionType::EDGE_ACTION);
|
||||
EXPECT_EQ(missions.front()->actions[1].type, ActionType::NODE_ACTION);
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, ManagerRunsMissionLifecycle)
|
||||
{
|
||||
MissionManager manager;
|
||||
|
||||
manager.submit(goal_adapter.convert(makeGoal(1.0, 2.0)));
|
||||
EXPECT_EQ(manager.state(), MissionState::QUEUED);
|
||||
EXPECT_TRUE(manager.hasMission());
|
||||
|
||||
auto mission = manager.nextMission();
|
||||
ASSERT_NE(mission, nullptr);
|
||||
EXPECT_EQ(manager.state(), MissionState::RUNNING);
|
||||
|
||||
manager.onNavigationDone();
|
||||
EXPECT_EQ(manager.state(), MissionState::IDLE);
|
||||
EXPECT_FALSE(manager.hasMission());
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, ManagerHandlesPauseResumeCancelAndFailure)
|
||||
{
|
||||
MissionManager manager;
|
||||
|
||||
manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
|
||||
manager.pause();
|
||||
EXPECT_EQ(manager.state(), MissionState::PAUSED);
|
||||
|
||||
manager.resume();
|
||||
EXPECT_EQ(manager.state(), MissionState::QUEUED);
|
||||
|
||||
manager.cancel();
|
||||
EXPECT_EQ(manager.state(), MissionState::CANCELLED);
|
||||
EXPECT_FALSE(manager.hasMission());
|
||||
|
||||
manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
|
||||
manager.nextMission();
|
||||
manager.onNavigationFailed();
|
||||
EXPECT_EQ(manager.state(), MissionState::FAILED);
|
||||
EXPECT_FALSE(manager.hasMission());
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, NavigationResultIsIgnoredOutsideRunningState)
|
||||
{
|
||||
MissionManager manager;
|
||||
|
||||
manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
|
||||
manager.emergency();
|
||||
manager.onNavigationDone();
|
||||
EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
|
||||
|
||||
manager.onNavigationFailed();
|
||||
EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
|
||||
|
||||
manager.clearEmergency();
|
||||
manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
|
||||
manager.cancel();
|
||||
manager.onNavigationDone();
|
||||
EXPECT_EQ(manager.state(), MissionState::CANCELLED);
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, EmergencyClearsAndBlocksNewMissionsUntilCleared)
|
||||
{
|
||||
MissionManager manager;
|
||||
|
||||
manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
|
||||
manager.emergency();
|
||||
EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
|
||||
EXPECT_FALSE(manager.hasMission());
|
||||
|
||||
manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
|
||||
EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
|
||||
EXPECT_FALSE(manager.hasMission());
|
||||
|
||||
manager.clearEmergency();
|
||||
manager.submit(goal_adapter.convert(makeGoal(3.0, 3.0)));
|
||||
EXPECT_EQ(manager.state(), MissionState::QUEUED);
|
||||
EXPECT_TRUE(manager.hasMission());
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, EventProcessorProcessesGoalAndEmergency)
|
||||
{
|
||||
MissionManager manager;
|
||||
EventProcessor processor(manager);
|
||||
|
||||
processor.start();
|
||||
processor.goalEvent(makeGoal(5.0, 6.0));
|
||||
EXPECT_TRUE(waitForState(manager, MissionState::QUEUED, std::chrono::milliseconds(250)));
|
||||
|
||||
processor.emergencyEvent();
|
||||
EXPECT_TRUE(waitForState(manager, MissionState::EMERGENCY, std::chrono::milliseconds(250)));
|
||||
|
||||
processor.goalEvent(makeGoal(7.0, 8.0));
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
|
||||
|
||||
processor.clearEmergencyEvent();
|
||||
EXPECT_TRUE(waitForState(manager, MissionState::CLEAR_EMERGENCY, std::chrono::milliseconds(250)));
|
||||
|
||||
processor.stop();
|
||||
}
|
||||
|
||||
TEST_F(MissionAdaptersTest, MissionExecutorDispatchesEachMissionOnce)
|
||||
{
|
||||
MissionManager manager;
|
||||
MissionExecutor executor(manager);
|
||||
std::atomic<int> callback_count{0};
|
||||
|
||||
executor.setMissionCallback(
|
||||
[&callback_count](const std::shared_ptr<Mission>& mission)
|
||||
{
|
||||
ASSERT_NE(mission, nullptr);
|
||||
++callback_count;
|
||||
});
|
||||
|
||||
manager.submit(goal_adapter.convert(makeGoal(10.0, 10.0)));
|
||||
executor.start();
|
||||
|
||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(250);
|
||||
while (callback_count.load() < 1 && std::chrono::steady_clock::now() < deadline)
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
|
||||
manager.onNavigationDone();
|
||||
manager.submit(goal_adapter.convert(makeGoal(11.0, 11.0)));
|
||||
|
||||
const auto second_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(250);
|
||||
while (callback_count.load() < 2 && std::chrono::steady_clock::now() < second_deadline)
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
|
||||
executor.stop();
|
||||
|
||||
EXPECT_EQ(callback_count.load(), 2);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user