SafetySupervisor.hpp

Go to the documentation of this file.

// Copyright 2026 Intelligent Robotics Lab
//
// 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 EASYNAV_SYSTEM__SAFETY__SAFETYSUPERVISOR_HPP_
#define EASYNAV_SYSTEM__SAFETY__SAFETYSUPERVISOR_HPP_
 
#include <atomic>
#include <mutex>
#include <optional>
#include <string>
 
#include "easynav_interfaces/msg/heartbeat.hpp"
#include "rclcpp/callback_group.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "std_msgs/msg/string.hpp"
 
#include "easynav_common/types/NavState.hpp"
#include "easynav_controller/ControllerNode.hpp"
#include "easynav_system/safety/ConfigurationFingerprint.hpp"
#include "easynav_system/safety/ParameterFreezer.hpp"
#include "easynav_system/safety/RtMonitor.hpp"
#include "easynav_system/safety/SafetyChannelMonitor.hpp"
 
namespace easynav::safety
{

class SafetySupervisor
{
public:
  void declare_parameters(rclcpp_lifecycle::LifecycleNode & node);

  bool check_system(
    rclcpp_lifecycle::LifecycleNode & node, rclcpp::CallbackGroup::SharedPtr rt_group = nullptr);

  bool check_controller(ControllerNode & controller) const;

  void on_configured(const Nodes & nodes, NavState & nav_state);

  bool allows_reconfiguration(const std::string & reason) const;

  void on_activate();

  bool cycle_rt(NavState & nav_state, RtMonitor::Clock::time_point now);

  [[nodiscard]] const std::string & failure() const {return failure_;}
 
  [[nodiscard]] const RtMonitor & get_rt_monitor() const {return rt_monitor_;}

  [[nodiscard]] bool is_safety_status_enabled() const {return safety_status_sub_ != nullptr;}

  [[nodiscard]] bool is_safety_mode() const {return safety_mode_;}

  [[nodiscard]] bool is_memory_lock_requested() const {return lock_memory_;}

  [[nodiscard]] std::string get_configuration_hash() const;
 
private:
  std::atomic<bool> safety_mode_ {false};
  std::atomic<bool> lock_memory_ {false};
  double max_linear_vel_ {0.0};
  double max_angular_vel_ {0.0};
  rclcpp::Logger logger_ {rclcpp::get_logger("system_node")};
  std::string namespace_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr configuration_pub_;

  void report_rt_status(NavState & nav_state, RtMonitor::Status status);

  void check_pose_age(NavState & nav_state, const rclcpp::Time & now);

  void report_safety_status(
    NavState & nav_state, const SafetyChannelMonitor::Evaluation & evaluation);
 
  ParameterFreezer freezer_;
 
  // Real-time cycle: monitor and heartbeat, used only from the RT cycle once active.
  RtMonitor rt_monitor_;
  double max_period_factor_ {2.0};
  std::optional<RtMonitor::Status> last_rt_report_;
  std::string hardware_id_ {"system_node"};
  std::string failure_;
  rclcpp::Clock::SharedPtr clock_;
  double heartbeat_period_ {0.0};
  std::optional<RtMonitor::Clock::time_point> last_heartbeat_;
  easynav_interfaces::msg::Heartbeat heartbeat_;
  rclcpp::Publisher<easynav_interfaces::msg::Heartbeat>::SharedPtr heartbeat_pub_;
  std::string configuration_hash_;
 
  // Age of the robot pose, checked from the RT cycle.
  double max_pose_age_ {1.0};
  std::optional<bool> last_pose_stale_;
 
  // Safety channel: received in the RT callback group, applied from the RT cycle.
  double safety_status_timeout_ {0.0};
  SafetyChannelMonitor safety_channel_;
  rclcpp::Subscription<easynav_interfaces::msg::SafetyStatus>::SharedPtr safety_status_sub_;
  std::optional<SafetyChannelMonitor::Evaluation> last_safety_report_;
  mutable std::mutex configuration_hash_mutex_;
};
 
}  // namespace easynav::safety
 
#endif  // EASYNAV_SYSTEM__SAFETY__SAFETYSUPERVISOR_HPP_