Line data    Source code

       1             : #ifndef CONTROLS_UTILS_NODEUTILS_H
       2             : #define CONTROLS_UTILS_NODEUTILS_H
       3             : 
       4             : #include <rcl/time.h>
       5             : 
       6             : #include <rclcpp/rclcpp.hpp>
       7             : 
       8             : namespace Controller {
       9             : 
      10             : /**
      11             :  * @brief Singleton utility providing access to the shared ROS node and time functions
      12             :  *
      13             :  * The node pointer must be set by the application entrypoint before any
      14             :  * other method is called.  All methods silently degrade when the node is null.
      15             :  */
      16           1 : class NodeUtils {
      17             : public:
      18             :     /**
      19             :      * @brief Shared pointer to the single active ROS2 node.
      20             :      */
      21             :     inline static std::shared_ptr<rclcpp::Node> node_;
      22             : 
      23             :     /**
      24             :      * @brief Returns the current ROS clock time in seconds
      25             :      *
      26             :      * @return Current ROS time in seconds, or 0.0 if the node is not yet set
      27             :      */
      28           1 :     static double current_ros_time() {
      29             :         if (!Controller::NodeUtils::node_) {
      30             :             return 0.0;
      31             :         }
      32             :         return node_->get_clock()->now().seconds();
      33             :     }
      34             : 
      35             :     /**
      36             :      * @brief Returns the current system (wall-clock) time in seconds
      37             :      *
      38             :      * @return Current system time in seconds
      39             :      */
      40           1 :     static double current_system_time() {
      41             :         return rclcpp::Clock{RCL_SYSTEM_TIME}.now().seconds();
      42             :     }
      43             : 
      44             :     /**
      45             :      * @brief Returns the current ROS time as an rclcpp::Time stamp
      46             :      *
      47             :      * @return Current ROS time as an rclcpp::Time
      48             :      */
      49           1 :     static rclcpp::Time current_ros_stamp() {
      50             :         return rclcpp::Time{static_cast<int64_t>(current_ros_time())};
      51             :     }
      52             : 
      53             :     /**
      54             :      * @brief Returns the ROS logger associated with the shared node
      55             :      *
      56             :      * @return Node logger, or the "controls" logger if no node is set
      57             :      */
      58           1 :     static rclcpp::Logger get_logger() {
      59             :         if (node_) {
      60             :             return node_->get_logger();
      61             :         }
      62             :         return rclcpp::get_logger("controls");
      63             :     }
      64             : };
      65             : 
      66             : }  // namespace Controller
      67             : #endif  // CONTROLS_UTILS_NODEUTILS_H