diff --git a/CMakeLists.txt b/CMakeLists.txt index 9516de1aa..04854e740 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,6 +185,7 @@ list(APPEND BT_SOURCE src/controls/try_catch_node.cpp src/controls/while_do_else_node.cpp + src/loggers/abstract_logger.cpp src/loggers/bt_cout_logger.cpp src/loggers/bt_file_logger_v2.cpp src/loggers/bt_minitrace_logger.cpp diff --git a/include/behaviortree_cpp/loggers/abstract_logger.h b/include/behaviortree_cpp/loggers/abstract_logger.h index aea611a5f..1f74765e6 100644 --- a/include/behaviortree_cpp/loggers/abstract_logger.h +++ b/include/behaviortree_cpp/loggers/abstract_logger.h @@ -4,6 +4,8 @@ #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/bt_factory.h" +#include + namespace BT { enum class TimestampType @@ -18,7 +20,7 @@ class StatusChangeLogger /// Construct and immediately subscribe to status changes. StatusChangeLogger(TreeNode* root_node); - virtual ~StatusChangeLogger() = default; + virtual ~StatusChangeLogger(); StatusChangeLogger(const StatusChangeLogger& other) = delete; StatusChangeLogger& operator=(const StatusChangeLogger& other) = delete; @@ -31,87 +33,38 @@ class StatusChangeLogger virtual void flush() = 0; - void setEnabled(bool enabled) - { - enabled_ = enabled; - } + void setEnabled(bool enabled); - void setTimestampType(TimestampType type) - { - type_ = type; - } + void setTimestampType(TimestampType type); - bool enabled() const - { - return enabled_; - } + bool enabled() const; // false by default. - bool showsTransitionToIdle() const - { - return show_transition_to_idle_; - } + bool showsTransitionToIdle() const; - void enableTransitionToIdle(bool enable) - { - show_transition_to_idle_ = enable; - } + void enableTransitionToIdle(bool enable); protected: /// Default constructor for deferred subscription. Call subscribeToTreeChanges() when ready. - StatusChangeLogger() = default; + StatusChangeLogger(); /// Subscribe to status changes. Call at end of constructor for deferred subscription. void subscribeToTreeChanges(TreeNode* root_node); + /// Stop new callbacks and wait until callbacks already in progress have completed. + /// Derived destructors must call this before destroying state used by callback(). + void unsubscribeFromTreeChanges(); + private: - bool enabled_ = true; - bool show_transition_to_idle_ = true; - std::vector subscribers_; - TimestampType type_ = TimestampType::absolute; - BT::TimePoint first_timestamp_ = {}; - std::mutex callback_mutex_; + /// Forward a status transition to callback(), unless disabled. + void handleStatusChange(TimePoint timestamp, const TreeNode& node, NodeStatus prev, + NodeStatus status); + + // All state lives behind this pointer, so that changing it does not alter the + // layout that derived classes (including user-defined loggers) compile against. + struct PImpl; + std::unique_ptr _p; }; - -//-------------------------------------------- - -inline StatusChangeLogger::StatusChangeLogger(TreeNode* root_node) -{ - subscribeToTreeChanges(root_node); -} - -inline void StatusChangeLogger::subscribeToTreeChanges(TreeNode* root_node) -{ - first_timestamp_ = std::chrono::high_resolution_clock::now(); - - auto subscribeCallback = [this](TimePoint timestamp, const TreeNode& node, - NodeStatus prev, NodeStatus status) { - // Copy state under lock, then release before calling user code - // This prevents recursive mutex locking when multiple nodes change status - bool should_callback = false; - Duration adjusted_timestamp; - { - std::unique_lock lk(callback_mutex_); - if(enabled_ && (status != NodeStatus::IDLE || show_transition_to_idle_)) - { - should_callback = true; - adjusted_timestamp = (type_ == TimestampType::absolute) ? - timestamp.time_since_epoch() : - (timestamp - first_timestamp_); - } - } - if(should_callback) - { - this->callback(adjusted_timestamp, node, prev, status); - } - }; - - auto visitor = [this, subscribeCallback](TreeNode* node) { - subscribers_.push_back(node->subscribeToStatusChange(std::move(subscribeCallback))); - }; - - applyRecursiveVisitor(root_node, visitor); -} } // namespace BT #endif // ABSTRACT_LOGGER_H diff --git a/include/behaviortree_cpp/utils/callback_gate.h b/include/behaviortree_cpp/utils/callback_gate.h new file mode 100644 index 000000000..8209bacba --- /dev/null +++ b/include/behaviortree_cpp/utils/callback_gate.h @@ -0,0 +1,87 @@ +#pragma once + +#include +#include +#include + +namespace BT::details +{ + +/** + * @brief Synchronization gate used to coordinate callbacks with object teardown. + * + * Callbacks enter the gate with tryStart() and must signal completion when done + * (use Guard for RAII). During teardown, call close() to reject new callbacks + * and closeAndDrain() to block until the callbacks still running have completed. + */ +class CallbackGate +{ +public: + using Ptr = std::shared_ptr; + + /// RAII helper that signals the completion of a callback entered with tryStart() + class Guard + { + public: + explicit Guard(Ptr gate) : gate_(std::move(gate)) + {} + + Guard(const Guard&) = delete; + Guard& operator=(const Guard&) = delete; + Guard(Guard&&) = delete; + Guard& operator=(Guard&&) = delete; + + ~Guard() + { + gate_->completed(); + } + + private: + Ptr gate_; + }; + + /// Returns false if the gate has been closed and the callback must not run. + bool tryStart() + { + const std::lock_guard lk(mutex_); + if(!accepting_callbacks_) + { + return false; + } + running_callbacks_++; + return true; + } + + /// Reject new callbacks, without waiting for the running ones. + void close() + { + const std::lock_guard lk(mutex_); + accepting_callbacks_ = false; + } + + /// Reject new callbacks and wait until callbacks already running have completed. + void closeAndDrain() + { + std::unique_lock lk(mutex_); + accepting_callbacks_ = false; + condition_variable_.wait(lk, [this] { return running_callbacks_ == 0; }); + } + +private: + void completed() + { + const std::lock_guard lk(mutex_); + running_callbacks_--; + if(running_callbacks_ == 0 && !accepting_callbacks_) + { + condition_variable_.notify_all(); + } + } + + std::mutex mutex_; + std::condition_variable condition_variable_; + bool accepting_callbacks_ = true; + size_t running_callbacks_ = 0; +}; + +} // namespace BT::details diff --git a/src/loggers/abstract_logger.cpp b/src/loggers/abstract_logger.cpp new file mode 100644 index 000000000..899a36c24 --- /dev/null +++ b/src/loggers/abstract_logger.cpp @@ -0,0 +1,117 @@ +#include "behaviortree_cpp/loggers/abstract_logger.h" + +#include "behaviortree_cpp/utils/callback_gate.h" + +#include + +namespace BT +{ + +struct StatusChangeLogger::PImpl +{ + bool enabled = true; + bool show_transition_to_idle = true; + std::vector subscribers; + TimestampType type = TimestampType::absolute; + BT::TimePoint first_timestamp = {}; + std::mutex callback_mutex; + details::CallbackGate::Ptr callback_gate = std::make_shared(); +}; + +StatusChangeLogger::StatusChangeLogger() : _p(std::make_unique()) +{} + +StatusChangeLogger::StatusChangeLogger(TreeNode* root_node) : StatusChangeLogger() +{ + subscribeToTreeChanges(root_node); +} + +StatusChangeLogger::~StatusChangeLogger() +{ + unsubscribeFromTreeChanges(); +} + +void StatusChangeLogger::setEnabled(bool enabled) +{ + const std::lock_guard lk(_p->callback_mutex); + _p->enabled = enabled; +} + +void StatusChangeLogger::setTimestampType(TimestampType type) +{ + const std::lock_guard lk(_p->callback_mutex); + _p->type = type; +} + +bool StatusChangeLogger::enabled() const +{ + const std::lock_guard lk(_p->callback_mutex); + return _p->enabled; +} + +bool StatusChangeLogger::showsTransitionToIdle() const +{ + const std::lock_guard lk(_p->callback_mutex); + return _p->show_transition_to_idle; +} + +void StatusChangeLogger::enableTransitionToIdle(bool enable) +{ + const std::lock_guard lk(_p->callback_mutex); + _p->show_transition_to_idle = enable; +} + +void StatusChangeLogger::subscribeToTreeChanges(TreeNode* root_node) +{ + _p->first_timestamp = std::chrono::high_resolution_clock::now(); + + auto subscribeCallback = + [this, gate = _p->callback_gate](TimePoint timestamp, const TreeNode& node, + NodeStatus prev, NodeStatus status) { + if(!gate->tryStart()) + { + return; + } + const details::CallbackGate::Guard callback_guard(gate); + handleStatusChange(timestamp, node, prev, status); + }; + + auto visitor = [this, subscribeCallback](TreeNode* node) { + _p->subscribers.push_back( + node->subscribeToStatusChange(std::move(subscribeCallback))); + }; + + applyRecursiveVisitor(root_node, visitor); +} + +void StatusChangeLogger::handleStatusChange(TimePoint timestamp, const TreeNode& node, + NodeStatus prev, NodeStatus status) +{ + // Copy state under lock, then release before calling user code + // This prevents recursive mutex locking when multiple nodes change status + bool should_callback = false; + Duration adjusted_timestamp; + { + std::unique_lock lk(_p->callback_mutex); + if(_p->enabled && (status != NodeStatus::IDLE || _p->show_transition_to_idle)) + { + should_callback = true; + adjusted_timestamp = (_p->type == TimestampType::absolute) ? + timestamp.time_since_epoch() : + (timestamp - _p->first_timestamp); + } + } + + if(should_callback) + { + this->callback(adjusted_timestamp, node, prev, status); + } +} + +void StatusChangeLogger::unsubscribeFromTreeChanges() +{ + _p->callback_gate->closeAndDrain(); + _p->subscribers.clear(); +} + +} // namespace BT diff --git a/src/loggers/bt_cout_logger.cpp b/src/loggers/bt_cout_logger.cpp index c08ba7475..0a1c3671a 100644 --- a/src/loggers/bt_cout_logger.cpp +++ b/src/loggers/bt_cout_logger.cpp @@ -5,7 +5,11 @@ namespace BT StdCoutLogger::StdCoutLogger(const BT::Tree& tree) : StatusChangeLogger(tree.rootNode()) {} -StdCoutLogger::~StdCoutLogger() = default; +StdCoutLogger::~StdCoutLogger() +{ + // Stop status callbacks while the derived object is still intact. + unsubscribeFromTreeChanges(); +} void StdCoutLogger::callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status, NodeStatus status) diff --git a/src/loggers/bt_file_logger_v2.cpp b/src/loggers/bt_file_logger_v2.cpp index b0547d2de..845d37da9 100644 --- a/src/loggers/bt_file_logger_v2.cpp +++ b/src/loggers/bt_file_logger_v2.cpp @@ -94,6 +94,9 @@ FileLogger2::FileLogger2(const BT::Tree& tree, std::filesystem::path const& file FileLogger2::~FileLogger2() { + // Stop status callbacks before tearing down the state they use. + unsubscribeFromTreeChanges(); + _p->loop = false; _p->queue_cv.notify_one(); _p->writer_thread.join(); diff --git a/src/loggers/bt_minitrace_logger.cpp b/src/loggers/bt_minitrace_logger.cpp index 5b53a68a0..8f1fa25c8 100644 --- a/src/loggers/bt_minitrace_logger.cpp +++ b/src/loggers/bt_minitrace_logger.cpp @@ -17,6 +17,9 @@ MinitraceLogger::MinitraceLogger(const Tree& tree, const char* filename_json) MinitraceLogger::~MinitraceLogger() { + // Stop status callbacks before shutting down minitrace. + unsubscribeFromTreeChanges(); + mtr_flush(); mtr_shutdown(); } diff --git a/src/loggers/bt_observer.cpp b/src/loggers/bt_observer.cpp index bbe3caa50..76aaeebea 100644 --- a/src/loggers/bt_observer.cpp +++ b/src/loggers/bt_observer.cpp @@ -45,7 +45,10 @@ TreeObserver::TreeObserver(const BT::Tree& tree) : StatusChangeLogger(tree.rootN } TreeObserver::~TreeObserver() -{} +{ + // Stop status callbacks before the statistics map is destroyed. + unsubscribeFromTreeChanges(); +} void TreeObserver::callback(Duration timestamp, const TreeNode& node, NodeStatus /*prev_status*/, NodeStatus status) diff --git a/src/loggers/bt_sqlite_logger.cpp b/src/loggers/bt_sqlite_logger.cpp index 89cff98b1..f5960dbfa 100644 --- a/src/loggers/bt_sqlite_logger.cpp +++ b/src/loggers/bt_sqlite_logger.cpp @@ -140,6 +140,9 @@ SqliteLogger::SqliteLogger(const Tree& tree, std::filesystem::path const& filepa SqliteLogger::~SqliteLogger() { + // Stop status callbacks before tearing down the state they use. + unsubscribeFromTreeChanges(); + try { loop_ = false; diff --git a/src/loggers/groot2_publisher.cpp b/src/loggers/groot2_publisher.cpp index 4726c1a7b..fcbf89fb0 100644 --- a/src/loggers/groot2_publisher.cpp +++ b/src/loggers/groot2_publisher.cpp @@ -3,6 +3,7 @@ #include "zmq_addon.hpp" #include "behaviortree_cpp/loggers/groot2_protocol.h" +#include "behaviortree_cpp/utils/callback_gate.h" #include "behaviortree_cpp/xml_parsing.h" #include @@ -73,6 +74,9 @@ struct Groot2Publisher::PImpl publisher.set(zmq::sockopt::sndtimeo, timeout_ms); } + /// Body of the pre/post tick callback installed by insertHook(). + NodeStatus runHook(const Monitor::Hook::Ptr& hook, TreeNode& node); + unsigned server_port = 0; std::string server_address; std::string publisher_address; @@ -97,13 +101,16 @@ struct Groot2Publisher::PImpl std::unordered_map post_hooks; std::mutex last_heartbeat_mutex; - std::chrono::steady_clock::time_point last_heartbeat; + std::chrono::steady_clock::time_point last_heartbeat = std::chrono::steady_clock::now(); std::chrono::milliseconds max_heartbeat_delay = std::chrono::milliseconds(5000); - std::atomic_bool recording = false; + bool recording = false; std::deque transitions_buffer; std::chrono::microseconds recording_fist_time{}; + details::CallbackGate::Ptr hook_gate = std::make_shared(); + std::mutex publisher_mutex; + std::thread heartbeat_thread; zmq::context_t context; @@ -112,7 +119,7 @@ struct Groot2Publisher::PImpl }; Groot2Publisher::Groot2Publisher(const BT::Tree& tree, unsigned server_port) - : StatusChangeLogger(tree.rootNode()), _p(new PImpl()) + : StatusChangeLogger(), _p(std::make_unique()) { _p->server_port = server_port; _p->tree_xml = WriteTreeToXML(tree, true, true); @@ -153,20 +160,31 @@ Groot2Publisher::Groot2Publisher(const BT::Tree& tree, unsigned server_port) _p->server_thread = std::thread(&Groot2Publisher::serverLoop, this); _p->heartbeat_thread = std::thread(&Groot2Publisher::heartbeatLoop, this); + + // Subscribe only after all state used by callback() has been initialized. + subscribeToTreeChanges(tree.rootNode()); } void Groot2Publisher::setMaxHeartbeatDelay(std::chrono::milliseconds delay) { + const std::lock_guard lk(_p->last_heartbeat_mutex); _p->max_heartbeat_delay = delay; } std::chrono::milliseconds Groot2Publisher::maxHeartbeatDelay() const { + const std::lock_guard lk(_p->last_heartbeat_mutex); return _p->max_heartbeat_delay; } Groot2Publisher::~Groot2Publisher() { + // Stop status callbacks before tearing down any state they access. + unsubscribeFromTreeChanges(); + + // Prevent new hook callbacks from starting. + _p->hook_gate->close(); + // First, signal threads to stop _p->active_server = false; @@ -186,8 +204,10 @@ Groot2Publisher::~Groot2Publisher() _p->heartbeat_thread.join(); } - // Remove hooks after threads are stopped to avoid race conditions + // Remove installed hooks, waking callbacks currently blocked at breakpoints, + // then wait for the callbacks still running to complete. removeAllHooks(); + _p->hook_gate->closeAndDrain(); flush(); @@ -237,18 +257,19 @@ void Groot2Publisher::serverLoop() auto& socket = _p->server; auto sendErrorReply = [&socket](const std::string& msg) { - zmq::multipart_t error_msg; - error_msg.addstr("error"); - error_msg.addstr(msg); - error_msg.send(socket); + try + { + zmq::multipart_t error_msg; + error_msg.addstr("error"); + error_msg.addstr(msg); + return error_msg.send(socket); + } + catch(const zmq::error_t&) + { + return false; + } }; - // initialize _p->last_heartbeat - { - const std::unique_lock lk(_p->last_heartbeat_mutex); - _p->last_heartbeat = std::chrono::steady_clock::now(); - } - while(_p->active_server) { zmq::multipart_t requestMsg; @@ -288,206 +309,238 @@ void Groot2Publisher::serverLoop() zmq::multipart_t reply_msg; reply_msg.addstr(Monitor::SerializeHeader(reply_header)); - switch(request_header.type) + try { - case Monitor::RequestType::FULLTREE: { - reply_msg.addstr(_p->tree_xml); - } - break; - - case Monitor::RequestType::STATUS: { - const std::unique_lock lk(_p->status_mutex); - reply_msg.addstr(_p->status_buffer); - } - break; - - case Monitor::RequestType::BLACKBOARD: { - if(requestMsg.size() != 2) - { - sendErrorReply("must be 2 parts message"); - continue; + switch(request_header.type) + { + case Monitor::RequestType::FULLTREE: { + reply_msg.addstr(_p->tree_xml); } - std::string const bb_names_str = requestMsg[1].to_string(); - auto msg = generateBlackboardsDump(bb_names_str); - reply_msg.addmem(msg.data(), msg.size()); - } - break; + break; - case Monitor::RequestType::HOOK_INSERT: { - if(requestMsg.size() != 2) - { - sendErrorReply("must be 2 parts message"); - continue; + case Monitor::RequestType::STATUS: { + const std::unique_lock lk(_p->status_mutex); + reply_msg.addstr(_p->status_buffer); } + break; - auto InsertHook = [this](nlohmann::json const& json) { - uint16_t const node_uid = json["uid"].get(); - Position const pos = static_cast(json["position"].get()); + case Monitor::RequestType::BLACKBOARD: { + if(requestMsg.size() != 2) + { + sendErrorReply("must be 2 parts message"); + continue; + } + std::string const bb_names_str = requestMsg[1].to_string(); + auto msg = generateBlackboardsDump(bb_names_str); + reply_msg.addmem(msg.data(), msg.size()); + } + break; - if(auto hook = getHook(pos, node_uid)) + case Monitor::RequestType::HOOK_INSERT: { + if(requestMsg.size() != 2) { - std::unique_lock lk(hook->mutex); - const bool was_interactive = (hook->mode == Monitor::Hook::Mode::BREAKPOINT); - BT::Monitor::from_json(json, *hook); + sendErrorReply("must be 2 parts message"); + continue; + } + + auto InsertHook = [this](nlohmann::json const& json) { + uint16_t const node_uid = json["uid"].get(); + Position const pos = static_cast(json["position"].get()); + + if(auto hook = getHook(pos, node_uid)) + { + std::unique_lock lk(hook->mutex); + const bool was_interactive = + (hook->mode == Monitor::Hook::Mode::BREAKPOINT); + BT::Monitor::from_json(json, *hook); + + // if it WAS interactive and it is not anymore, unlock it + if(was_interactive && (hook->mode == Monitor::Hook::Mode::REPLACE)) + { + hook->ready = true; + lk.unlock(); + hook->wakeup.notify_all(); + } + } + else // if not found, create a new one + { + auto new_hook = std::make_shared(); + BT::Monitor::from_json(json, *new_hook); + insertHook(new_hook); + } + }; - // if it WAS interactive and it is not anymore, unlock it - if(was_interactive && (hook->mode == Monitor::Hook::Mode::REPLACE)) + auto const received_json = nlohmann::json::parse(requestMsg[1].to_string()); + + // the json may contain a Hook or an array of Hooks + if(received_json.is_array()) + { + for(auto const& json : received_json) { - hook->ready = true; - lk.unlock(); - hook->wakeup.notify_all(); + InsertHook(json); } } - else // if not found, create a new one + else { - auto new_hook = std::make_shared(); - BT::Monitor::from_json(json, *new_hook); - insertHook(new_hook); + InsertHook(received_json); } - }; - - auto const received_json = nlohmann::json::parse(requestMsg[1].to_string()); + } + break; - // the json may contain a Hook or an array of Hooks - if(received_json.is_array()) - { - for(auto const& json : received_json) + case Monitor::RequestType::BREAKPOINT_UNLOCK: { + if(requestMsg.size() != 2) { - InsertHook(json); + sendErrorReply("must be 2 parts message"); + continue; } - } - else - { - InsertHook(received_json); - } - } - break; - case Monitor::RequestType::BREAKPOINT_UNLOCK: { - if(requestMsg.size() != 2) - { - sendErrorReply("must be 2 parts message"); - continue; - } + auto json = nlohmann::json::parse(requestMsg[1].to_string()); + const uint16_t node_uid = json.at("uid").get(); + const std::string status_str = json.at("desired_status").get(); + auto position = static_cast(json.at("position").get()); + const bool remove = json.at("remove_when_done").get(); - auto json = nlohmann::json::parse(requestMsg[1].to_string()); - const uint16_t node_uid = json.at("uid").get(); - const std::string status_str = json.at("desired_status").get(); - auto position = static_cast(json.at("position").get()); - const bool remove = json.at("remove_when_done").get(); + NodeStatus desired_status = NodeStatus::SKIPPED; + if(status_str == "SUCCESS") + { + desired_status = NodeStatus::SUCCESS; + } + else if(status_str == "FAILURE") + { + desired_status = NodeStatus::FAILURE; + } - NodeStatus desired_status = NodeStatus::SKIPPED; - if(status_str == "SUCCESS") - { - desired_status = NodeStatus::SUCCESS; + if(!unlockBreakpoint(position, node_uid, desired_status, remove)) + { + sendErrorReply("Node ID not found"); + continue; + } } - else if(status_str == "FAILURE") - { - desired_status = NodeStatus::FAILURE; + break; + + case Monitor::RequestType::REMOVE_ALL_HOOKS: { + removeAllHooks(); } + break; - if(!unlockBreakpoint(position, node_uid, desired_status, remove)) - { - sendErrorReply("Node ID not found"); - continue; + case Monitor::RequestType::DISABLE_ALL_HOOKS: { + enableAllHooks(false); } - } - break; + break; - case Monitor::RequestType::REMOVE_ALL_HOOKS: { - removeAllHooks(); - } - break; + case Monitor::RequestType::HOOK_REMOVE: { + if(requestMsg.size() != 2) + { + sendErrorReply("must be 2 parts message"); + continue; + } - case Monitor::RequestType::DISABLE_ALL_HOOKS: { - enableAllHooks(false); - } - break; + auto json = nlohmann::json::parse(requestMsg[1].to_string()); + const uint16_t node_uid = json.at("uid").get(); + auto position = static_cast(json.at("position").get()); - case Monitor::RequestType::HOOK_REMOVE: { - if(requestMsg.size() != 2) - { - sendErrorReply("must be 2 parts message"); - continue; + if(!removeHook(position, node_uid)) + { + sendErrorReply("Node ID not found"); + continue; + } } + break; - auto json = nlohmann::json::parse(requestMsg[1].to_string()); - const uint16_t node_uid = json.at("uid").get(); - auto position = static_cast(json.at("position").get()); - - if(!removeHook(position, node_uid)) - { - sendErrorReply("Node ID not found"); - continue; + case Monitor::RequestType::HOOKS_DUMP: { + auto json_out = nlohmann::json::array(); + const std::unique_lock map_lk(_p->hooks_map_mutex); + for(const auto* hooks : { &_p->pre_hooks, &_p->post_hooks }) + { + for(const auto& entry : *hooks) + { + const auto& hook = entry.second; + const std::unique_lock lk(hook->mutex); + json_out.push_back(*hook); + } + } + reply_msg.addstr(json_out.dump()); } - } - break; + break; - case Monitor::RequestType::HOOKS_DUMP: { - const std::unique_lock lk(_p->hooks_map_mutex); - auto json_out = nlohmann::json::array(); - for(const auto& [node_uid, breakpoint] : _p->pre_hooks) - { - std::ignore = node_uid; // unused in this loop - json_out.push_back(*breakpoint); - } - reply_msg.addstr(json_out.dump()); - } - break; + case Monitor::RequestType::TOGGLE_RECORDING: { + if(requestMsg.size() != 2) + { + sendErrorReply("must be 2 parts message"); + continue; + } - case Monitor::RequestType::TOGGLE_RECORDING: { - if(requestMsg.size() != 2) - { - sendErrorReply("must be 2 parts message"); - continue; + auto const cmd = (requestMsg[1].to_string()); + if(cmd == "start") + { + { + const std::unique_lock lk(_p->status_mutex); + // Configure all recording state before callbacks can observe it. + _p->recording_fist_time = + std::chrono::duration_cast( + std::chrono::high_resolution_clock::now().time_since_epoch()); + _p->transitions_buffer.clear(); + _p->recording = true; + } + // to send consistent time for client + auto now = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + reply_msg.addstr(std::to_string(now.count())); + } + else if(cmd == "stop") + { + const std::unique_lock lk(_p->status_mutex); + _p->recording = false; + } } + break; - auto const cmd = (requestMsg[1].to_string()); - if(cmd == "start") - { - _p->recording = true; - // to keep the first time for callback - _p->recording_fist_time = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now().time_since_epoch()); - // to send consistent time for client - auto now = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - reply_msg.addstr(std::to_string(now.count())); - const std::unique_lock lk(_p->status_mutex); - _p->transitions_buffer.clear(); - } - else if(cmd == "stop") - { - _p->recording = false; + case Monitor::RequestType::GET_TRANSITIONS: { + // Move the transitions out, then serialize without holding the mutex + // that the status callback contends on. + std::deque transitions; + { + const std::unique_lock lk(_p->status_mutex); + std::swap(transitions, _p->transitions_buffer); + } + thread_local std::string trans_buffer; + trans_buffer.resize(9 * transitions.size()); + size_t offset = 0; + for(const auto& trans : transitions) + { + std::memcpy(&trans_buffer[offset], &trans.timestamp_usec, 6); + offset += 6; + std::memcpy(&trans_buffer[offset], &trans.node_uid, 2); + offset += 2; + std::memcpy(&trans_buffer[offset], &trans.status, 1); + offset += 1; + } + trans_buffer.resize(offset); + reply_msg.addstr(trans_buffer); } - } - break; + break; - case Monitor::RequestType::GET_TRANSITIONS: { - thread_local std::string trans_buffer; - trans_buffer.resize(9 * _p->transitions_buffer.size()); - - const std::unique_lock lk(_p->status_mutex); - size_t offset = 0; - for(const auto& trans : _p->transitions_buffer) - { - std::memcpy(&trans_buffer[offset], &trans.timestamp_usec, 6); - offset += 6; - std::memcpy(&trans_buffer[offset], &trans.node_uid, 2); - offset += 2; - std::memcpy(&trans_buffer[offset], &trans.status, 1); - offset += 1; + default: { + sendErrorReply("Request not recognized"); + continue; } - _p->transitions_buffer.clear(); - trans_buffer.resize(offset); - reply_msg.addstr(trans_buffer); } - break; - - default: { - sendErrorReply("Request not recognized"); - continue; + } + catch(const std::exception& ex) + { + if(!sendErrorReply(ex.what())) + { + break; } + continue; + } + catch(...) + { + if(!sendErrorReply("Unknown error while processing request")) + { + break; + } + continue; } // send the reply try @@ -504,17 +557,22 @@ void Groot2Publisher::serverLoop() void BT::Groot2Publisher::enableAllHooks(bool enable) { - const std::unique_lock lk(_p->hooks_map_mutex); - for(const auto& [node_uid, hook] : _p->pre_hooks) + // Holding hooks_map_mutex while locking each hook is safe: all code paths + // acquire hooks_map_mutex before hook->mutex, never the other way around. + const std::unique_lock map_lk(_p->hooks_map_mutex); + for(const auto* hooks : { &_p->pre_hooks, &_p->post_hooks }) { - std::ignore = node_uid; // unused in this loop - std::unique_lock lk(hook->mutex); - hook->enabled = enable; - // when disabling, remember to wake up blocked ones - if(!hook->enabled && hook->mode == Monitor::Hook::Mode::BREAKPOINT) + for(const auto& entry : *hooks) { - lk.unlock(); - hook->wakeup.notify_all(); + const auto& hook = entry.second; + std::unique_lock lk(hook->mutex); + hook->enabled = enable; + // when disabling, remember to wake up blocked ones + if(!hook->enabled && hook->mode == Monitor::Hook::Mode::BREAKPOINT) + { + lk.unlock(); + hook->wakeup.notify_all(); + } } } } @@ -578,49 +636,108 @@ bool Groot2Publisher::insertHook(std::shared_ptr hook) return false; } - auto injectedCallback = [hook, this](TreeNode& node) -> NodeStatus { - std::unique_lock lk(hook->mutex); + auto injectedCallback = [hook, this, gate = _p->hook_gate](TreeNode& node) { + if(!gate->tryStart()) + { + return NodeStatus::SKIPPED; + } + const details::CallbackGate::Guard callback_guard(gate); + return _p->runHook(hook, node); + }; + + const std::unique_lock lk(_p->hooks_map_mutex); + if(hook->position == Position::PRE) + { + _p->pre_hooks[node_uid] = hook; + node->setPreTickFunction(injectedCallback); + } + else + { + _p->post_hooks[node_uid] = hook; + node->setPostTickFunction([callback = std::move(injectedCallback)]( + TreeNode& node, NodeStatus) { return callback(node); }); + } + + return true; +} + +NodeStatus Groot2Publisher::PImpl::runHook(const Monitor::Hook::Ptr& hook, TreeNode& node) +{ + { + const std::unique_lock lk(hook->mutex); if(!hook->enabled) { return NodeStatus::SKIPPED; } + } - // Notify that a breakpoint was reached, using the _p->publisher + // Notify that a breakpoint was reached, using the publisher socket. + // ZeroMQ sockets must not be used concurrently from multiple callbacks. + try + { + const std::lock_guard publisher_lk(publisher_mutex); const Monitor::RequestHeader breakpoint_request(Monitor::BREAKPOINT_REACHED); zmq::multipart_t request_msg; request_msg.addstr(Monitor::SerializeHeader(breakpoint_request)); request_msg.addstr(std::to_string(hook->node_uid)); - request_msg.send(_p->publisher); + request_msg.send(publisher); + } + catch(const zmq::error_t&) + { + return NodeStatus::SKIPPED; + } - // wait until someone wake us up + bool remove_when_done = false; + NodeStatus desired_status = NodeStatus::SKIPPED; + Position position = Position::PRE; + { + std::unique_lock lk(hook->mutex); + // wait until someone wakes us up. The predicate makes this robust to the + // hook being unlocked or disabled before the wait starts. if(hook->mode == Monitor::Hook::Mode::BREAKPOINT) { hook->wakeup.wait(lk, [hook]() { return hook->ready || !hook->enabled; }); - hook->ready = false; - // wait was unblocked but it could be the breakpoint becoming disabled. - // in this case, just skip - if(!hook->enabled) - { - return NodeStatus::SKIPPED; - } } - - if(hook->remove_when_done) + // skip if the hook was disabled, either before the notification + // or to unblock the wait above. + if(!hook->enabled) { - // self-destruction at the end of this lambda function - const std::unique_lock lk(_p->hooks_map_mutex); - _p->pre_hooks.erase(hook->node_uid); - node.setPreTickFunction({}); + return NodeStatus::SKIPPED; } - return hook->desired_status; - }; - const std::unique_lock lk(_p->hooks_map_mutex); - _p->pre_hooks[node_uid] = hook; - node->setPreTickFunction(injectedCallback); + remove_when_done = hook->remove_when_done; + desired_status = hook->desired_status; + position = hook->position; + if(remove_when_done) + { + // A TreeNode may already have copied this callback before it is removed. + // Keep such stale copies from executing the one-shot hook again. + hook->enabled = false; + } + } - return true; + if(remove_when_done) + { + // The hook mutex is deliberately released before taking hooks_map_mutex, + // to keep a single lock order (hooks_map_mutex first). + const std::unique_lock lk(hooks_map_mutex); + auto* hooks = position == Position::PRE ? &pre_hooks : &post_hooks; + auto hook_it = hooks->find(hook->node_uid); + if(hook_it != hooks->end() && hook_it->second == hook) + { + hooks->erase(hook_it); + if(position == Position::PRE) + { + node.setPreTickFunction({}); + } + else + { + node.setPostTickFunction({}); + } + } + } + return desired_status; } bool Groot2Publisher::unlockBreakpoint(Position pos, uint16_t node_uid, NodeStatus result, @@ -670,27 +787,41 @@ bool Groot2Publisher::removeHook(Position pos, uint16_t node_uid) return false; } - auto hook = getHook(pos, node_uid); - if(!hook) - { - return false; - } - + Monitor::Hook::Ptr hook; + bool notify_breakpoint = false; { - const std::unique_lock lk(_p->hooks_map_mutex); - _p->pre_hooks.erase(node_uid); - } - node->setPreTickFunction({}); + const std::unique_lock lk(_p->hooks_map_mutex); + auto* hooks = pos == Position::PRE ? &_p->pre_hooks : &_p->post_hooks; + auto hook_it = hooks->find(node_uid); + if(hook_it == hooks->end()) + { + return false; + } + hook = hook_it->second; - // Disable breakpoint, if it was interactive and blocked - { - std::unique_lock lk(hook->mutex); - if(hook->mode == Monitor::Hook::Mode::BREAKPOINT) + // Disable before erasing: stale copies of the injected callback check + // this flag, and an erased hook can never be re-enabled. { + const std::unique_lock hook_lk(hook->mutex); hook->enabled = false; - lk.unlock(); - hook->wakeup.notify_all(); + notify_breakpoint = hook->mode == Monitor::Hook::Mode::BREAKPOINT; + } + + hooks->erase(hook_it); + if(pos == Position::PRE) + { + node->setPreTickFunction({}); } + else + { + node->setPostTickFunction({}); + } + } + + // Wake the hook if it is blocked at an interactive breakpoint. + if(notify_breakpoint) + { + hook->wakeup.notify_all(); } return true; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index a009ee5ea..48294626a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -62,6 +62,17 @@ set(BT_TESTS test_helper.hpp ) +# Functional coverage for the real Groot2 ZMQ protocol. +if(BTCPP_GROOT_INTERFACE AND NOT WIN32) + list(APPEND BT_TESTS gtest_groot2_publisher_integration.cpp) +endif() + +# These stress tests are useful only with TSan instrumentation, where the +# otherwise invisible data races and lock-order inversions are reported. +if(BTCPP_GROOT_INTERFACE AND BTCPP_ENABLE_TSAN AND NOT WIN32) + list(APPEND BT_TESTS gtest_groot2_publisher_thread_safety.cpp) +endif() + if(ament_cmake_FOUND) find_package(ament_cmake_gtest REQUIRED) diff --git a/tests/groot2_test_utils.hpp b/tests/groot2_test_utils.hpp new file mode 100644 index 000000000..8362f1264 --- /dev/null +++ b/tests/groot2_test_utils.hpp @@ -0,0 +1,110 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace Groot2Test +{ +struct PublisherAndPort +{ + std::unique_ptr publisher; + unsigned port = 0; +}; + +inline PublisherAndPort makePublisher(const BT::Tree& tree) +{ + std::random_device random_device; + std::uniform_int_distribution port_distribution(20000, 60000); + + for(size_t attempt = 0; attempt < 100; ++attempt) + { + // Groot2Publisher also binds port + 1. Using an even base avoids overlap + // between candidates and makes accidental clashes less likely. + const unsigned port = port_distribution(random_device) & ~1U; + try + { + return { std::make_unique(tree, port), port }; + } + catch(const zmq::error_t& error) + { + if(error.num() != EADDRINUSE) + { + throw; + } + } + } + throw std::runtime_error("Could not find two free ports for Groot2Publisher"); +} + +/// JSON payload for a HOOK_INSERT request, as Groot2 would send it. +inline nlohmann::json makeHook(uint16_t uid, BT::Monitor::Hook::Mode mode, + BT::Monitor::Hook::Position position, bool once = false) +{ + return { { "enabled", true }, + { "uid", uid }, + { "mode", int(mode) }, + { "once", once }, + { "desired_status", "FAILURE" }, + { "position", int(position) } }; +} + +class Client +{ +public: + explicit Client(unsigned port) : socket_(context_, ZMQ_REQ) + { + socket_.set(zmq::sockopt::linger, 0); + socket_.set(zmq::sockopt::rcvtimeo, 2000); + socket_.set(zmq::sockopt::sndtimeo, 2000); + socket_.connect("tcp://127.0.0.1:" + std::to_string(port)); + } + + zmq::multipart_t rawRequest(BT::Monitor::RequestType type, + std::optional payload = std::nullopt) + { + const BT::Monitor::RequestHeader header(type); + zmq::multipart_t request; + request.addstr(BT::Monitor::SerializeHeader(header)); + if(payload) + { + request.addstr(*payload); + } + if(!request.send(socket_)) + { + throw std::runtime_error("Timed out sending a Groot2 request"); + } + + zmq::multipart_t reply; + if(!reply.recv(socket_)) + { + throw std::runtime_error("Timed out receiving a Groot2 reply"); + } + return reply; + } + + zmq::multipart_t request(BT::Monitor::RequestType type, + std::optional payload = std::nullopt) + { + auto reply = rawRequest(type, std::move(payload)); + if(reply.empty() || reply[0].size() != BT::Monitor::ReplyHeader::size()) + { + throw std::runtime_error("Received an invalid Groot2 reply"); + } + return reply; + } + +private: + zmq::context_t context_; + zmq::socket_t socket_; +}; +} // namespace Groot2Test diff --git a/tests/gtest_groot2_publisher_integration.cpp b/tests/gtest_groot2_publisher_integration.cpp new file mode 100644 index 000000000..019c6d66d --- /dev/null +++ b/tests/gtest_groot2_publisher_integration.cpp @@ -0,0 +1,89 @@ +#include "groot2_test_utils.hpp" + +#include +#include + +#include +#include + +#include + +namespace +{ +constexpr auto kHookTreeXml = R"( + + + + + +)"; + +void malformedRequestScenario() +{ + BT::BehaviorTreeFactory factory; + auto tree = factory.createTreeFromText(R"( + + + + + +)"); + auto [publisher, port] = Groot2Test::makePublisher(tree); + Groot2Test::Client client(port); + + auto error_reply = client.rawRequest(BT::Monitor::RequestType::HOOK_INSERT, "{"); + if(error_reply.size() != 2 || error_reply[0].to_string() != "error") + { + std::exit(EXIT_FAILURE); + } + + // The server thread must remain usable after rejecting the bad request. + auto valid_reply = client.request(BT::Monitor::RequestType::FULLTREE); + if(valid_reply.size() != 2) + { + std::exit(EXIT_FAILURE); + } + std::exit(EXIT_SUCCESS); +} +} // namespace + +TEST(Groot2PublisherIntegration, PostHookRunsAfterNodeAndCanBeRemoved) +{ + std::atomic_int tick_count = 0; + BT::BehaviorTreeFactory factory; + factory.registerSimpleAction("CountAction", [&](BT::TreeNode&) { + ++tick_count; + return BT::NodeStatus::SUCCESS; + }); + auto tree = factory.createTreeFromText(kHookTreeXml); + auto [publisher, port] = Groot2Test::makePublisher(tree); + Groot2Test::Client client(port); + const auto uid = tree.rootNode()->UID(); + + const auto hook = Groot2Test::makeHook(uid, BT::Monitor::Hook::Mode::REPLACE, + BT::Monitor::Hook::Position::POST); + client.request(BT::Monitor::RequestType::HOOK_INSERT, hook.dump()); + + auto dump_reply = client.request(BT::Monitor::RequestType::HOOKS_DUMP); + ASSERT_EQ(dump_reply.size(), 2); + const auto hooks = nlohmann::json::parse(dump_reply[1].to_string()); + ASSERT_EQ(hooks.size(), 1); + EXPECT_EQ(hooks[0].at("position"), int(BT::Monitor::Hook::Position::POST)); + + EXPECT_EQ(tree.tickExactlyOnce(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tick_count, 1); + + const auto remove = + nlohmann::json{ { "uid", uid }, + { "position", int(BT::Monitor::Hook::Position::POST) } }; + client.request(BT::Monitor::RequestType::HOOK_REMOVE, remove.dump()); + + EXPECT_EQ(tree.tickExactlyOnce(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tick_count, 2); +} + +TEST(Groot2PublisherIntegration, MalformedRequestDoesNotTerminateServer) +{ + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; + EXPECT_EXIT(malformedRequestScenario(), ::testing::ExitedWithCode(EXIT_SUCCESS), ".*"); +} diff --git a/tests/gtest_groot2_publisher_thread_safety.cpp b/tests/gtest_groot2_publisher_thread_safety.cpp new file mode 100644 index 000000000..7ea12bd13 --- /dev/null +++ b/tests/gtest_groot2_publisher_thread_safety.cpp @@ -0,0 +1,148 @@ +#include "groot2_test_utils.hpp" + +#include +#include +#include +#include + +#include +#include + +using namespace std::chrono_literals; + +namespace +{ +constexpr auto kTreeXml = R"( + + + + + +)"; + +} // namespace + +// This is an end-to-end TSan reproducer rather than a deterministic functional +// failure: without TSan the data races may remain invisible and the test passes. +TEST(Groot2PublisherThreadSafety, UpdateHeartbeatDelayWhileHeartbeatThreadIsRunning) +{ + BT::BehaviorTreeFactory factory; + auto tree = factory.createTreeFromText(kTreeXml); + auto publisher_and_port = Groot2Test::makePublisher(tree); + + // The publisher's heartbeat thread reads this setting concurrently. + for(size_t i = 0; i < 100; ++i) + { + publisher_and_port.publisher->setMaxHeartbeatDelay((i % 2 == 0) ? 1ms : 1h); + std::this_thread::sleep_for(1ms); + } +} + +TEST(Groot2PublisherThreadSafety, RecordTransitionsWhileTreeIsTicking) +{ + BT::BehaviorTreeFactory factory; + auto tree = factory.createTreeFromText(kTreeXml); + auto [publisher, port] = Groot2Test::makePublisher(tree); + Groot2Test::Client client(port); + + std::atomic_bool keep_ticking = true; + std::thread tick_thread([&] { + while(keep_ticking.load()) + { + tree.tickExactlyOnce(); + tree.haltTree(); + } + }); + + try + { + // Starting while callbacks are already active also covers initialization of + // the recording timestamp relative to the recording flag. + client.request(BT::Monitor::RequestType::TOGGLE_RECORDING, "start"); + + // GET_TRANSITIONS runs on the publisher's server thread while status-change + // callbacks append transitions from tick_thread. + for(size_t i = 0; i < 100; ++i) + { + client.request(BT::Monitor::RequestType::GET_TRANSITIONS); + } + + client.request(BT::Monitor::RequestType::TOGGLE_RECORDING, "stop"); + } + catch(...) + { + keep_ticking = false; + tick_thread.join(); + throw; + } + + keep_ticking = false; + tick_thread.join(); +} + +TEST(Groot2PublisherThreadSafety, DestroyWhileStatusCallbacksAreRunning) +{ + BT::BehaviorTreeFactory factory; + auto tree = factory.createTreeFromText(kTreeXml); + auto publisher_and_port = Groot2Test::makePublisher(tree); + + std::atomic_bool keep_ticking = true; + std::thread tick_thread([&] { + while(keep_ticking.load()) + { + tree.tickExactlyOnce(); + } + }); + + std::this_thread::sleep_for(20ms); + publisher_and_port.publisher.reset(); + keep_ticking = false; + tick_thread.join(); +} + +// TSan's lock-order detector reports an inversion in the old implementation: +// disabling took hooks_map_mutex -> hook->mutex, while one-shot removal took +// hook->mutex -> hooks_map_mutex. +TEST(Groot2PublisherThreadSafety, DisableThenRemoveOneShotHookUsesConsistentLockOrder) +{ + BT::BehaviorTreeFactory factory; + auto tree = factory.createTreeFromText(kTreeXml); + auto [publisher, port] = Groot2Test::makePublisher(tree); + Groot2Test::Client client(port); + + auto hook = + Groot2Test::makeHook(tree.rootNode()->UID(), BT::Monitor::Hook::Mode::REPLACE, + BT::Monitor::Hook::Position::PRE); + + client.request(BT::Monitor::RequestType::HOOK_INSERT, hook.dump()); + client.request(BT::Monitor::RequestType::DISABLE_ALL_HOOKS); + + hook["enabled"] = true; + hook["once"] = true; + client.request(BT::Monitor::RequestType::HOOK_INSERT, hook.dump()); + + EXPECT_EQ(tree.tickExactlyOnce(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickExactlyOnce(), BT::NodeStatus::SUCCESS); +} + +TEST(Groot2PublisherThreadSafety, DestroyWhileBreakpointCallbackIsBlocked) +{ + BT::BehaviorTreeFactory factory; + auto tree = factory.createTreeFromText(kTreeXml); + auto [publisher, port] = Groot2Test::makePublisher(tree); + Groot2Test::Client client(port); + + const auto hook = + Groot2Test::makeHook(tree.rootNode()->UID(), BT::Monitor::Hook::Mode::BREAKPOINT, + BT::Monitor::Hook::Position::PRE); + client.request(BT::Monitor::RequestType::HOOK_INSERT, hook.dump()); + + auto tick_result = + std::async(std::launch::async, [&tree] { return tree.tickExactlyOnce(); }); + std::this_thread::sleep_for(20ms); + + publisher.reset(); + + ASSERT_EQ(tick_result.wait_for(1s), std::future_status::ready); + EXPECT_EQ(tick_result.get(), BT::NodeStatus::SUCCESS); +} diff --git a/tests/tsan_suppressions.txt b/tests/tsan_suppressions.txt index ee24259a4..ebc390bb9 100644 --- a/tests/tsan_suppressions.txt +++ b/tests/tsan_suppressions.txt @@ -1,7 +1,11 @@ # ThreadSanitizer suppressions file for behaviortree_cpp_test -# ZeroMQ false positives (third-party library) -race:zmq::epoll_t::add_fd +# ZeroMQ false positives (third-party library). libzmq is not built with TSan +# instrumentation and uses hand-rolled atomics that TSan cannot understand. +# The module rule covers a system-installed shared library; the namespace rule +# covers the Conan build, where zmq is compiled from source and linked +# statically, so its symbols do not live in a libzmq.so module. +race:zmq::* race:libzmq.so # False positive: std::swap of std::deque under mutex provides proper synchronization