masterNode Class Reference

#include <masterNode.hpp>

Inheritance diagram for masterNode:

Collaboration diagram for masterNode:

Public Member Functions
	masterNode ()
	~masterNode ()

Private Member Functions
void	init (void)
void	heartbeat_callback (const messages::msg::Heartbeat::SharedPtr msg)
bool	getSystemOnlineState ()
void	print_system_status ()
void	check_heartbeats ()
void	switch_system_state (galaxis::serviceStates::State p_state, bool p_force)
void	callback_buttonState (ButtonStateMessageType::SharedPtr const buttonStateMsgSPtr)
void	handle_controller (bool start)
void	initial_set_ecu_settings ()
uint8_t	get_service_state_by_enum_state (galaxis::serviceStates::State p_state)
galaxis::serviceStates::State	get_service_state_by_uint8 (uint8_t p_state)

Private Attributes
std::map< std::string, std::pair< long long, galaxis::serviceStates::State > >	system_state_
std::string	nodeName
rclcpp::Publisher< ServiceStateType >::SharedPtr	service_state_pub_
rclcpp::Publisher< SettingsValuesType >::SharedPtr	settingsValuesPubSPtr
rclcpp::Subscription< HeartbeatMessageType >::SharedPtr	heartbeat_sub_
rclcpp::Subscription< ButtonStateMessageType >::SharedPtr	buttonStateSubSPtr
rclcpp::TimerBase::SharedPtr	periodic_timer_
rclcpp::TimerBase::SharedPtr	periodic_timer_print_system_status_
galaxis::serviceStates::State	m_currentServiceState = galaxis::serviceStates::State::BOOTSTRAP
galaxis::serviceStates::State	m_previousServiceState = galaxis::serviceStates::State::BOOTSTRAP
bool	m_systemOnlineStatus = false
bool	m_ecuOnlineStatus = false
bool	m_ecuInitStatus = false

Detailed Description

Definition at line 14 of file masterNode.hpp.

Constructor & Destructor Documentation

masterNode()

masterNode::masterNode

(

)

Definition at line 20 of file masterNode.cpp.

                       :
    Node("master_node") {
  this->nodeName       = this->get_name();
  std::string info_msg = "Initializing master_node";
 
  galaxis::utils::log(info_msg);
 
  init();
}

References init(), galaxis::utils::log(), and nodeName.

Here is the call graph for this function:

~masterNode()

masterNode::~masterNode

(

)

Definition at line 30 of file masterNode.cpp.

                        {
  std::string info_msg("Shutting down");
  galaxis::utils::log(info_msg);
}

References galaxis::utils::log().

Here is the call graph for this function:

Member Function Documentation

callback_buttonState()

void masterNode::callback_buttonState ( ButtonStateMessageType::SharedPtr const  buttonStateMsgSPtr )

private

Definition at line 174 of file masterNode.cpp.

                                                                                         {
  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
  }
}

References galaxis::serviceStates::BOOTSTRAP, cuprules::modes::training::erpm_max_rc, cuprules::modes::free_driving::erpm_max_rc, cuprules::modes::obstacle_driving::erpm_max_rc, galaxis::serviceStates::FREE_DRIVING, handle_controller(), galaxis::serviceStates::OBSTACLE_DRIVING, cuprules::modes::training::rc_use_erpm, cuprules::modes::free_driving::rc_use_erpm, cuprules::modes::obstacle_driving::rc_use_erpm, settingsValuesPubSPtr, and switch_system_state().

Referenced by init().

Here is the call graph for this function:

Here is the caller graph for this function:

check_heartbeats()

void masterNode::check_heartbeats ( )

private

Definition at line 119 of file masterNode.cpp.

                             {
  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();
  }
}

References galaxis::serviceStates::BOOTSTRAP, getSystemOnlineState(), m_ecuInitStatus, m_previousServiceState, m_systemOnlineStatus, and switch_system_state().

Referenced by init().

Here is the call graph for this function:

Here is the caller graph for this function:

get_service_state_by_enum_state()

uint8_t masterNode::get_service_state_by_enum_state ( galaxis::serviceStates::State  p_state )

private

Definition at line 281 of file masterNode.cpp.

                                                                             {
  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;
}

References galaxis::serviceStates::BOOTSTRAP, galaxis::serviceStates::FREE_DRIVING, and galaxis::serviceStates::OBSTACLE_DRIVING.

Referenced by switch_system_state().

Here is the caller graph for this function:

get_service_state_by_uint8()

galaxis::serviceStates::State masterNode::get_service_state_by_uint8 ( uint8_t  p_state )

private

Definition at line 259 of file masterNode.cpp.

                                                      {
  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;
}

References galaxis::serviceStates::BOOTSTRAP, galaxis::serviceStates::FREE_DRIVING, and galaxis::serviceStates::OBSTACLE_DRIVING.

getSystemOnlineState()

bool masterNode::getSystemOnlineState ( )

private

Definition at line 102 of file masterNode.cpp.

                                 {
  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;
}

References __get_time_now_ms(), _HEARTBEAT_MAXIMUM_OFF_TIME_MS, m_ecuOnlineStatus, and system_state_.

