Send goal code

edit

So I am writing a code that sends goal to robot then send another goal but it is not working

Here is my code:

    #include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_datatypes.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <math.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

 float current_x,current_y; // current pose
 int i = 0;
void poseCallBack(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg){
current_x = msg->pose.pose.position.x;
current_y = msg->pose.pose.position.y;
 int i = 1;
}
    int main(int argc, char** argv){
 ros::init(argc, argv, "simple_navigation_goals");
 ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/amcl_pose", 1, poseCallBack); // subscribe to amcl_pose/current pose

 //tell the action client that we want to spin a thread by default
 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
 goal.target_pose.header.frame_id = "base_link";
if (i == 1)
{
 goal.target_pose.header.stamp = ros::Time::now();

 goal.target_pose.pose.position.x = 0.01;

 /*calculate angle*/
 double x = 0.01 - current_x;
 double y = 0 - current_y;
 double theta = atan2(y,x);
 // convert angle to quarternion
 tf::Quaternion quaternion;
 quaternion = tf::createQuaternionFromYaw(theta);
 geometry_msgs::Quaternion qMsg;
 tf::quaternionTFToMsg(quaternion, qMsg);
 // set quarternion to goal
 goal.target_pose.pose.orientation = qMsg;
 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");

 ros::spinOnce();
 }
  return 0;
}

basically what I want to do is calculate the quaternion by using the current pose by subscribing to the amcl_pose topic then I use the current pose and the goal pose to calculate the quaternion and sendgoal. But it didn't work and said trajectory error.

What did I do something wrong? Thank you!