i want to get the initial pose in x,y,theta coordinates of turtlebot in gazebo [closed]

edit

hello. i am trying to simulate a parking problem of the turtlebot in gazebo. so i made a client node, but i dont know hot to get the initial pose of the turtlebot! can anyone help me??? i roslaunch the turtlebot in an empty world first, and afterwards i am trying to rosrun my client node. here is the client node:

#include "ros/ros.h"
#include "gazebo_msgs/SetModelState.h"
#include "gazebo_msgs/GetModelState.h"
#include "gazebo_msgs/GetPhysicsProperties.h"

int main(int argc, char** argv)

{

    ros::init(argc, argv, "kouna_to_asistola2");

    ros::NodeHandle n;


double f, t, th, a, x, y, e, p, csa, sna, u, Xfinal[3];

double pi=3.141592654;

int g = 3;
int h = 1;
int k = 6;

int i;



for(i=1; i<100000; i++)
{

ros::ServiceClient gmscl=n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
    gazebo_msgs::GetModelState getmodelstate;
    getmodelstate.request.model_name ="turtlebot";
    gmscl.call(getmodelstate);

    ros::ServiceClient gphspro=n.serviceClient<gazebo_msgs::GetPhysicsProperties>("/gazebo/get_physics_properties");
    gazebo_msgs::GetPhysicsProperties getphysicsproperties;
    gphspro.call(getphysicsproperties);

    geometry_msgs::Pose pose;

    geometry_msgs::Twist twist;

    t=0.001;

Xfinal[0]=6.0;
Xfinal[1]=7.0;
Xfinal[2]=86.0;

x = Xfinal[0] - getmodelstate.pose.position.x;  

y = Xfinal[1] - getmodelstate.pose.position.y;  

f = Xfinal[2] - getmodelstate.pose.orientation.z; 

p = atan2(y,x);
p = p*180/pi;

th = Xfinal[2] - p;

a = th - f;

if(a>0)
a=-a;

csa = cos(a*pi/180);
sna = sin(a*pi/180);

e = sqrt(x*x + y*y);


u = (e*g*csa);


twist.angular.z = (k*a + ((g*csa*sna*(a + h*th))/a));

ROS_INFO("x = %f",x);
ROS_INFO("y = %f",y);
ROS_INFO("a = %f",a);
ROS_INFO("csa = %f",csa);
ROS_INFO("sna = %f",sna);
ROS_INFO("e = %f",e);
ROS_INFO("u = %f",u);



  if(pose.position.x <= Xfinal[0])
    pose.position.x = getmodelstate.response.pose.position.x+cos(pose.orientation.z*pi/180)*(u*t);
  else
    pose.position.x = pose.position.x;

  if(pose.position.y <= Xfinal[1])
    pose.position.y = getmodelstate.response.pose.position.y+sin(pose.orientation.z*pi/180)*(u*t);
  else
    pose.position.y = pose.position.y;


 if(pose.orientation.z <= Xfinal[2])
    pose.orientation.z = getmodelstate.response.pose.position.z+(twist.angular.z*t);
 else
    pose.orientation.z = pose.orientation.z;


    ros::ServiceClient client = n.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");

    gazebo_msgs::SetModelState setmodelstate;
    gazebo_msgs::ModelState modelstate;
    modelstate.model_name ="turtlebot";
    modelstate.pose = pose;
    modelstate.twist = twist;

    setmodelstate.request.model_state=modelstate;        

     if (client.call(setmodelstate))
      {
       ROS_INFO("BRILLIANT AGAIN!!!");
       ROS_INFO("%f",getphysicsproperties.response.time_step);
       ROS_INFO("%f",modelstate.pose.position.x);
       ROS_INFO("%f",modelstate.pose.position.y);
       ROS_INFO("%f",modelstate.twist.linear.x);
       ROS_INFO("%f",getmodelstate.response.pose.position.y);
      }
     else
      {
       ROS_ERROR("Failed to call service for setmodelstate ");
       return 1;
      }

  }

}

i want to get the initial pose.x , y and orientation, and put them in:

x = Xfinal[0] - getmodelstate.pose.position.x;

y = Xfinal[1] - getmodelstate.pose.position.y;

f = Xfinal[2] - getmodelstate.pose.orientation.z;

so i can start the control...................