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("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/set_model_state");
gazebo::SetModelState setmodelstate;
setmodelstate.request.model_state = modelstate;
client.call(setmodelstate);
↧