node_utils.hpp

// Copyright (c) 2019 Intel Corporation
// Copyright (c) 2023 Open Navigation LLC
//
// 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 NAV2_ROS_COMMON__NODE_UTILS_HPP_
#define NAV2_ROS_COMMON__NODE_UTILS_HPP_
 
#include <vector>
#include <string>
#include <chrono>
#include <algorithm>
#include <cctype>
#include <stdexcept>
#include "rclcpp/version.h"
#include "rclcpp/clock.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/node_options.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/parameter_value.hpp"
#include "rclcpp/qos.hpp"
#include "rcl_interfaces/srv/list_parameters.hpp"
#include "pluginlib/exceptions.hpp"
#if RCLCPP_VERSION_GTE(30, 1, 4)
#include "ament_index_cpp/get_package_share_path.hpp"
#else
#include "ament_index_cpp/get_package_share_directory.hpp"
#endif
 
#ifdef __APPLE__
  #include <pthread.h>
  #include <mach/mach.h>
  #include <mach/thread_policy.h>
#elif defined(_WIN32)
  #ifndef NOMINMAX
    #define NOMINMAX
  #endif
  #include <windows.h>
#else
  #include <sched.h>
  #include <errno.h>
#endif
 
using std::chrono::high_resolution_clock;
using std::to_string;
using std::string;
using std::replace_if;
using std::isalnum;
 
namespace nav2
{
 
 
inline std::string sanitize_node_name(const std::string & potential_node_name)
{
  string node_name(potential_node_name);
  // read this as `replace` characters in `node_name` `if` not alphanumeric.
  // replace with '_'
  replace_if(
    begin(node_name), end(node_name),
    [](auto c) {return !isalnum(c);},
    '_');
  return node_name;
}
 
 
inline std::string add_namespaces(const std::string & top_ns, const std::string & sub_ns = "")
{
  if (!top_ns.empty() && top_ns.back() == '/') {
    if (top_ns.front() == '/') {
      return top_ns + sub_ns;
    } else {
      return "/" + top_ns + sub_ns;
    }
  }
 
  return top_ns + "/" + sub_ns;
}
 
 
inline std::string time_to_string(size_t len)
{
  string output(len, '0');  // prefill the string with zeros
  auto timepoint = high_resolution_clock::now();
  auto timecount = timepoint.time_since_epoch().count();
  auto timestring = to_string(timecount);
  if (timestring.length() >= len) {
    // if `timestring` is shorter, put it at the end of `output`
    output.replace(
      0, len,
      timestring,
      timestring.length() - len, len);
  } else {
    // if `output` is shorter, just copy in the end of `timestring`
    output.replace(
      len - timestring.length(), timestring.length(),
      timestring,
      0, timestring.length());
  }
  return output;
}
 
 
inline std::string generate_internal_node_name(const std::string & prefix = "")
{
  return sanitize_node_name(prefix) + "_" + time_to_string(8);
}
 
 
inline rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix = "")
{
  auto options =
    rclcpp::NodeOptions()
    .start_parameter_services(false)
    .start_parameter_event_publisher(false)
    .arguments({"--ros-args", "-r", "__node:=" + generate_internal_node_name(prefix), "--"});
  return rclcpp::Node::make_shared("_", options);
}
 
using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor;
 
/* Declares static ROS2 parameter and sets it to a given value
 * if it was not already declared.
 *
 * \param[in] node A node in which given parameter to be declared
 * \param[in] parameter_name The name of parameter
 * \param[in] default_value Parameter value to initialize with
 * \param[in] parameter_descriptor Parameter descriptor (optional)
 */
template<typename NodeT>
inline void declare_parameter_if_not_declared(
  NodeT node,
  const std::string & parameter_name,
  const rclcpp::ParameterValue & default_value,
  const ParameterDescriptor & parameter_descriptor = ParameterDescriptor())
{
  if (!node->has_parameter(parameter_name)) {
    node->declare_parameter(parameter_name, default_value, parameter_descriptor);
  }
}
 
/* Declares static ROS2 parameter with given type if it was not already declared.
 *
 * \param[in] node A node in which given parameter to be declared
 * \param[in] parameter_name Name of the parameter
 * \param[in] param_type The type of parameter
 * \param[in] parameter_descriptor Parameter descriptor (optional)
 */
