Gazebo和ROS通信实现发球

ROS版本： ROS Kinetic
操作系统：Ubuntu16.04 LTS

YouTube上有一个视频，是一个乒乓球机器人在gazebo里面的方针
https://www.youtube.com/watch?v=HBNGeYZKJM4
因为项目需要，需要实现这个发球的装置。因为第一次接触gazebo和ros所以这里把自己的研究过程记录下来。

预备知识

1.ros的安装和通信机制，主要是话题和服务通信。
这里只需要根据根据ros wiki上的教程实现前20讲，就可以实现安装（包括gazebo_ros包）和简单入门。
http://wiki.ros.org/ROS/Tutorials
2.gazebo的一些基础知识（非必须），这里可以看一下gazebo的入门教程
http://gazebosim.org/tutorials

总体实现思路

整个方针的实现可以分为以下几步：
###环境的搭建
这里我们并不采用gazebo教程里面直接gui插入模型的方法，而是采用ros和gazebo的通信机制实现。如果我们按前面的方法正确安装了ros。接下来，打开终端，依次执行下列命令，每个命令可以打开一个终端
$roscore
$rosrun gazebo_ros gazebo
这事我们就打开了gazebo界面。接下来，我们需要一个小球.
$rosrun gazebo_ros spawn_model -database robocup_spl_ball -sdf -model my_ball1
ok,那么现在我们已经把所有需要的模型加载进来了（虽然就1个球）
参考网站：http://gazebosim.org/tutorials/?tut=ros_comm
###动起来
如果仔细看了上面的教程和参考网站，那我们就会有一个整体的思路：利用gazebo提供的话题和服务就可以实现,在环境的搭建中，我们是直接采用spawn_sdf_model服务实现的模型的加载。接下来我们需要把加载的网球删除。接着实现流程如下：
1.使用spawn_sdf_model服务加载一个网球，并指定小球的位置
2.使用apply_body_wrench服务，给网球施加一个斜向上的力，使其做斜抛运动
3.当球弹出后，进入while循环，使用get_model_state服务得到小球的位置，当小球位置飞出预定轨迹后，跳出循环。
4.使用delete_model服务将这个小球删除，回到1.
程序如下：

#!/usr/bin/env python


import time
import rospy
from gazebo_msgs.srv import SpawnModel
from gazebo_msgs.srv import DeleteModel
from gazebo_msgs.srv import ApplyBodyWrench
from gazebo_msgs.srv import GetModelState
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Wrench
#初始位置
initial_pose = Pose()
initial_pose.position.x = -4
initial_pose.position.y = -1
initial_pose.position.z = 1
#所用力的大小
initial_wrench = Wrench()
initial_wrench.force.x = 0.16
initial_wrench.force.y = 0.0
initial_wrench.force.z = 0.25
#ctrl+h显示隐藏文档，将XXX替换为自己的目录，从自己的目录中找的这个文件
f = open('/home/XXXXX/.gazebo/models/robocup_spl_ball/model.sdf','r')
sdff = f.read()
#产生小球的服务
rospy.wait_for_service('gazebo/spawn_sdf_model')
spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
#施加一个作用力
rospy.wait_for_service('gazebo/apply_body_wrench')
apply_wrench = rospy.ServiceProxy('gazebo/apply_body_wrench', ApplyBodyWrench)
#删除小球
rospy.wait_for_service('gazebo/delete_model')
deletesrv = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
#得到小球位置
rospy.wait_for_service('gazebo/get_model_state')
get_m_state = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
while(1):
	spawn_model_prox("my_ball", sdff, "robotos_name_space", initial_pose, "world")
	time.sleep(1)

	apply_wrench("my_ball::ball", "world", None, initial_wrench, rospy.Time.from_sec(0), rospy.Duration.from_sec(0.2))
	s = get_m_state("my_ball","world")
	while(1):
		if(s.pose.position.x < 6):
			s = get_m_state("my_ball","world")
			print(s.pose.position.x)
		else:
			rospy.wait_for_service('gazebo/delete_model')
			deletesrv = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
			ass = deletesrv("my_ball")
			print(ass)			
			break

关于程序中各个服务的的调用方法，可以直接参考相关的消息和服务文档。

最后，可以加上一个球场，改变网球的形状，使仿真环境更加逼真。这需要我们自己去创建自己的模型。
突然发现经常出现小球删除不掉的情况，会出现
DeleteModel: Model pushed to delete queue, but delete service timed out waiting for
\ model to disappear from simulation"
调试了好久也没找出问题。有人说是gazebo的bug，真是迷了。。。