ROS开发笔记（4）——基于 python 开发Turtlebot3 Gazebo仿真环境下 wander-bot 随机移动机器人

环境：Ubuntu18.04+ROS melodic 版

本文参考《ROS机器人编程实践》中的内容，一步步构建了一个能在Turtlebot3 Gazebo仿真环境中到处移动的机器人。主要步骤如下：

1、Turtlebot3 Gazebo仿真环境搭建

2、通过 cmd_vel 话题控制Turtlebot3在Gazebo仿真环境下运动

3、获取Turtlebot3的激光扫描数据话题scan (sensor_msgs/LaserScan)感知障碍物的存在

4、综合2和3 编写Python代码实现Turtlebot3在仿真环境中自动避障随机移动

 

1、Turtlebot3 Gazebo仿真环境搭建

安装ros的桌面完整版默认也是没有Turtlebot3相关的包的，使用前必须先安装，一开始尝试 sudo apt install 方式安装相关包，但是报错，后来通过源码编译安装的方式装好了，过程如下：

cd ~/catkin_ws/src/
git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations
git clone https://github.com/ROBOTIS-GIT/turtlebot3_turtlebot3_machine_learning

cd ~/catkin_ws && catkin_make

编译成功后，先 source  ~/catkin_ws/devel/setup.bash (也可以把这条写到 ~/.bashrc 里)，然后就可以通过下面的指令启动turtlebot3_gazebo仿真环境了：

roslaunch  turtlebot3_gazebo  turtlebot3_world.launch

这是顶视图，有时我们喜欢改变视角观察，Gazebo界面操作方法：左键单击拖动——平移，中键单击拖动——改变观察角度，滚轮缩放，右键单击拖动——缩放。例如我们可以调整成下面的样子：

2、通过 cmd_vel 话题控制Turtlebot3在Gazebo仿真环境下运动

这里直接上代码，说明见注释部分：

#!/usr/bin/env python
#-*- coding:utf-8   -*-
'''red_light_green_light ROS Node'''
# license removed for brevity
import rospy
from geometry_msgs.msg import Twist


#创建名为cmd_vel，类型为Twist的cmd_vel_pub话题
#queue_size  缓存消息队列大小
cmd_vel_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)

#初始化节点
rospy.init_node('red_light_green_light')

#定义两条消息，红灯  绿灯消息
red_light_twist=Twist()
green_light_twist=Twist()
deriving_forward=False


green_light_twist.linear.x=0.5
light_change_time=rospy.Time.now()+rospy.Duration(3)
rate=rospy.Rate(10)

while not rospy.is_shutdown():
    if deriving_forward :
        cmd_vel_pub.publish(green_light_twist)
    else:
        cmd_vel_pub.publish(red_light_twist)

    # print "light_change_time : %f" % (light_change_time.to_sec())
    # print "rospy.Time.now    : %f" % (rospy.Time.now().to_sec())

    if light_change_time < rospy.Time.now():
        deriving_forward= not deriving_forward
        light_change_time= rospy.Time.now()+rospy.Duration(3)
    rate.sleep()

通过查看cmd_vel话题信息得知， 其由/red_light_green_light节点发布，/gazebo节点订阅，其消息格式是geometry_msgs/Twist ，包含三维线速度与三维角速度，

wsc@wsc-pc:~/wanderbot_ws$ rostopic info cmd_vel
Type: geometry_msgs/Twist

