masterNode.cpp

Go to the documentation of this file.

#include "masterNode.hpp"
#include <rclcpp/rclcpp.hpp>
#include <galaxis/node_ros_pubsub_wrapper.hpp>
#include <functional>
#include <string>
#include <sstream>
#include <galaxis/cuprules.hpp>
#include <utils/logger.hpp>
 
#include <unistd.h>
 
static long long
__get_time_now_ms() {
  auto time_now    = std::chrono::system_clock::now();
  auto time_now_ms = std::chrono::time_point_cast<std::chrono::milliseconds>(time_now);
  auto time_value  = time_now_ms.time_since_epoch();
  return time_value.count();
}
 
masterNode::masterNode() :
    Node("master_node") {
  this->nodeName       = this->get_name();
  std::string info_msg = "Initializing master_node";
 
  galaxis::utils::log(info_msg);
 
  init();
}
 
masterNode::~masterNode() {
  std::string info_msg("Shutting down");
  galaxis::utils::log(info_msg);
}
 
void
masterNode::init(void) {
  m_ecuOnlineStatus = false;
 
  /* for create_publisher and create_subscriber */
  using std::placeholders::_1;
 
  galaxis::ros::create_publisher(this, service_state_pub_);
  galaxis::ros::create_publisher(this, settingsValuesPubSPtr);
  galaxis::ros::create_subscription(this,
                                    heartbeat_sub_,
                                    std::bind(&masterNode::heartbeat_callback, this, _1));
  galaxis::ros::create_subscription(this,
                                    buttonStateSubSPtr,
                                    std::bind(&masterNode::callback_buttonState, this, _1));
 
  periodic_timer_                     = this->create_wall_timer(std::chrono::milliseconds{ _HEARTBEAT_CHECK_PERIOD }, std::bind(&masterNode::check_heartbeats, this));
  periodic_timer_print_system_status_ = this->create_wall_timer(std::chrono::milliseconds{ 1000 }, std::bind(&masterNode::print_system_status, this));
  /* WAIT FOR ALL NODES TO BE RUNNING AND THEN SEND STATE?! */
 
  // Insert Nodes // TODO
  system_state_.insert(std::make_pair<std::string, std::pair<long long, galaxis::serviceStates::State>>("ControllerNode", std::make_pair<long long, galaxis::serviceStates::State>(0, galaxis::serviceStates::State::BOOTSTRAP)));
  system_state_.insert(std::make_pair<std::string, std::pair<long long, galaxis::serviceStates::State>>("Autopilot", std::make_pair<long long, galaxis::serviceStates::State>(0, galaxis::serviceStates::State::BOOTSTRAP)));
  system_state_.insert(std::make_pair<std::string, std::pair<long long, galaxis::serviceStates::State>>("ecu_node", std::make_pair<long long, galaxis::serviceStates::State>(0, galaxis::serviceStates::State::BOOTSTRAP)));
  // system_state_.insert(std::make_pair<std::string, std::pair<long long, galaxis::serviceStates::State>>("ObjectDetector", std::make_pair<long long, galaxis::serviceStates::State>(0, galaxis::serviceStates::State::BOOTSTRAP)));
 
  // std::thread(&masterNode::initial_set_ecu_settings, this).detach();
}
 
void
masterNode::initial_set_ecu_settings() {
  return;
  // if(m_ecuOnlineStatus) {
  //   RCLCPP_INFO(this->get_logger(), "SET INITIAL ECU SETTINGS");
  //   // Training
  //   /* set required car settings like speed */
  //   auto msgUPtr = std::make_unique<SettingsValuesType>();
  //   msgUPtr->firmware.rc_use_erpm = cuprules::modes::training::rc_use_erpm;
  //   msgUPtr->firmware.erpm_max_rc = cuprules::modes::training::erpm_max_rc;
  //   RCLCPP_INFO(this->get_logger(), "ERPM max is : %f", msgUPtr->firmware.erpm_max_rc);
  //   RCLCPP_INFO(this->get_logger(), "Sending settings for btn1");
  //   //settingsValuesPubSPtr->publish(std::move(msgUPtr));
 
  //   m_ecuInitStatus = true;
  // }
}
 
void
masterNode::print_system_status() {
  std::ostringstream ostring;
  ostring << "System State (" << m_currentServiceState << "): \n";
  long long timeNow = __get_time_now_ms();
 
  for(auto& x: system_state_) {
    ostring << x.first << "\t\t";
    if(timeNow - x.second.first > _HEARTBEAT_MAXIMUM_OFF_TIME_MS) {
      ostring << "offline";
    } else {
      ostring << "online\t\tmode: " << x.second.second << "\t\t" << (timeNow - x.second.first);
    }
    ostring << "\n";
  }
 
  RCLCPP_INFO(this->get_logger(), "%s", ostring.str().c_str());
}
 
