GalaxisNode.cpp

Go to the documentation of this file.

#include "galaxis/GalaxisNode.hpp"
 
#include "utils/settings.hpp"
#include "utils/logger.hpp"
#include <exception>
 
using namespace galaxis::utils;
using std::placeholders::_1;
 
GalaxisNode::GalaxisNode(std::string p_name, unsigned int p_heartbeatTimePeriod) :
    Node(p_name), m_heartbeatHandler(static_cast<rclcpp::Node*>(this), p_name, p_heartbeatTimePeriod) {
  
  m_nodeName = p_name;
  galaxis::ros::create_subscription(this, m_serviceSubscription, std::bind(&GalaxisNode::service_state_callback, this, std::placeholders::_1));
 
  m_parameterCallbackHandle = nullptr;
 
  // load global settings: settings/global.json
  try {
    auto settings = galaxis::utils::read_json_file("settings/onstartup_global.json");
    m_globalSettings = (*settings)["settings"];
  }
  catch (std::exception &e) {
    log_warning("Failed to load Global System Settings!");
  }
}
GalaxisNode::GalaxisNode(std::string p_name) : GalaxisNode(p_name, 200) {}
 
void GalaxisNode::register_callback(std::function<void(galaxis::serviceStates::State)> p_function) {
  m_callback_service_state = p_function;
}
 
void GalaxisNode::service_state_callback(ServiceStateType::SharedPtr const p_msgSPtr) {
  switch(p_msgSPtr->state) {
    case ServiceStateType::STATE_BOOTSTRAP:
      RCLCPP_INFO(this->get_logger(), "galaxis::serviceStates::State: switch to BOOTSTRAP");
      if(m_callback_service_state) {
        m_callback_service_state(galaxis::serviceStates::State::BOOTSTRAP);
      }
      break;
    case ServiceStateType::STATE_FREE_DRIVING:
      RCLCPP_INFO(this->get_logger(), "galaxis::serviceStates::State: switch to FREE_DRIVING");
      if(m_callback_service_state) {
        m_callback_service_state(galaxis::serviceStates::State::FREE_DRIVING);
      }
      break;
    case ServiceStateType::STATE_OBSTACLE_DRIVING:
      RCLCPP_INFO(this->get_logger(), "galaxis::serviceStates::State: switch to OBSTACLE_DRIVING");
      if(m_callback_service_state) {
        m_callback_service_state(galaxis::serviceStates::State::OBSTACLE_DRIVING);
      }
      break;
    default:
      RCLCPP_ERROR(this->get_logger(), "galaxis::serviceStates::State not implemented. %i", m_serviceState);
      // throw galaxis::Errors::NOT_IMPLEMENTED_ERROR();
      break;
  }
 
  m_serviceState = static_cast<galaxis::serviceStates::State>(p_msgSPtr->state);
 
  // set service state for heartbeat message
  m_heartbeatHandler.set_service_state(m_serviceState);
}
 
galaxis::serviceStates::State GalaxisNode::get_current_service_state() const {
  return m_serviceState;
}
 
void GalaxisNode::enable_parameters(std::function<rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter>&)> callbackFunc) {
  // read parameters from json, create ros params, run node implemented cb and set cb for new params
  std::shared_ptr<json> nodeSettings = galaxis::utils::read_system_settings(m_nodeName);
  this->create_parameters(nodeSettings);
  auto result = callbackFunc(this->get_parameters(m_rosParameterNamesVec));
  m_parameterCallbackHandle = this->add_on_set_parameters_callback(callbackFunc);
 
  if (result.successful == false) {
    log_error("[GALAXIS_NODE] Can NOT enable parameter functionality. Reason:\n", result.reason);
  }
}
 
void GalaxisNode::create_parameters(std::shared_ptr<json> configJsonSPtr) {
  log("[GALAXIS_NODE] Create ROS parameters from JSON.");
 
  try {
    for (json::iterator itSubSec = configJsonSPtr->begin(); itSubSec != configJsonSPtr->end(); ++itSubSec) {
      for (json::iterator itParam = itSubSec.value().begin(); itParam != itSubSec.value().end(); ++itParam) {
        std::string fullParameterName = itSubSec.key() + "." + itParam.key();
 
        rcl_interfaces::msg::ParameterDescriptor paramDesc;
        paramDesc.description = fullParameterName;
        if(itParam.value().is_number_integer()) {
          this->declare_parameter<int>(fullParameterName.c_str(), itParam.value(), paramDesc);
        }
        else if(itParam.value().is_number_float()) {
          this->declare_parameter<double>(fullParameterName.c_str(), itParam.value(), paramDesc);
        }
        else {
          log_warning("[GALAXIS_NODE] Type of parameter ", fullParameterName, " is NOT valid (INT or FLOAT/DOUBLE). Skipping.");
        }
 
        m_rosParameterNamesVec.push_back(fullParameterName);
      }
    }
  }
  catch(const std::exception& e) {
    log_warning("[GALAXIS_NODE] Failed to create parameters!");
  }
}
 
// rcl_interfaces::msg::SetParametersResult GalaxisNode::new_parameters_callback(const std::vector<rclcpp::Parameter>& parameters) {
//   log_warning("[GALAXIS_NODE] Parameter Callback not implemented :(");
 
//   rcl_interfaces::msg::SetParametersResult result;
//   result.successful = false;
//   result.reason = "[" + this->m_nodeName + "] CB NOT IMPLEMENTED";
//   return result;
// }
 
json const& GalaxisNode::get_global_settings() {
  return m_globalSettings;
}