Ask Your Question
0

Position in gazebo

asked 2012-03-13 10:44:56 -0500

Penny gravatar image

updated 2013-12-16 02:18:29 -0500

tfoote gravatar image

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;
   }
edit retag flag offensive close delete

2 answers

Sort by ยป oldest newest most voted
1

answered 2012-03-14 10:18:30 -0500

kmaroney gravatar image

It seems that your client is only running once and exiting because you are not using ros::spin().

Please refer to the documentation:

Callbacks and Spinning

edit flag offensive delete publish link more
1

answered 2012-03-13 14:54:10 -0500

Robert Krug gravatar image

updated 2012-03-13 15:24:15 -0500

Hi,

When you create your gazebo_msgs::GetPhysicsProperties getphysicsproperties object with the default constructor, all values will be initialized to zero (including time_step). In order to get the current properties from Gazebo you first have to call the /gazebo/get_physics_properties service with getphysicsproperties as argument. Also, in order to get the current model state from Gazebo you have to call the /gazebo/get_model_state service with a gazebo_msgs/GetModelState object as argument:

ros::ServiceClient  pp_c = n.serviceClient<gazebo_msgs::GetPhysicsProperties>("/gazebo/get_physics_properties");
pp_c.call(getphysicsproperties);//getphysicsproperties now holds the current properties

//create your desired model state and call the /gazebo/set_model_state service here

gazebo_msgs::GetModelState gms;
gms.request.model_name="my_robot";
ros::ServiceClient gms_c = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
gms_c.call(gms);//gms now holds the current state
edit flag offensive delete publish link more

Comments

I have corrected my client and replaced the old one with the new one above, now I get that step_size is 0.001 but still my values change only the first time, and also I get the following error now: [ERROR] [1331734373.311149845, 367.274000000]: GetModelState: model [] does not exist

Penny ( 2012-03-14 09:18:03 -0500 )edit

Did you spawn your model in Gazebo before trying to execute the service calls? Also, as mentioned in the answer below, you have to spin so the callbacks are getting called.

Robert Krug ( 2012-03-14 11:01:48 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

[hide preview]

Stats

Asked: 2012-03-13 10:44:56 -0500

Seen: 697 times

Last updated: Dec 16 '13