CommandGuard.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_CONTROLLER__SAFETY__COMMANDGUARD_HPP_
#define EASYNAV_CONTROLLER__SAFETY__COMMANDGUARD_HPP_
 
#include <cstdint>
#include <optional>
#include <string>
#include <utility>
 
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
#include "easynav_common/types/NavState.hpp"
#include "easynav_controller/VelocityMux.hpp"
 
namespace easynav::safety
{

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

  bool configure(rclcpp_lifecycle::LifecycleNode & node);

  bool check_controller_period(double period, const std::string & controller) const;

  [[nodiscard]] rclcpp::QoS qos() const;

  bool is_new(const geometry_msgs::msg::TwistStamped & cmd);

  bool discard_non_finite(NavState & nav_state);

  VelocityMux::Selection supervise(
    const VelocityMux::Selection & selection, const rclcpp::Time & now);

  [[nodiscard]] bool timed_out() const {return timed_out_;}

  void report(NavState & nav_state, bool discarded, bool fresh);

  [[nodiscard]] bool keepalive_due(const rclcpp::Time & now) const;

  void published(const rclcpp::Time & stamp) {last_publish_ = stamp;}

  void reset();
 
  [[nodiscard]] double cmd_timeout() const {return cmd_timeout_;}
  [[nodiscard]] double keepalive_period() const {return keepalive_period_;}
 
private:
  void write_report(NavState & nav_state, uint8_t level, const std::string & message);
 
  double cmd_timeout_ {0.5};
  double keepalive_period_ {0.0};
  rclcpp::Logger logger_ {rclcpp::get_logger("controller_node")};
  std::string hardware_id_ {"controller_node"};
  std::string timeout_message_;
 
  std::optional<geometry_msgs::msg::TwistStamped> last_controller_cmd_;
  std::optional<rclcpp::Time> last_proposal_;
  std::optional<rclcpp::Time> last_publish_;
  bool timed_out_ {false};
  std::optional<std::pair<uint8_t, std::string>> last_report_;
};
 
}  // namespace easynav::safety
 
#endif  // EASYNAV_CONTROLLER__SAFETY__COMMANDGUARD_HPP_