ros_heartbeat.cpp

Go to the documentation of this file.

#include "galaxis/ros_heartbeat.hpp"
 
using namespace galaxis;
 
ROSHeartbeat::ROSHeartbeat(rclcpp::Node* p_nodePtr, std::string& p_nodeNameRef, unsigned int p_period) :
    m_nodePtr(p_nodePtr) {
  if(m_nodePtr == nullptr) {
    throw galaxis::Errors::POINTER_ERROR();
  }
 
  ::galaxis::ros::create_publisher(m_nodePtr, m_publisherSPtr);
 
  m_nodeName = p_nodeNameRef;
 
  // create timer for periodical heartbeating
  if(p_period > _HEARTBEAT_MAXIMUM_PERIOD_TIME) {
    RCLCPP_ERROR(m_nodePtr->get_logger(), "maximum heartbeat period time exceeded");
    p_period = _HEARTBEAT_MAXIMUM_PERIOD_TIME;
    // throw galaxis::Errors::UNDEFINED_ERROR();
  }
 
  m_period        = std::chrono::milliseconds{ p_period };
  m_periodicTimer = m_nodePtr->create_wall_timer(m_period, std::bind(&ROSHeartbeat::beat, this));
}
 
ROSHeartbeat::~ROSHeartbeat() {
  m_periodicTimer->cancel();
}
 
void ROSHeartbeat::beat(void) {
  // RCLCPP_INFO(m_nodePtr->get_logger(), "[HEARTBEAT]");
  auto messageUPtr                 = std::make_unique<HeartbeatMessageType>();
  messageUPtr->node_name           = m_nodeName;
  messageUPtr->service_state.state = m_serviceState;
  m_publisherSPtr->publish(std::move(messageUPtr));
}
 
void ROSHeartbeat::set_service_state(::galaxis::serviceStates::State const p_serviceState) {
  if(p_serviceState < 0 || p_serviceState >= ::galaxis::serviceStates::State::COUNT) {
    RCLCPP_ERROR(m_nodePtr->get_logger(), "service state not implemented");
    // throw galaxis::Errors::NOT_IMPLEMENTED_ERROR();
    return;
  }
 
  m_serviceState = p_serviceState;
}