Referenced by check_heartbeats().

Here is the call graph for this function:

Here is the caller graph for this function:

handle_controller()

void masterNode::handle_controller ( bool  start )

private

Definition at line 169 of file masterNode.cpp.

                                        {
  // TODO?
}

Referenced by callback_buttonState().

Here is the caller graph for this function:

heartbeat_callback()

void masterNode::heartbeat_callback ( const messages::msg::Heartbeat::SharedPtr  msg )

private

Definition at line 153 of file masterNode.cpp.

                                                                      {
  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();
}

References __get_time_now_ms(), m_ecuOnlineStatus, and system_state_.

Referenced by init().

Here is the call graph for this function:

Here is the caller graph for this function:

init()

void masterNode::init ( void  )

private

Definition at line 36 of file masterNode.cpp.

                     {
  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();
}

References _HEARTBEAT_CHECK_PERIOD, galaxis::serviceStates::BOOTSTRAP, buttonStateSubSPtr, callback_buttonState(), check_heartbeats(), heartbeat_callback(), heartbeat_sub_, m_ecuOnlineStatus, periodic_timer_, periodic_timer_print_system_status_, print_system_status(), service_state_pub_, settingsValuesPubSPtr, and system_state_.

Referenced by masterNode().

Here is the call graph for this function:

Here is the caller graph for this function:

initial_set_ecu_settings()

void masterNode::initial_set_ecu_settings ( )

private

Definition at line 65 of file masterNode.cpp.

                                     {
  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;
  // }
}

print_system_status()

void masterNode::print_system_status ( )

private

Definition at line 83 of file masterNode.cpp.

                                {
  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());
}

References __get_time_now_ms(), _HEARTBEAT_MAXIMUM_OFF_TIME_MS, m_currentServiceState, and system_state_.

Referenced by init().

Here is the call graph for this function:

Here is the caller graph for this function:

switch_system_state()

void masterNode::switch_system_state ( galaxis::serviceStates::State  p_state, bool  p_force = false  )

private

Definition at line 140 of file masterNode.cpp.

                                                                                       {
  //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);
  //}
}

References get_service_state_by_enum_state(), m_currentServiceState, m_previousServiceState, and service_state_pub_.

Referenced by callback_buttonState(), and check_heartbeats().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

buttonStateSubSPtr

rclcpp::Subscription<ButtonStateMessageType>::SharedPtr masterNode::buttonStateSubSPtr

private

Definition at line 26 of file masterNode.hpp.

Referenced by init().

heartbeat_sub_

rclcpp::Subscription<HeartbeatMessageType>::SharedPtr masterNode::heartbeat_sub_

private

Definition at line 25 of file masterNode.hpp.

Referenced by init().

m_currentServiceState

galaxis::serviceStates::State masterNode::m_currentServiceState = galaxis::serviceStates::State::BOOTSTRAP

private

Definition at line 29 of file masterNode.hpp.

Referenced by print_system_status(), and switch_system_state().

m_ecuInitStatus

bool masterNode::m_ecuInitStatus = false

private

Definition at line 34 of file masterNode.hpp.

Referenced by check_heartbeats().

m_ecuOnlineStatus

bool masterNode::m_ecuOnlineStatus = false

private

Definition at line 33 of file masterNode.hpp.

Referenced by getSystemOnlineState(), heartbeat_callback(), and init().

m_previousServiceState

galaxis::serviceStates::State masterNode::m_previousServiceState = galaxis::serviceStates::State::BOOTSTRAP

private

Definition at line 30 of file masterNode.hpp.

Referenced by check_heartbeats(), and switch_system_state().

m_systemOnlineStatus

bool masterNode::m_systemOnlineStatus = false

private

Definition at line 31 of file masterNode.hpp.

Referenced by check_heartbeats().

nodeName

std::string masterNode::nodeName

private

Definition at line 22 of file masterNode.hpp.

Referenced by masterNode().

periodic_timer_

rclcpp::TimerBase::SharedPtr masterNode::periodic_timer_

private

Definition at line 27 of file masterNode.hpp.

Referenced by init().

periodic_timer_print_system_status_

rclcpp::TimerBase::SharedPtr masterNode::periodic_timer_print_system_status_

private

Definition at line 27 of file masterNode.hpp.

Referenced by init().

service_state_pub_

rclcpp::Publisher<ServiceStateType>::SharedPtr masterNode::service_state_pub_

private

Definition at line 23 of file masterNode.hpp.

Referenced by init(), and switch_system_state().

settingsValuesPubSPtr

rclcpp::Publisher<SettingsValuesType>::SharedPtr masterNode::settingsValuesPubSPtr

private

Definition at line 24 of file masterNode.hpp.

Referenced by callback_buttonState(), and init().

system_state_

std::map<std::string, std::pair<long long, galaxis::serviceStates::State> > masterNode::system_state_

private

Definition at line 20 of file masterNode.hpp.

Referenced by getSystemOnlineState(), heartbeat_callback(), init(), and print_system_status().

---

The documentation for this class was generated from the following files:

• nodes/MasterNode/include/masterNode.hpp
• nodes/MasterNode/src/masterNode.cpp