MMNode.cpp

Go to the documentation of this file.

#include <MMNode.hpp>
#include <galaxis/node_ros_pubsub_wrapper.hpp>
 
MMNode* MMNode::m_instancePtr = nullptr;
 
MMNode::MMNode() :
    Node("MarvelmindNode") {
 
  m_instancePtr = this;
  node_name_    = "MarvelmindNode";
 
  galaxis::ros::create_publisher(this, m_positionPubSPtr);
 
  m_hedgePtr                         = createMarvelmindHedge();
  m_hedgePtr->ttyFileName            = DEFAULT_TTY_FILENAME;
  m_hedgePtr->verbose                = true;
  m_hedgePtr->anyInputPacketCallback = MMNode::on_mm_callback;
 
  if(!m_hedgePtr) {
    return;
  }
 
  startMarvelmindHedge(m_hedgePtr);
  printStationaryBeaconsPositionsFromMarvelmindHedge(m_hedgePtr, false);
 
  RCLCPP_INFO(this->get_logger(), "Node initialized, starting operation.");
}
 
MMNode::~MMNode() {
  m_instancePtr = nullptr;
}
 
void MMNode::on_mm_callback() {
  auto msg = std::make_shared<GPSDataType>();
 
  if(MMNode::m_instancePtr == nullptr) {
    std::cout << "m_instancePtr is nullptr" << std::endl;
    throw galaxis::Errors::POINTER_ERROR();
  }
 
  FusionIMUValue fusion;
  PositionValue position;
 
  getPositionFromMarvelmindHedge(MMNode::m_instancePtr->m_hedgePtr, &position);
  getFusionIMUFromMarvelmindHedge(MMNode::m_instancePtr->m_hedgePtr, &fusion);
 
  msg->gps_pos.x = static_cast<float>(position.x) / 1000.f;
  msg->gps_pos.y = static_cast<float>(position.y) / 1000.f;
 
  msg->fus_pos.x = static_cast<float>(fusion.x) / 1000.f;
  msg->fus_pos.y = static_cast<float>(fusion.y) / 1000.f;
 
  msg->timestamp = fusion.timestamp;
 
  RCLCPP_INFO(m_instancePtr->get_logger(), "Publish position: %f , %f", msg->gps_pos.x, msg->gps_pos.y);
  RCLCPP_INFO(m_instancePtr->get_logger(), "Publish fusion position: %f , %f", msg->fus_pos.x, msg->fus_pos.y);
 
  MMNode::m_instancePtr->publish_data(msg);
}
 
void MMNode::publish_data(const GPSDataType::SharedPtr p_msgSPtr) {
  if(p_msgSPtr == nullptr) {
    RCLCPP_ERROR(this->get_logger(), "p_msgSPtr is nullptr");
    throw galaxis::Errors::POINTER_ERROR();
  }
 
  auto msgUPtr = std::make_unique<GPSDataType>(*p_msgSPtr);
 
  m_positionPubSPtr->publish(std::move(msgUPtr));
}