pause_resume_controller.hpp

// Copyright (c) 2025 Enjoy Robotics
//
// 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 NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_
 
// Other includes
#include <string>
#include <memory>
#include <map>
 
// ROS includes
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/callback_group.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "behaviortree_cpp/control_node.h"
#include "nav2_ros_common/service_server.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
 
// Interface definitions
#include "std_srvs/srv/trigger.hpp"
 
 
namespace nav2_behavior_tree
{
 
using Trigger = std_srvs::srv::Trigger;
 
enum state_t {RESUMED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME};
const std::map<state_t, std::string> state_names = {
  {RESUMED, "RESUMED"},
  {PAUSED, "PAUSED"},
  {PAUSE_REQUESTED, "PAUSE_REQUESTED"},
  {ON_PAUSE, "ON_PAUSE"},
  {RESUME_REQUESTED, "RESUME_REQUESTED"},
  {ON_RESUME, "ON_RESUME"}
};
const std::map<state_t, uint16_t> child_indices = {
  {RESUMED, 0},
  {PAUSED, 1},
  {ON_PAUSE, 2},
  {ON_RESUME, 3}
};
 
class PauseResumeController : public BT::ControlNode
{
public:
  PauseResumeController(
    const std::string & xml_tag_name,
    const BT::NodeConfiguration & conf);
 
  void halt() override;
 
  BT::NodeStatus tick() override;
 
  static BT::PortsList providedPorts()
  {
    return {
      BT::InputPort<std::string>(
        "pause_service_name",
        "Name of the service to pause"),
      BT::InputPort<std::string>(
        "resume_service_name",
        "Name of the service to resume"),
    };
  }
 
  [[nodiscard]] inline state_t getState() const
  {
    return state_;
  }
 
private:
  void pauseServiceCallback(
    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
    const std::shared_ptr<Trigger::Request> request,
    std::shared_ptr<Trigger::Response> response);
 
  void resumeServiceCallback(
    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
    const std::shared_ptr<Trigger::Request> request,
    std::shared_ptr<Trigger::Response> response);
 
  void switchToNextState();
 
  rclcpp::Logger logger_{rclcpp::get_logger("PauseResumeController")};
  rclcpp::CallbackGroup::SharedPtr cb_group_;
  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
  nav2::ServiceServer<Trigger>::SharedPtr pause_srv_;
  nav2::ServiceServer<Trigger>::SharedPtr resume_srv_;
  state_t state_;
};
 
}  // namespace nav2_behavior_tree
 
#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_