template<typename NodeT>
inline void declare_parameter_if_not_declared(
  NodeT node,
  const std::string & parameter_name,
  const rclcpp::ParameterType & param_type,
  const ParameterDescriptor & parameter_descriptor = ParameterDescriptor())
{
  if (!node->has_parameter(parameter_name)) {
    node->declare_parameter(parameter_name, param_type, parameter_descriptor);
  }
}
 
/* Declares a parameter with the specified type if it was not already declared.
 * If the parameter was overridden, its value is returned, otherwise an
 * rclcpp::exceptions::InvalidParameterValueException is thrown
 *
 * \param[in] node A node in which given parameter to be declared
 * \param[in] parameter_name Name of the parameter
 * \param[in] parameter_descriptor Parameter descriptor (optional)
 * \return The value of the parameter or an exception
 */
template<typename ParameterT, typename NodeT>
inline ParameterT declare_or_get_parameter(
  NodeT node,
  const std::string & parameter_name,
  const ParameterDescriptor & parameter_descriptor = ParameterDescriptor())
{
  if (node->has_parameter(parameter_name)) {
    return node->get_parameter(parameter_name).template get_value<ParameterT>();
  }
  auto param_type = rclcpp::ParameterValue{ParameterT{}}.get_type();
  auto parameter = node->declare_parameter(parameter_name, param_type, parameter_descriptor);
  if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
    std::string description = "Parameter " + parameter_name + " not in overrides";
    throw rclcpp::exceptions::InvalidParameterValueException(description.c_str());
  }
  return parameter.template get<ParameterT>();
}
 
using NodeParamInterfacePtr = rclcpp::node_interfaces::NodeParametersInterface::SharedPtr;
 
 
template<typename ParamType>
inline ParamType declare_or_get_parameter(
  const rclcpp::Logger & logger, NodeParamInterfacePtr param_interface,
  const std::string & parameter_name, const ParamType & default_value,
  bool warn_if_no_override = false, bool strict_param_loading = false,
  const ParameterDescriptor & parameter_descriptor = ParameterDescriptor())
{
  if (param_interface->has_parameter(parameter_name)) {
    rclcpp::Parameter param(parameter_name, default_value);
    param_interface->get_parameter(parameter_name, param);
    return param.get_value<ParamType>();
  }
 
  auto return_value = param_interface
    ->declare_parameter(
    parameter_name, rclcpp::ParameterValue{default_value},
    parameter_descriptor)
    .get<ParamType>();
 
  const bool no_param_override = param_interface->get_parameter_overrides().find(parameter_name) ==
    param_interface->get_parameter_overrides().end();
  if (no_param_override) {
    if (warn_if_no_override) {
      RCLCPP_WARN_STREAM(
        logger,
        "Failed to get param " << parameter_name << " from overrides, using default value.");
    }
    if (strict_param_loading) {
      std::string description = "Parameter " + parameter_name +
        " not in overrides and strict_param_loading is True";
      throw rclcpp::exceptions::InvalidParameterValueException(description.c_str());
    }
  }
 
  return return_value;
}
 
 
template<typename ParamType, typename NodeT>
inline ParamType declare_or_get_parameter(
  NodeT node, const std::string & parameter_name,
  const ParamType & default_value,
  const ParameterDescriptor & parameter_descriptor = ParameterDescriptor())
{
  declare_parameter_if_not_declared(node, "warn_on_missing_params", rclcpp::ParameterValue(false));
  bool warn_if_no_override{false};
  node->get_parameter("warn_on_missing_params", warn_if_no_override);
  declare_parameter_if_not_declared(node, "strict_param_loading", rclcpp::ParameterValue(false));
  bool strict_param_loading{false};
  node->get_parameter("strict_param_loading", strict_param_loading);
  return declare_or_get_parameter(
    node->get_logger(), node->get_node_parameters_interface(),
    parameter_name, default_value, warn_if_no_override, strict_param_loading, parameter_descriptor);
}
 
 
template<typename NodeT>
inline std::string get_plugin_type_param(
  NodeT node,
  const std::string & plugin_name)
{
  declare_parameter_if_not_declared(node, plugin_name + ".plugin", rclcpp::PARAMETER_STRING);
  std::string plugin_type;
  try {
    if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) {
      RCLCPP_FATAL(
        node->get_logger(), "Can not get 'plugin' param value for %s", plugin_name.c_str());
      throw pluginlib::PluginlibException("No 'plugin' param for param ns!");
    }
  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
    RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str());
    throw pluginlib::PluginlibException("No 'plugin' param for param ns!");
  }
 
  return plugin_type;
}
 
