ROS笔记八（基于Python、Kinetic）：gazebo初探——实现turtlebot移动并避开障碍

前言：代码实现在gazebo7仿真环境中turtlebot机器人的自由移动并避开障碍物。

1.创建新的工作空间：

创建工作空间文件树：

$ mkdir -p ~/wanderbot_ws/src
$ cd ~/wanderbot_ws/src
$ catkin_init_workspace

创建一个“wanderbot”程序包：

$ cd ~/wanderbot_ws/src
$ catkin_create-pkg wanderbot rospy geometry_msgs sensor_msgs

依赖包:rospy(python编译)、geometry_msgs（用于传递机器人速度等数据）、sensor_msgs(用于传递机器人传感器数据)。

2.代码实现：

代码1:实现turtlebot移动和停止：

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
#声明‘cmd_vel’的话题，消息类型为Twist，“queue_size=1”:rospy只缓冲一个消息
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
#节点初始化
rospy.init_node('red_light_green_light')
#创建一个Twist消息，初始值为零
red_light_twist = Twist() #停止消息流
green_light_twist = Twist() #移动消息流
#指定消息中linear沿着x方向速度
green_light_twist.linear.x = 0.5

driving_forward = False
light_change_time = rospy.Time.now()
#设置速率
rate = rospy.Rate(10)

while not rospy.is_shutdown():
  if driving_forward:
    #持续发布速度命令消息流
    cmd_vel_pub.publish(green_light_twist) 
  else:
    cmd_vel_pub.publish(red_light_twist)
  #检测系统时间，三秒周期性改变
  if rospy.Time.now() > light_change_time: 
    driving_forward = not driving_forward
    light_change_time = rospy.Time.now() + rospy.Duration(3)
  rate.sleep() 

 代码2:检测机器人前方障碍物的距离：

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
#回调函数：将数据输出到终端
def scan_callback(msg):
  range_ahead = msg.ranges[len(msg.ranges)/2]
  print "range ahead: %0.1f" % range_ahead
#节点初始化
rospy.init_node('range_ahead')
#订阅‘scan’话题,消息类型LaserScan
scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback)
rospy.spin()

 代码3：感知环境并移动

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

def scan_callback(msg):
   #全局变量存储激光扫描器检测到的最小距离
  global g_range_ahead
  g_range_ahead = min(msg.ranges)

g_range_ahead = 1 
#订阅‘scan’话题，消息类型LaserScan
scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback)
#发布‘cmd_vel’话题，消息类型Twist，只缓冲一个消息
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
#节点初始化
rospy.init_node('wander')
#获取系统当前时间
state_change_time = rospy.Time.now()
#前进/转向状态参数
driving_forward = True
#设置速率
rate = rospy.Rate(10)

while not rospy.is_shutdown():
  if driving_forward:
    # 如果机器人距离障碍小于0.8M或则运行时间大于30秒，停止前进并转向
    if (g_range_ahead < 0.8 or rospy.Time.now() > state_change_time):
      #停止
      driving_forward = False
      #转向
      state_change_time = rospy.Time.now() + rospy.Duration(5)
  
  else:
    if rospy.Time.now() > state_change_time:
      driving_forward = True # 完成转向，继续前进
      state_change_time = rospy.Time.now() + rospy.Duration(30)
  
  twist = Twist()
  if driving_forward:
    #指定前进时的线速度
    twist.linear.x = 1
  else:
    #指定转向时的角速度
    twist.angular.z = 1
  cmd_vel_pub.publish(twist)

  rate.sleep()

3.安装gazebo：

$ sudo apt-get install ros-kinetic-turtlebot-gazebo

4.打开gazebo：

打开gazebo：

$ roslaunch turtlebot_gazebo turtlebot_world.launch

因为系统没有模型文件，所以加载非常缓慢，点击下载模型库即可。下载完成后解压至~/.gazebo/models/目录下即可。

5.在gazebo中运行turtlebot：

打开新的终端：

$ cd ~/wanderbot_ws/src/wanderbot/src

增加可执行权限：

$ chmod +x wander.py

运行程序：

./wander.py cmd_vel:=cmd_vel_mux/input/teleop

这里使用ROS的重映射，因为turtlebot订阅‘Twist’消息的话题是cmd_vel_mux/input/teleop而不是cmd_vel。 

6.问题记录：

笔者在运行时候出现了下面的问题：

始终没找到办法解决，现在回想可能是在安装过程中：在终端最后一行出现File to patch：不明所以，Ctrl C也不能退出，最后强行关掉了终端。最后卸载了gazebo重新安装才得以解决。

安装完成后gazebo7所依赖的包如下：

 关于gazebo卸载：

$ sudo apt-get remove gazebo*
$ sudo apt-get remove libgazebo*
$ sudo apt-get remove ros-kinetic-gazebo*

直到终端执行dpkg -l | grep gazebo 没有输出即完全卸载。