ROS_gazebo 设置/获取模型状态 set_model_state/get_model_state

        用gazebo做仿真时，有时候需要初始化环境状态，或者想要不通过传感器直接获取模型状态，比如做机器人强化学习场景，失败后reset（）场景，step（）时获取状态，（reset和step是openai gym强化学习库中的标准化函数）。gazebo提供了相关的服务话题，可以非常方便的帮助我们set model state 和 get model state。

        服务名分别为（可以通过rosservice list查看）：

/gazebo/set_model_state
/gazebo/get_model_state

        我们可以通过多种方式set/get model state，1）命令行方式；2）C++/python方式。这些也是ROS中的常规操作。

1. 命令行方式

1.1. /gazebo/get_model_state

$ rosservice call gazebo/get_model_state '{model_name: blue_cube}'

解释：向服务端发送请求，获取blue_cube的状态，blue_cube是gazebo中Models中必须存在的模型。

将会得到：

pose: 
  position: 
    x: 0.799992373827
    y: 0.700044327224
    z: 0.848674788355
  orientation: 
    x: -0.000492196305707
    y: -0.000128524007798
    z: 1.46004051516e-08
    w: 0.999999870612
twist: 
  linear: 
    x: 0.0
    y: 0.0
    z: 0.0
  angular: 
    x: 0.0
    y: 0.0
    z: 0.0
success: True
status_message: GetModelState: got properties

这就是blue_cube的状态。

1.2. /gazebo/set_model_state

$ rosservice call /gazebo/set_model_state '{model_state: { model_name: blue_cube, pose: { position: { x: 0.6 , y: 0.4 , z: 0.83 }, orientation: {x: 0, y: -0.0032, z: 0, w: 1 } }, twist: { linear: {x: 0.0, y: 0.0, z: 0.0 } , angular: { x: 0.0, y: 0.0, z: 0.0 } } , reference_frame: world } }'

 解释：向服务端发送请求，设置blue_cube状态，可以看到需要输入的内容比较多，基本是节1.1中输入和输出的内容之和。

2. C++方式

通过ROS C++完成向服务端发送请求。这里需要知道srv类型，可以在下面目录中找到

/opt/ros/indigo/share/gazebo_msgs/srv

2.1. set_model_state code:

#include <ros/ros.h>
#include <gazebo_msgs/SetModelState.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "gazebo_set_states_client");

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

    gazebo_msgs::SetModelState objstate;

    objstate.request.model_state.model_name = "blue_cube";
    objstate.request.model_state.pose.position.x = 0.5;
    objstate.request.model_state.pose.position.y = 0.5;
    objstate.request.model_state.pose.position.z = 0.5;
    objstate.request.model_state.pose.orientation.w = 1;
    objstate.request.model_state.pose.orientation.x = 0;
    objstate.request.model_state.pose.orientation.y = 0;
    objstate.request.model_state.pose.orientation.z = 0;
    objstate.request.model_state.twist.linear.x = 0.0;
    objstate.request.model_state.twist.linear.y = 0.0;
    objstate.request.model_state.twist.linear.z = 0.0;
    objstate.request.model_state.twist.angular.x = 0.0;
    objstate.request.model_state.twist.angular.y = 0.0;
    objstate.request.model_state.twist.angular.z = 0.0;
    objstate.request.model_state.reference_frame = "world";

    client.call(objstate);
}

2.2. get_model_state 略

3. Python方式

3.1. set_model_state code

#!/usr/bin/env python
#coding=utf-8

import rospy
from gazebo_msgs.srv import *

rospy.init_node('p_robot_env')
rospy.wait_for_service('/gazebo/set_model_state')

set_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
objstate = SetModelStateRequest()  # Create an object of type SetModelStateRequest

# set red cube pose
objstate.model_state.model_name = "blue_cube"
objstate.model_state.pose.position.x = 0.5
objstate.model_state.pose.position.y = 0.5
objstate.model_state.pose.position.z = 0.5
objstate.model_state.pose.orientation.w = 1
objstate.model_state.pose.orientation.x = 0
objstate.model_state.pose.orientation.y = 0
objstate.model_state.pose.orientation.z = 0
objstate.model_state.twist.linear.x = 0.0
objstate.model_state.twist.linear.y = 0.0
objstate.model_state.twist.linear.z = 0.0
objstate.model_state.twist.angular.x = 0.0
objstate.model_state.twist.angular.y = 0.0
objstate.model_state.twist.angular.z = 0.0
objstate.model_state.reference_frame = "world"

result = set_state_service(objstate)

3.2. get_model_state 

#!/usr/bin/env python

import rospy
import gazebo_msg.srv import *

get_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
model = GetModelStateRequest()
model.model_name = 'blue_cube'
objstate = get_state_service(model)
state = (objstate.pose.position.x, objstate.pose.position.y, objstate.pose.position.z)