Ask Your Question
4

Calling gazebo service set_model_state in C++ Code

asked 2011-11-28 01:02:55 -0500

Bastian gravatar image

updated 2014-01-28 17:10:53 -0500

ngrennan gravatar image

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

7 Answers

Sort by » oldest newest most voted
6

answered 2011-11-28 01:08:41 -0500

DimitriProsser gravatar image

updated 2011-11-29 00:02:12 -0500

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.

edit flag offensive delete link more

Comments

Sorry, I think I have to post this as answer to keep it readable.
Bastian gravatar imageBastian ( 2011-11-28 02:19:59 -0500 )edit
You can edit/update your original code for follow-ups.
dornhege gravatar imagedornhege ( 2011-11-28 02:24:49 -0500 )edit
Thanks for the hint, I will do so in the future.
Bastian gravatar imageBastian ( 2011-11-28 02:30:11 -0500 )edit
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?
Bastian gravatar imageBastian ( 2011-11-28 03:29:00 -0500 )edit
Yes, you'll need to make sure that you also add the "gazebo_msgs" package to your manifest.xml
DimitriProsser gravatar imageDimitriProsser ( 2011-11-28 03:41:15 -0500 )edit
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'.
Bastian gravatar imageBastian ( 2011-11-29 01:38:50 -0500 )edit
2

answered 2015-09-17 04:03:22 -0500

sam gravatar image

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;
}
edit flag offensive delete link more

Comments

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.

Zheng gravatar imageZheng ( 2017-08-29 20:19:21 -0500 )edit
1

answered 2011-11-28 02:20:56 -0500

Bastian gravatar image

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

edit flag offensive delete link more

Comments

You need to call the right name /gazebo/set_model_state instead of SetModelState.
dornhege gravatar imagedornhege ( 2011-11-28 02:24:30 -0500 )edit
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?
DimitriProsser gravatar imageDimitriProsser ( 2011-11-28 02:25:55 -0500 )edit
i didn't understand one thing. i have to write n.serviceClient<gazebo::SetModelState> or n.serviceClient<gazebo::ModelState> ? and ("/gazebo/set_model_state") or ("/gazebo/SetModelState")?
Maurizio88 gravatar imageMaurizio88 ( 2012-02-08 02:59:27 -0500 )edit
0

answered 2012-06-05 05:24:33 -0500

jdarango gravatar image

I had the same error. You should check your Makefile, this solved my problem: echo 'include $(shell rospack find mk)/cmake.mk' > Makefile

edit flag offensive delete link more
0

answered 2012-01-21 00:35:19 -0500

abasit gravatar image

when I call gazebo header from my c++ code I get the following error. gazebo_msgs/SetModelState.h: No such file or directory compilation terminated. What am I missing to add to compile correctly. Thanks for your help in advance

edit flag offensive delete link more

Comments

Did you include `<depend package="gazebo_msgs" />` in your manifest.xml?
Maurizio88 gravatar imageMaurizio88 ( 2012-02-08 02:57:08 -0500 )edit
0

answered 2012-06-05 05:21:43 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

I had the same error. You should check your Makefile. This solved my problem: echo include $(shell rospack find mk)/cmake.mk > Makefile

edit flag offensive delete link more
0

answered 2012-02-08 09:13:34 -0500

hsu gravatar image

here's a similar post

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2011-11-28 01:02:55 -0500

Seen: 5,503 times

Last updated: Sep 17 '15