Calling gazebo service set_model_state in C++ Code

edit

Hi,

I'm trying to set the state of a robot in simulation. In a terminal, this works:

rosservice call /gazebo/set_model_state '{model_state: { model_name: my_robot, pose: { position: { x: 0.0, y: 0.0 ,z: 0.1 }, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0 } }, twist: { linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 } } , reference_frame: world } }'

Now I want to do the same in C++ Code. Can someone give me a hint, how I can achieve this?

Thanks in advance!

It works, thanks for your help!!

If someone runs into to same problem, here is the complete solution: First, how to do it in general (I know that there are tutorials, maybe this helps anyway); then I will post the concrete code.

Search with 'rostopic list' the service you want to call, you see the service names. With 'rosservice type name_of_service' you get the message_type. In message_type, you need to replace "/" by "::" for the usage below. In your code, you can write

ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<message_type>("name_of_service");

Now you need to create a message of the message type, i.e.

message_type my_message;

(still with "::" instead of "/" in message_type) To find out what the service wants as input and returns as output, search for the file short_message_type.srv, where short_message_type.srv is the part after the most right "::", see concrete example at the buttom. [Probably this is also possible with a ros command in terminal.] In this file, you find names and types of the arguments of the service. You need to put the input arguments in my_message.request. So if there is a input argument of type double named 'number', you need to set something like my_message.request.number=42. Now you can call the service with

client.call(my_message);

After that, you will find in my_message.response what the service has returned (output arguments).

For my problem (under diamondback), the complete code is:

ros::init(argc, argv, "my_node");
ros::NodeHandle n;
geometry_msgs::Pose start_pose;
start_pose.position.x = 0.0;
start_pose.position.y = 0.0;
start_pose.position.z = 0.1;
start_pose.orientation.x = 0.0;
start_pose.orientation.y = 0.0;
start_pose.orientation.z = 0.0;
start_pose.orientation.w = 0.0;

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

gazebo::ModelState modelstate;
modelstate.model_name = (std::string) "my_robot";
modelstate.reference_frame = (std::string) "world";
modelstate.pose = start_pose;
modelstate.twist = start_twist;

ros::ServiceClient client = n.serviceClient<gazebo::SetModelState>("/gazebo/set_model_state");
gazebo::SetModelState setmodelstate;
setmodelstate.request.model_state = modelstate;
client.call(setmodelstate);