#include "amr_control/amr_monitor.h"

namespace amr_control
{
  AmrMonitor::AmrMonitor() {}

  AmrMonitor::AmrMonitor(const ros::NodeHandle &nh)
  {
    if (!initalized_)
      this->init(nh);
  }

  AmrMonitor::~AmrMonitor()
  {
    if (odom_sub_)
      odom_sub_.reset();
  }

  void AmrMonitor::init(const ros::NodeHandle &nh)
  {
    if (!initalized_)
    {
      ros::NodeHandle nh_core;
      odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh_core);
      initalized_ = true;
    }
  }

  bool AmrMonitor::getVelocity(nav_2d_msgs::Twist2D &velocity)
  {
    if (!odom_sub_)
      return false;
    else
      velocity = odom_sub_->getTwist();
    return true;
  }
}