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

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

        服务名分别为(可以通过rosservice list查看):


 
 
  1. /gazebo/set_model_state
  2. /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中必须存在的模型。

将会得到:


 
 
  1. pose:
  2. position:
  3. x: 0 .799992373827
  4. y: 0 .700044327224
  5. z: 0 .848674788355
  6. orientation:
  7. x: -0 .000492196305707
  8. y: -0 .000128524007798
  9. z: 1 .46004051516e-08
  10. w: 0 .999999870612
  11. twist:
  12. linear:
  13. x: 0 .0
  14. y: 0 .0
  15. z: 0 .0
  16. angular:
  17. x: 0 .0
  18. y: 0 .0
  19. z: 0 .0
  20. success: True
  21. 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:


 
 
  1. #include <ros/ros.h>
  2. #include <gazebo_msgs/SetModelState.h>
  3. int main( int argc, char **argv)
  4. {
  5. ros::init(argc, argv, "gazebo_set_states_client");
  6. ros::NodeHandle n;
  7. ros::ServiceClient client = n.serviceClient<gazebo_msgs::SetModelState>( "/gazebo/set_model_state");
  8. gazebo_msgs::SetModelState objstate;
  9. objstate. request.model_state.model_name = "blue_cube";
  10. objstate. request.model_state.pose.position.x = 0.5;
  11. objstate. request.model_state.pose.position.y = 0.5;
  12. objstate. request.model_state.pose.position.z = 0.5;
  13. objstate. request.model_state.pose.orientation.w = 1;
  14. objstate. request.model_state.pose.orientation.x = 0;
  15. objstate. request.model_state.pose.orientation.y = 0;
  16. objstate. request.model_state.pose.orientation.z = 0;
  17. objstate. request.model_state.twist.linear.x = 0.0;
  18. objstate. request.model_state.twist.linear.y = 0.0;
  19. objstate. request.model_state.twist.linear.z = 0.0;
  20. objstate. request.model_state.twist.angular.x = 0.0;
  21. objstate. request.model_state.twist.angular.y = 0.0;
  22. objstate. request.model_state.twist.angular.z = 0.0;
  23. objstate. request.model_state.reference_frame = "world";
  24. client. call(objstate);
  25. }

2.2. get_model_state 略

3. Python方式

3.1. set_model_state code


 
 
  1. #!/usr/bin/env python
  2. #coding=utf-8
  3. import rospy
  4. from gazebo_msgs.srv import *
  5. rospy.init_node( 'p_robot_env')
  6. rospy.wait_for_service( '/gazebo/set_model_state')
  7. set_state_service = rospy.ServiceProxy( '/gazebo/set_model_state', SetModelState)
  8. objstate = SetModelStateRequest() # Create an object of type SetModelStateRequest
  9. # set red cube pose
  10. objstate.model_state.model_name = "blue_cube"
  11. objstate.model_state.pose.position.x = 0.5
  12. objstate.model_state.pose.position.y = 0.5
  13. objstate.model_state.pose.position.z = 0.5
  14. objstate.model_state.pose.orientation.w = 1
  15. objstate.model_state.pose.orientation.x = 0
  16. objstate.model_state.pose.orientation.y = 0
  17. objstate.model_state.pose.orientation.z = 0
  18. objstate.model_state.twist.linear.x = 0.0
  19. objstate.model_state.twist.linear.y = 0.0
  20. objstate.model_state.twist.linear.z = 0.0
  21. objstate.model_state.twist.angular.x = 0.0
  22. objstate.model_state.twist.angular.y = 0.0
  23. objstate.model_state.twist.angular.z = 0.0
  24. objstate.model_state.reference_frame = "world"
  25. result = set_state_service(objstate)

3.2. get_model_state 


 
 
  1. #!/usr/bin/env python
  2. import rospy
  3. import gazebo_msg.srv import *
  4. get_state_service = rospy.ServiceProxy( '/gazebo/get_model_state', GetModelState)
  5. model = GetModelStateRequest()
  6. model.model_name = 'blue_cube'
  7. objstate = get_state_service(model)
  8. state = (objstate.pose.position.x, objstate.pose.position.y, objstate.pose.position.z)

 

  • 1
    点赞
  • 11
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值