Position in gazebo

edit

I made a client, but when I rosrun it, I do not get any errors but I do not get either what I want.

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

  int main(int argc, char** argv)
   {

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

    ros::NodeHandle n;

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

    double t=0.1;

    t=getphysicsproperties.response.time_step + t;

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

    geometry_msgs::Twist twist;
    twist.linear.x = 2.0;
    twist.linear.y = 2.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;

    geometry_msgs::Pose pose;
    pose.position.x = getmodelstate.response.pose.position.x + getphysicsproperties.response.time_step*twist.linear.x;
    pose.position.y = getmodelstate.response.pose.position.y+3.0;
    pose.position.z = getmodelstate.response.pose.position.z+4.5;
    pose.orientation.x = 0.0;
    pose.orientation.y = 0.0;
    pose.orientation.z = 0.0;

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

    gazebo_msgs::ModelState modelstate;
    modelstate.model_name ="my_robot";
    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;
      }



     return 0;
   }