#include "amr_control/amr_subcriber.h"
#include <geometry_msgs/PoseStamped.h>

amr_control::AmrSubscriber::AmrSubscriber(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
  : nh_(nh), move_base_ptr_(move_base_ptr)
{   
    robot::log_info("[%s:%d] Initializing AmrSubscriber namespace: %s", __FILE__, __LINE__, nh_.getNamespace().c_str());
    ros::NodeHandle nh_move_base_simple;
    move_base_simple_sub_ = nh_move_base_simple.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this);
}

amr_control::AmrSubscriber::~AmrSubscriber()
{
}

void amr_control::AmrSubscriber::moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  robot_geometry_msgs::PoseStamped goal;
  goal.header.stamp = robot::Time::now();
  goal.header.frame_id = msg->header.frame_id;
  goal.pose.position.x = msg->pose.position.x;
  goal.pose.position.y = msg->pose.position.y;
  goal.pose.orientation.x = msg->pose.orientation.x;
  goal.pose.orientation.y = msg->pose.orientation.y;
  goal.pose.orientation.z = msg->pose.orientation.z;
  goal.pose.orientation.w = msg->pose.orientation.w;
  move_base_ptr_->moveTo(goal, 0.02, 0.02);
}