#include <iostream>

#include <ros/ros.h>
#include "tf2_msgs/TFMessage.h"
#include <tf/tf.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
 
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "send_goal");
    MoveBaseClient ac("move_base", true);
 
  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }
 
  move_base_msgs::MoveBaseGoal goal;
 
  //we'll send a goal to the robot to move 1 meter forward

    char c='f';
    std::cout<<" input direction(f/q):";
    std::cin>>c;


  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();
  if(c=='f')
  {
    goal.target_pose.pose.position.x = 3.0;
    goal.target_pose.pose.position.y = 1.25;
    goal.target_pose.pose.orientation.w = 1.0;
    /*goal.target_pose.pose.orientation.z = -1.0;
    goal.target_pose.pose.orientation.w = 0;*/
  }
  else
  {
    goal.target_pose.pose.position.x = 1.0;
    goal.target_pose.pose.position.y = 1.0;
    goal.target_pose.pose.orientation.z = -1.0;
    goal.target_pose.pose.orientation.w = 0;
   
   // goal.target_pose.pose.orientation.w=1.0;

  }
  
  
 
  ROS_INFO("Sending goal");
  ac.sendGoal(goal);
 
  ac.waitForResult();
 
  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Hooray, the base moved 1 meter forward");
  else
    ROS_INFO("The base failed to move forward 1 meter for some reason");
 
  return 0;

}