104 lines
2.7 KiB
C++
104 lines
2.7 KiB
C++
#include <mission_adapters/mission_adapters.h>
|
|
#include <move_base_core/navigation.h>
|
|
|
|
#include <utility>
|
|
|
|
using namespace mission_adapters;
|
|
|
|
class RobotControlTest
|
|
{
|
|
robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
|
|
|
|
mission_adapters::MissionManager mission_manager_;
|
|
mission_adapters::EventProcessor event_processor_{mission_manager_};
|
|
mission_adapters::MissionExecutor mission_executor_{mission_manager_};
|
|
|
|
// FIX #7: Fully qualified namespace for the initial value.
|
|
robot::move_base_core::State prev_nav_state_ = robot::move_base_core::State::PENDING;
|
|
|
|
// Tracks whether the actions of the current mission are complete.
|
|
// FIX #8: Stub — replace with real action-done check from your action executor.
|
|
bool areActionsDone() const
|
|
{
|
|
// TODO: query your action executor for completion status.
|
|
return true;
|
|
}
|
|
|
|
public:
|
|
explicit RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base);
|
|
~RobotControlTest();
|
|
|
|
void run();
|
|
|
|
private:
|
|
void executeMission(const Mission& mission);
|
|
};
|
|
|
|
RobotControlTest::RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base)
|
|
: move_base_ptr_(std::move(move_base))
|
|
{}
|
|
|
|
RobotControlTest::~RobotControlTest()
|
|
{
|
|
event_processor_.stop();
|
|
mission_executor_.stop();
|
|
}
|
|
|
|
void RobotControlTest::run()
|
|
{
|
|
if (!move_base_ptr_)
|
|
{
|
|
robot::log_error("RobotControlTest requires a valid BaseNavigation pointer");
|
|
return;
|
|
}
|
|
|
|
robot::Rate rate(50);
|
|
|
|
mission_executor_.setMissionCallback(
|
|
[this](const std::shared_ptr<Mission>& mission)
|
|
{
|
|
executeMission(*mission);
|
|
});
|
|
|
|
event_processor_.start();
|
|
mission_executor_.start();
|
|
|
|
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_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
|
|
{
|
|
event_processor_.navDoneEvent();
|
|
}
|
|
else if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::ABORTED)
|
|
{
|
|
event_processor_.navFailedEvent();
|
|
}
|
|
prev_nav_state_ = nav_state;
|
|
|
|
// Example: receive an order (replace condition with your real source)
|
|
if (/* new order available */ false)
|
|
{
|
|
robot_protocol_msgs::Order order;
|
|
// ... populate order ...
|
|
event_processor_.orderEvent(order);
|
|
}
|
|
|
|
rate.sleep();
|
|
}
|
|
}
|
|
|
|
void RobotControlTest::executeMission(const Mission& mission)
|
|
{
|
|
// TODO: send mission goal to move_base_ptr_
|
|
(void)mission;
|
|
}
|