turtlebot simulation: controller via client-node [closed]

edit

hello guys. i am trying to simulate a control problem for turtlebot in gazebo. the controller takes the pose of the robot in each timestep and produces the linear and angular velocities until the robot reachew it's goal. so i made a client node, in order to rosrun it after i roslaunch the turtlebot in an empty world. unfortunatelly, when i rosrun my node i get no response from the robot, and the following warning:

Gazebo ROS Create plugin. NaN in d2. Step time: 0.03. WD: 0.07. Velocity: -nan.

i think the problem is that my client program doesn't get the robot pose properly. here is my client code. please help me. thank u in advance. cheers!

#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;

ros::ServiceClient gmscl=n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");

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

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


  gmscl.waitForExistence();
  gphspro.waitForExistence();
  client.waitForExistence();

    geometry_msgs::Pose pose;

    geometry_msgs::Twist twist;

pose.position.x=0.0;
pose.position.y=0.0;
pose.orientation.z=0.0;

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

gazebo_msgs::GetModelState getmodelstate;
    getmodelstate.request.model_name ="turtlebot";
    gmscl.call(getmodelstate);

gazebo_msgs::GetPhysicsProperties getphysicsproperties;
    gphspro.call(getphysicsproperties);


    geometry_msgs::Pose pose;

    geometry_msgs::Twist twist;

    t=0.1;

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

ROS_INFO("position.x = %f",pose.position.x);
ROS_INFO("position.y = %f",pose.position.y);
ROS_INFO("orientation = %f",pose.orientation.z);


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

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

f = Xfinal[2] - 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;

    pose.position.z = 0.0;
    pose.orientation.x = 0.0;
    pose.orientation.y = 0.0;

 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_INFO("=========================================");  
ROS_INFO("position.x(2) = %f",pose ...