用gazebo做仿真时，有时候需要初始化环境状态，或者想要不通过传感器直接获取模型状态，比如做机器人强化学习场景，失败后reset（）场景，step（）时获取状态，（reset和step是openai gym强化学习库中的标准化函数）。gazebo提供了相关的服务话题，可以非常方便的帮助我们set model state 和 get model state。
服务名分别为（可以通过rosservice list查看）：
我们可以通过多种方式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)
2474
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