bool
masterNode::getSystemOnlineState() {
  long long timeNow = __get_time_now_ms();
  bool systemOnline = true;
 
  for(auto& node: system_state_) {
    if(timeNow - node.second.first > _HEARTBEAT_MAXIMUM_OFF_TIME_MS) {
      systemOnline = false;
 
      if(node.first.compare("ecu_node") == 0)
        m_ecuOnlineStatus = false;
    }
  }
 
  return systemOnline;
}
 
void
masterNode::check_heartbeats() {
  return; // DISABLE FUNCTION
  bool systemOnline = getSystemOnlineState();
 
  if(systemOnline == false) {
    switch_system_state(galaxis::serviceStates::State::BOOTSTRAP, true);
  }
 
  if(m_systemOnlineStatus == false && systemOnline == true) {
    switch_system_state(m_previousServiceState, true);
  }
 
  m_systemOnlineStatus = systemOnline;
 
  //ecu init
  if(m_ecuInitStatus == false) {
    // initial_set_ecu_settings();
  }
}
 
void
masterNode::switch_system_state(galaxis::serviceStates::State p_state, bool p_force = false) {
  //if (m_previousServiceState != p_state && (m_systemOnlineStatus || p_force || true)) { // m_systemOnlineStatus disabled
  m_previousServiceState = m_currentServiceState;
  m_currentServiceState  = p_state;
 
  ServiceStateType::UniquePtr serviceStateMsgUPtr = std::make_unique<ServiceStateType>();
  serviceStateMsgUPtr->state                      = get_service_state_by_enum_state(p_state);
  service_state_pub_->publish(std::move(serviceStateMsgUPtr));
  RCLCPP_INFO(this->get_logger(), "!SystemState switched to: %i", p_state);
  //}
}
 
void
masterNode::heartbeat_callback(HeartbeatMessageType::SharedPtr const msg) {
  if(system_state_.find(msg->node_name.c_str()) == system_state_.end()) {
    system_state_.insert(std::pair<std::string, std::pair<long long, galaxis::serviceStates::State>>(msg->node_name, std::pair<long long, galaxis::serviceStates::State>(__get_time_now_ms(), static_cast<galaxis::serviceStates::State>(msg->service_state.state))));
  } else {
    system_state_.find(msg->node_name.c_str())->second = std::pair<long long, galaxis::serviceStates::State>(__get_time_now_ms(), static_cast<galaxis::serviceStates::State>(msg->service_state.state));
  }
 
  if(msg->node_name.compare("ecu_node") == 0) {
    m_ecuOnlineStatus = true;
  }
 
  //RCLCPP_INFO(this->get_logger(), "[HEARTBEAT] %s: %i", msg->node_name.c_str(), msg->service_state.state);
  //print_system_status();
}
 
void
masterNode::handle_controller(bool start) {
  // TODO?
}
 
