|
| 1 | +// Copyright (c) 2025 Enjoy Robotics |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ |
| 16 | +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ |
| 17 | + |
| 18 | +// Other includes |
| 19 | +#include <string> |
| 20 | +#include <memory> |
| 21 | +#include <map> |
| 22 | + |
| 23 | +// ROS includes |
| 24 | +#include "rclcpp/rclcpp.hpp" |
| 25 | +#include "rclcpp/callback_group.hpp" |
| 26 | +#include "rclcpp/executors/single_threaded_executor.hpp" |
| 27 | +#include "behaviortree_cpp/control_node.h" |
| 28 | +#include "nav2_ros_common/service_server.hpp" |
| 29 | + |
| 30 | +// Interface definitions |
| 31 | +#include "std_srvs/srv/trigger.hpp" |
| 32 | + |
| 33 | + |
| 34 | +namespace nav2_behavior_tree |
| 35 | +{ |
| 36 | + |
| 37 | +using Trigger = std_srvs::srv::Trigger; |
| 38 | + |
| 39 | +enum state_t {RESUMED, PAUSED, PAUSE_REQUESTED, ON_PAUSE, RESUME_REQUESTED, ON_RESUME}; |
| 40 | +const std::map<state_t, std::string> state_names = { |
| 41 | + {RESUMED, "RESUMED"}, |
| 42 | + {PAUSED, "PAUSED"}, |
| 43 | + {PAUSE_REQUESTED, "PAUSE_REQUESTED"}, |
| 44 | + {ON_PAUSE, "ON_PAUSE"}, |
| 45 | + {RESUME_REQUESTED, "RESUME_REQUESTED"}, |
| 46 | + {ON_RESUME, "ON_RESUME"} |
| 47 | +}; |
| 48 | +const std::map<state_t, uint16_t> child_indices = { |
| 49 | + {RESUMED, 0}, |
| 50 | + {PAUSED, 1}, |
| 51 | + {ON_PAUSE, 2}, |
| 52 | + {ON_RESUME, 3} |
| 53 | +}; |
| 54 | + |
| 55 | +/* @brief Controlled through service calls to pause and resume the execution of the tree |
| 56 | + * It has one mandatory child for the RESUMED, and three optional for the PAUSED state, |
| 57 | + * the ON_PAUSE event and the ON_RESUME event. |
| 58 | + * It has two input ports: |
| 59 | + * - pause_service_name: name of the service to pause |
| 60 | + * - resume_service_name: name of the service to resume |
| 61 | + * |
| 62 | + * Usage: |
| 63 | + * <PauseResumeController pause_service_name="/pause" resume_service_name="/resume"> |
| 64 | + * <!-- RESUMED branch --> |
| 65 | + * |
| 66 | + * <!-- PAUSED branch (optional) --> |
| 67 | + * |
| 68 | + * <!-- ON_PAUSE branch (optional) --> |
| 69 | + * |
| 70 | + * <!-- ON_RESUME branch (optional) --> |
| 71 | + * </PauseResumeController> |
| 72 | + * |
| 73 | + * The controller starts in RESUMED state, and ticks it until it returns success. |
| 74 | + * When the pause service is called, ON_PAUSE is ticked until completion, |
| 75 | + * then the controller switches to PAUSED state. |
| 76 | + * When the resume service is called, ON_RESUME is ticked until completion, |
| 77 | + * then the controller switches back to RESUMED state. |
| 78 | + * |
| 79 | + * The controller only returns success when the RESUMED child returns success. |
| 80 | + * The controller returns failure if any child returns failure. |
| 81 | + * In any other case, it returns running. |
| 82 | + */ |
| 83 | + |
| 84 | + |
| 85 | +class PauseResumeController : public BT::ControlNode |
| 86 | +{ |
| 87 | +public: |
| 88 | + //! @brief Constructor |
| 89 | + PauseResumeController( |
| 90 | + const std::string & xml_tag_name, |
| 91 | + const BT::NodeConfiguration & conf); |
| 92 | + |
| 93 | + //! @brief Reset state and go to Idle |
| 94 | + void halt() override; |
| 95 | + |
| 96 | + //! @brief Handle transitions if requested and tick child related to the actual state |
| 97 | + BT::NodeStatus tick() override; |
| 98 | + |
| 99 | + //! @brief Declare ports |
| 100 | + static BT::PortsList providedPorts() |
| 101 | + { |
| 102 | + return { |
| 103 | + BT::InputPort<std::string>( |
| 104 | + "pause_service_name", |
| 105 | + "Name of the service to pause"), |
| 106 | + BT::InputPort<std::string>( |
| 107 | + "resume_service_name", |
| 108 | + "Name of the service to resume"), |
| 109 | + }; |
| 110 | + } |
| 111 | + |
| 112 | + [[nodiscard]] inline state_t getState() const |
| 113 | + { |
| 114 | + return state_; |
| 115 | + } |
| 116 | + |
| 117 | +private: |
| 118 | + //! @brief Set state to PAUSE_REQUESTED |
| 119 | + void pauseServiceCallback( |
| 120 | + const std::shared_ptr<rmw_request_id_t>/*request_header*/, |
| 121 | + const std::shared_ptr<Trigger::Request> request, |
| 122 | + std::shared_ptr<Trigger::Response> response); |
| 123 | + |
| 124 | + //! @brief Set state to RESUME_REQUESTED |
| 125 | + void resumeServiceCallback( |
| 126 | + const std::shared_ptr<rmw_request_id_t>/*request_header*/, |
| 127 | + const std::shared_ptr<Trigger::Request> request, |
| 128 | + std::shared_ptr<Trigger::Response> response); |
| 129 | + |
| 130 | + /** @brief Switch to the next state based on the current state |
| 131 | + * |
| 132 | + * PAUSE_REQUESTED -> ON_PAUSE |
| 133 | + * ON_PAUSE -> PAUSED |
| 134 | + * |
| 135 | + * RESUME_REQUESTED -> ON_RESUME |
| 136 | + * ON_RESUME -> RESUMED |
| 137 | + * |
| 138 | + * Do nothing if in end state |
| 139 | + */ |
| 140 | + void switchToNextState(); |
| 141 | + |
| 142 | + rclcpp::Logger logger_{rclcpp::get_logger("PauseResumeController")}; |
| 143 | + rclcpp::CallbackGroup::SharedPtr cb_group_; |
| 144 | + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; |
| 145 | + nav2::ServiceServer<Trigger>::SharedPtr pause_srv_; |
| 146 | + nav2::ServiceServer<Trigger>::SharedPtr resume_srv_; |
| 147 | + state_t state_; |
| 148 | +}; |
| 149 | + |
| 150 | +} // namespace nav2_behavior_tree |
| 151 | + |
| 152 | +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PAUSE_RESUME_CONTROLLER_HPP_ |
0 commit comments