inline void setSoftRealTimePriority()
{
#ifdef __APPLE__
  // macOS: Use Mach thread API to approximate real-time scheduling
  thread_port_t thread = pthread_mach_thread_np(pthread_self());
 
  thread_time_constraint_policy_data_t policy;
  policy.period = 1000;       // in microseconds (1 kHz loop)
  policy.computation = 800;   // expected compute time per period
  policy.constraint = 1000;   // max latency
  policy.preemptible = 1;     // allow preemption by higher-priority threads
 
  kern_return_t result = thread_policy_set(
    thread,
    THREAD_TIME_CONSTRAINT_POLICY,
    (thread_policy_t)&policy,
    THREAD_TIME_CONSTRAINT_POLICY_COUNT
  );
 
  if (result != KERN_SUCCESS) {
    std::string errmsg =
      "Failed to set THREAD_TIME_CONSTRAINT_POLICY on macOS. "
      "Thread remains at default priority. Mach Error Code: " +
      std::to_string(result);
    throw std::runtime_error(errmsg);
  }
#elif defined(_WIN32)
  // Windows: Raise only the current thread one level above its normal priority
  // within the existing process priority class.
  if (!SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_ABOVE_NORMAL)) {
    throw std::runtime_error(
            "Failed to set soft real-time thread priority on Windows. Error: " +
            std::to_string(GetLastError()));
  }
#else
  // Linux: True real-time scheduling (requires privileges)
  sched_param sch;
  sch.sched_priority = 49;
  if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) {
    std::string errmsg(
      "Cannot set as real-time thread. Users must set: <username> hard rtprio 99 and "
      "<username> soft rtprio 99 in /etc/security/limits.conf to enable "
      "realtime prioritization! Error: ");
    throw std::runtime_error(errmsg + std::strerror(errno));
  }
#endif
}
 
template<typename InterfaceT>
inline void setIntrospectionMode(
  InterfaceT & ros_interface,
  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface,
  rclcpp::Clock::SharedPtr clock)
{
  #if RCLCPP_VERSION_GTE(29, 0, 0)
  rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
  if (!node_parameters_interface->has_parameter("introspection_mode")) {
    node_parameters_interface->declare_parameter(
      "introspection_mode", rclcpp::ParameterValue("disabled"));
  }
  std::string introspection_mode =
    node_parameters_interface->get_parameter("introspection_mode").as_string();
  if (introspection_mode == "metadata") {
    introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;
  } else if (introspection_mode == "contents") {
    introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS;
  }
 
  ros_interface->configure_introspection(clock, rclcpp::ServicesQoS(), introspection_state);
  #else
  (void)ros_interface;
  (void)node_parameters_interface;
  (void)clock;
  #endif
}
 
inline void replaceOrAddArgument(
  std::vector<std::string> & arguments, const std::string & option,
  const std::string & arg_name, const std::string & new_argument)
{
  auto argument = std::find_if(
    arguments.begin(), arguments.end(),
    [arg_name](const std::string & value) {return value.find(arg_name) != std::string::npos;});
  if (argument != arguments.end()) {
    *argument = new_argument;
  } else {
    arguments.push_back("--ros-args");
    arguments.push_back(option);
    arguments.push_back(new_argument);
  }
}
 
inline std::string get_package_share_directory(const std::string & package_name)
{
  #if RCLCPP_VERSION_GTE(30, 1, 4)
  std::filesystem::path pkg_share_dir = ament_index_cpp::get_package_share_path(package_name);
  return pkg_share_dir.string();
  #else
  return ament_index_cpp::get_package_share_directory(package_name);
  #endif
}
 
}  // namespace nav2
 
#endif  // NAV2_ROS_COMMON__NODE_UTILS_HPP_