#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;
  move_base_simple_sub_ = nh_move_base.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this);
  odometry_sub_ = nh_move_base.subscribe("/odom", 1, &AmrSubscriber::odometryCallback, 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);
}

void amr_control::AmrSubscriber::odometryCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
  if (!move_base_ptr_)
  {
    return;
  }
  robot_nav_msgs::Odometry odometry;
  odometry.header.stamp = robot::Time::now();
  odometry.header.frame_id = msg->header.frame_id;
  odometry.child_frame_id = msg->child_frame_id;
  odometry.pose.pose.position.x = msg->pose.pose.position.x;
  odometry.pose.pose.position.y = msg->pose.pose.position.y;
  odometry.pose.pose.position.z = msg->pose.pose.position.z;
  odometry.pose.pose.orientation.x = msg->pose.pose.orientation.x;
  odometry.pose.pose.orientation.y = msg->pose.pose.orientation.y;
  odometry.pose.pose.orientation.z = msg->pose.pose.orientation.z;
  odometry.pose.pose.orientation.w = msg->pose.pose.orientation.w;
  odometry.twist.twist.linear.x = msg->twist.twist.linear.x;
  odometry.twist.twist.linear.y = msg->twist.twist.linear.y;
  odometry.twist.twist.angular.z = msg->twist.twist.angular.z;
  move_base_ptr_->addOdometry("odometry", odometry);
}