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

版权声明：本文为CSDN博主「penge666」的原创文章，遵循CC 4.0 by-sa版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/penge666/article/details/87900911

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

 ————————————————  
版权声明：本文为CSDN博主「penge666」的原创文章，遵循CC 4.0 by-sa版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/penge666/article/details/87900911