Publishers:
 * /red_light_green_light (http://wsc-pc:34795/)

Subscribers:
 * /gazebo (http://wsc-pc:39455/)


wsc@wsc-pc:~/wanderbot_ws$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

3、获取Turtlebot3的激光扫描数据话题scan (sensor_msgs/LaserScan)感知障碍物的存在

首先查看Tb3发布的话题相关信息：

wsc@wsc-pc:~$ rostopic list
/clock
/cmd_vel
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/imu
/joint_states
/odom
/rosout
/rosout_agg
/scan
/tf

wsc@wsc-pc:~$ rostopic info scan
Type: sensor_msgs/LaserScan

Publishers: 
 * /gazebo (http://wsc-pc:34129/)

Subscribers: None

wsc@wsc-pc:~$ rosmsg show  sensor_msgs/LaserScan
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

wsc@wsc-pc:~$ rostopic  echo scan  -n 1
header: 
  seq: 0
  stamp: 
    secs: 588
    nsecs: 569000000
  frame_id: "base_scan"
angle_min: 0.0
angle_max: 6.28318977356
angle_increment: 0.0175019223243
time_increment: 0.0
scan_time: 0.0
range_min: 0.119999997318
range_max: 3.5
ranges: 太长省略
intensities: 太长省略 

其中scan为激光扫描传感器话题，消息类型为 sensor_msgs/LaserScan，float32[] ranges 数组包含了传感器各个方向到障碍物的距离。假如消息变量为msg,正前方障碍物的距离可以这么计算：

range_ahead=msg.ranges[len(msg.ranges)/2]

代码及注释如下：

#!/usr/bin/env python
#-*- coding:utf-8   -*-

import rospy
from sensor_msgs.msg import LaserScan

#处理scan话题数据的回调函数
def scan_callback(msg):
    range_ahead=msg.ranges[len(msg.ranges)/2]
    print "range_ahead: %0.1f"%range_ahead

#订阅Gazebo仿真环境Turtlebot3激光扫描仪的scan话题
scan_sub=rospy.Subscriber('scan',LaserScan,scan_callback)

#初始化节点
rospy.init_node('range_ahead')

rospy.spin()

4、编写Python实现Turtlebot3在仿真环境中自动避障随机移动

综合前面的驱动Turtlebot3运动以及获取正前方障碍物距离信息，编写Python代码，实现避障随机走动，代码及注释如下：

#!/usr/bin/env python
#-*- coding:utf-8   -*-

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

#处理scan话题数据的回调函数
def scan_callback(msg):
    global g_range_ahead
    # g_range_ahead=min(msg.ranges)
    # g_range_ahead=msg.ranges[len(msg.ranges)/2]
    g_range_ahead=msg.ranges[len(msg.ranges)/2*0]
    print "range_ahead: %0.1f"%g_range_ahead


g_range_ahead=1

#创建名为cmd_vel，类型为Twist的cmd_vel_pub话题
#queue_size  缓存消息队列大小
cmd_vel_pub=rospy.Publisher('cmd_vel',Twist,queue_size=1)

#订阅Gazebo仿真环境Turtlebot3激光扫描仪的scan话题
scan_sub=rospy.Subscriber('scan',LaserScan,scan_callback)


#初始化节点
rospy.init_node('wander')

deriving_forward=True
state_change_time=rospy.Time.now()+rospy.Duration(15)
rate=rospy.Rate(10)

while not rospy.is_shutdown():
    if deriving_forward :
        # print "3"
        if (g_range_ahead<0.8 or rospy.Time.now()>state_change_time):
            # print "4"
            deriving_forward=False
            state_change_time=rospy.Time.now()+rospy.Duration(5)
    else:
        # print "5"
        if (rospy.Time.now()>state_change_time or g_range_ahead>0.8 ):
        # if (rospy.Time.now()>state_change_time):
            # print "6"
            deriving_forward=True
            state_change_time=rospy.Time.now()+rospy.Duration(15)

    twist=Twist()
    
    if deriving_forward:
        
        if g_range_ahead>0.8:
            twist.linear.z=0.0
            twist.linear.x=0.5
            # print "1.1"
        else:
            twist.linear.x=-0.2
            twist.angular.z=0.5          
            print "1.2"
    else:
        if g_range_ahead>0.8:
            twist.linear.z=0.5
            twist.linear.x=0.0
            # print "2.1"
        else:
            twist.linear.x=-0.2
            twist.angular.z=0.5 
            print "2.2"

    cmd_vel_pub.publish(twist)
    rate.sleep()

效果如下：