0
$\begingroup$

Rosanswers logo

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

Originally posted by Bastian on ROS Answers with karma: 71 on 2011-11-28

Post score: 4

Original comments

Comment by JohnDoe on 2022-10-28:
Nice! But searching the service name should be conducted with 'rosservice list'.

Improve this question
$\endgroup$

3 Answers 3

Reset to default
0
$\begingroup$

Rosanswers logo

Since on the commandline, you're just calling a commandline version of a ROS service, to do it from a ROS node, you want to use the service SetModelState and call it via roscpp. The tutorial on how to do this can be found here.

EDIT: The gazebo/SetModelState documentation says to use the service located in the package gazebo_msgs instead of the one in the gazebo package.

Thus, your code should look like this:

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

The main reason your code wouldn't compile is because you used <gazebo::ModelState> instead of <gazebo::SetModelState>, just so you know what that error was.

EDIT2: If you're using Diamondback, use the gazebo package instead of the gazebo_msgs package. I apologize for the mistake. You should then use:

#include <gazebo/SetModelState.h>
...
ros::ServiceClient client = n.serviceClient<gazebo::SetModelState>("/gazebo/SetModelState");

There is no message type "ModelState". There is a class ModelState, but the message is called SetModelState.

Originally posted by DimitriProsser with karma: 11163 on 2011-11-28

This answer was ACCEPTED on the original site

Post score: 6

Original comments

Comment by Bastian on 2011-11-29:
Thanks for your help! I think there is one little mistake in your solution, the name of the service is wrong. The correct name is '/gazebo/set_model_state'.

Comment by DimitriProsser on 2011-11-28:
Yes, you'll need to make sure that you also add the "gazebo_msgs" package to your manifest.xml

Comment by Bastian on 2011-11-28:
Thanks for the explanation. Now I get the error that gazebo_msgs is not declared. I found info about gazebo_msgs here: http://www.ros.org/wiki/gazebo_msgs . Am I right, that I need to install this and include the corresponding .h?

Comment by Bastian on 2011-11-28:
Thanks for the hint, I will do so in the future.

Comment by dornhege on 2011-11-28:
You can edit/update your original code for follow-ups.

Comment by Bastian on 2011-11-28:
Sorry, I think I have to post this as answer to keep it readable.

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

Thanks for the links. Unfortunately, I'm still not successful. These are my approaches:

gazebo::ModelState modelstate;
// ... fill modelstate with data ...

// First approach
ros::service::call("SetModelState", modelstate);

// Second approach
ros::ServiceClient client = n.serviceClient<gazebo::ModelState>("SetModelState");
client.call(modelstate);

Both didn't work and gave errors when compiling (I can post them of course if this helps).

Originally posted by Bastian with karma: 71 on 2011-11-28

This answer was NOT ACCEPTED on the original site

Post score: 1

Original comments

Comment by Maurizio88 on 2012-02-08:
i didn't understand one thing. i have to write n.serviceClientgazebo::SetModelState or n.serviceClientgazebo::ModelState ? and ("/gazebo/set_model_state") or ("/gazebo/SetModelState")?

Comment by DimitriProsser on 2011-11-28:
The first thing I notice is the service name. I've noticed that most instances of Gazebo are launched under the namespace /gazebo. As such, your service call should call "/gazebo/SetModelState" instead of "SetModelState". Also, what are your compile errors?

Comment by dornhege on 2011-11-28:
You need to call the right name /gazebo/set_model_state instead of SetModelState.

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

I work with these code, and maybe you can give it a try:

#include <ros/ros.h>
//引入的是ROS Service的type
//因此在開新專案的時候，就需要引入Service端的Package相依性
//格式為<Server的Package名稱/Service名稱.h>
#include "gazebo_msgs/SetModelState.h"

using namespace std;

int main(int argc,char **argv)
{
    //初使化ROS的Client端
    ros::init(argc,argv,"move_pr2_by_magic_node"); //此字串不能有空白
    //建立一個Client端呼叫/gazebo/set_model_state來瞬移PR2的位置
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
    
    //設定PR2 Position
    geometry_msgs::Point pr2_position;
    pr2_position.x = 1.0;
    pr2_position.y = 0.0;
    pr2_position.z = 0.0;
    //設定PR2 orientation
    geometry_msgs::Quaternion pr2_orientation;
    pr2_orientation.x = 0.0;
    pr2_orientation.y = 0.0;
    pr2_orientation.z = 0.0;
    pr2_orientation.w = 1.0;
    
    //設定PR2 pose (Pose + Orientation)
    geometry_msgs::Pose pr2_pose;
    pr2_pose.position = pr2_position;
    pr2_pose.orientation = pr2_orientation;
    
    //設定ModelState
    gazebo_msgs::ModelState pr2_modelstate;
    pr2_modelstate.model_name = (std::string) "pr2";
    pr2_modelstate.pose = pr2_pose;
    
    //準備設定gazebo中PR2瞬移後的位置pose
    gazebo_msgs::SetModelState srv;
    srv.request.model_state = pr2_modelstate;
    
    //跟Server端連線，送出PR2要瞬移的位置
    if(client.call(srv))
    {
        ROS_INFO("PR2's magic moving success!!");
    }
    else
    {
        ROS_ERROR("Failed to magic move PR2! Error msg:%s",srv.response.status_message.c_str());
    }
    return 0;
}

Originally posted by sam with karma: 2570 on 2015-09-17

This answer was NOT ACCEPTED on the original site

Post score: 2

Original comments

Comment by Zheng on 2017-08-29:
Nice code. Works with ROS Kinetic and Gazebo 7.0.0 and Catkin Tools. Maybe you should remind people to add "gazebo_msgs" into CMakeLists.txt and package.xml as dependencies.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.