void
masterNode::callback_buttonState(ButtonStateMessageType::SharedPtr const buttonStateMsgSPtr) {
  RCLCPP_INFO(this->get_logger(), "Got new buttonState message");
  if(buttonStateMsgSPtr == nullptr) {
    RCLCPP_ERROR(this->get_logger(), "buttonStateMsgSPtr is nullptr");
    throw galaxis::Errors::POINTER_ERROR();
  }
 
  uint8_t btn1 = buttonStateMsgSPtr->pressed_button_1;
  uint8_t btn2 = buttonStateMsgSPtr->pressed_button_2;
  uint8_t btn3 = buttonStateMsgSPtr->pressed_button_3;
 
  uint8_t cnt = (btn1) + (btn3) + (btn2);
 
  if(cnt > 1) {
    RCLCPP_ERROR(this->get_logger(), "Pressed two or more buttons at once, aborting");
    return;
  } else if(cnt == 0) { // no button pressed
    return;
  }
 
  galaxis::serviceStates::State newServiceState = galaxis::serviceStates::BOOTSTRAP;
 
  if(btn1) {
    // Training
    /* set required car settings like speed */
    auto msgUPtr                         = std::make_unique<SettingsValuesType>();
    msgUPtr->firmware.rc_use_erpm        = cuprules::modes::training::rc_use_erpm;
    msgUPtr->firmware.erpm_max_rc        = cuprules::modes::training::erpm_max_rc;
    msgUPtr->firmware.servo_offset.front = 5;
    msgUPtr->firmware.servo_offset.rear  = 0;
    RCLCPP_INFO(this->get_logger(), "ERPM max is : %f", msgUPtr->firmware.erpm_max_rc);
    RCLCPP_INFO(this->get_logger(), "Sending settings for btn1");
    settingsValuesPubSPtr->publish(std::move(msgUPtr));
 
    // stop controller
    handle_controller(false);
 
    //newServiceState = galaxis::serviceStates::btnStateMap[galaxis::serviceStates::ButtonType::BTN_SC1];
    //switch_system_state(newServiceState);
  } else if(btn2) {
    // Free Driving
    /* set required car settings like speed */
    auto msgUPtr                         = std::make_unique<SettingsValuesType>();
    msgUPtr->firmware.rc_use_erpm        = cuprules::modes::free_driving::rc_use_erpm;
    msgUPtr->firmware.erpm_max_rc        = cuprules::modes::free_driving::erpm_max_rc;
    msgUPtr->firmware.servo_offset.front = 5;
    msgUPtr->firmware.servo_offset.rear  = 0;
    RCLCPP_INFO(this->get_logger(), "FREE DRIVING (btn2)");
    RCLCPP_INFO(this->get_logger(), "Sending settings for btn2");
    settingsValuesPubSPtr->publish(std::move(msgUPtr));
 
    usleep(100);
 
    // start controller
    handle_controller(true);
 
    //newServiceState = galaxis::serviceStates::btnStateMap[galaxis::serviceStates::ButtonType::BTN_SC2];
    newServiceState = galaxis::serviceStates::FREE_DRIVING;
    switch_system_state(newServiceState);
  } else if(btn3) {
    // Obstacle Driving
    /* set required car settings like speed */
    auto msgUPtr                         = std::make_unique<SettingsValuesType>();
    msgUPtr->firmware.rc_use_erpm        = cuprules::modes::obstacle_driving::rc_use_erpm;
    msgUPtr->firmware.erpm_max_rc        = cuprules::modes::obstacle_driving::erpm_max_rc;
    msgUPtr->firmware.servo_offset.front = 5;
    msgUPtr->firmware.servo_offset.rear  = 0;
    RCLCPP_INFO(this->get_logger(), "OBSTACLE DRIVING (btn3)");
    RCLCPP_INFO(this->get_logger(), "Sending settings for btn2");
    settingsValuesPubSPtr->publish(std::move(msgUPtr));
 
    // start controller
    handle_controller(true);
 
    usleep(100);
 
    // newServiceState = galaxis::serviceStates::btnStateMap[galaxis::serviceStates::ButtonType::BTN_SC3];
    newServiceState = galaxis::serviceStates::OBSTACLE_DRIVING;
    switch_system_state(newServiceState);
  } else {
    // pass
  }
}
 
galaxis::serviceStates::State
masterNode::get_service_state_by_uint8(uint8_t p_state) {
  galaxis::serviceStates::State state;
  switch(p_state) {
    case messages::msg::ServiceState::STATE_BOOTSTRAP:
      state = galaxis::serviceStates::State::BOOTSTRAP;
      break;
    case messages::msg::ServiceState::STATE_FREE_DRIVING:
      state = galaxis::serviceStates::State::FREE_DRIVING;
      break;
    case messages::msg::ServiceState::STATE_OBSTACLE_DRIVING:
      state = galaxis::serviceStates::State::OBSTACLE_DRIVING;
      break;
    default:
      RCLCPP_ERROR(this->get_logger(), "p_state not known %u", p_state);
      state = galaxis::serviceStates::State::BOOTSTRAP;
      break;
  }
 
  return state;
}
 
uint8_t
masterNode::get_service_state_by_enum_state(galaxis::serviceStates::State p_state) {
  uint8_t state;
  switch(p_state) {
    case galaxis::serviceStates::State::BOOTSTRAP:
      state = messages::msg::ServiceState::STATE_BOOTSTRAP;
      break;
    case galaxis::serviceStates::State::FREE_DRIVING:
      state = messages::msg::ServiceState::STATE_FREE_DRIVING;
      break;
    case galaxis::serviceStates::State::OBSTACLE_DRIVING:
      state = messages::msg::ServiceState::STATE_OBSTACLE_DRIVING;
      break;
    default:
      RCLCPP_ERROR(this->get_logger(), "p_state unknwon -> %u", p_state);
      state = messages::msg::ServiceState::STATE_BOOTSTRAP;
      break;
  }
 
  return state;
}