robot_navigation_watcher.hpp

// Copyright 2026 Intelligent Robotics Lab
//
// This file is part of the project EasyFleet
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef EASYFLEET_NAVIGATION_MANAGER__ROBOT_NAVIGATION_WATCHER_HPP_
#define EASYFLEET_NAVIGATION_MANAGER__ROBOT_NAVIGATION_WATCHER_HPP_
 
#include <cstdint>
#include <string>
 
#include "easynav_interfaces/msg/navigation_control.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
 
namespace easyfleet
{

class RobotNavigationWatcher
{
public:
  RobotNavigationWatcher(
    rclcpp::Node & node, const std::string & robot_id, const std::string & planner_plugin_key);

  [[nodiscard]] const std::string & robot_id() const {return robot_id_;}

  [[nodiscard]] const nav_msgs::msg::Path & get_latest_path() const {return latest_path_;}

  void pause();

  void resume();

  [[nodiscard]] bool is_paused() const {return paused_;}
 
private:
  void send(uint8_t control_type);
  void on_control(easynav_interfaces::msg::NavigationControl::UniquePtr msg);
 
  rclcpp::Node & node_;
  std::string robot_id_;
  std::string id_;
  int64_t seq_ {0};
 
  rclcpp::Publisher<easynav_interfaces::msg::NavigationControl>::SharedPtr control_pub_;
  rclcpp::Subscription<easynav_interfaces::msg::NavigationControl>::SharedPtr control_sub_;
  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
 
  nav_msgs::msg::Path latest_path_;
  bool paused_ {false};
};
 
}  // namespace easyfleet
 
#endif  // EASYFLEET_NAVIGATION_MANAGER__ROBOT_NAVIGATION_WATCHER_HPP_