ROS从入门到放弃——用TurtleBot3做Navigation模拟

本教程参考TurtleBot3

---

• 这里我们的机器人有3种选项burger, waffle, waffle_pi。
• Gazebo的地形有两种选项
  • roslaunch turtlebot3_gazebo turtlebot3_world.launch
  • roslaunch turtlebot3_gazebo turtlebot3_house.launch

6.2 SLAM Simulation

• Terminal 1：打开Gazebo仿真环境

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_gazebo turtlebot3_world.launch

• Terminal 2：运行SLAM Node

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=gmapping

• Terminal 3：运行Teleoperation Node

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

然后对着这个Terminal就可以wasdx来控制前后左右移动了，然后rviz里面的地图也会完整起来

• Terminal 3：关闭上面那个 运行自主导航

roslaunch turtlebot3_gazebo turtlebot3_simulation.launch

当建好图之后，我们就可以把地图截图保存在桌面上,-f表示我们的希望输入保存的路径，保存完有两个文件map.pgm和map.yaml，yaml里面调用了map.pgm。

rosrun map_server map_saver -f ~/Desktop/map

白色是可以去的地方，黑色是墙壁，灰色是我们不了解的地方。Occupancy Grid Map (OGM)

• 地图利用了机器人的里程表(Odometry)，tf和扫描信息。
• 本节里面还介绍了如何使用多个机器人同时建图。

SLAM 参数介绍

我们可以定时或者定距离来采样。

我们可以调节我们的SLAM的参数（在gmapping_params.yaml的文件中）

• maxUrange：此参数设置激光雷达传感器的最大可用范围。
• map_update_interval：更新地图的时间，数字越小速度越快
• minimumScore：此参数设置最小分数值，该分数确定传感器的扫描数据匹配测试的成功或失败。 这样可以减少大面积机器人的预期位置中的误差。 如果参数设置正确，您将看到类似于以下所示的信息:

  Average Scan Matching Score=278.965
  neff= 100
  Registering Scans:Done
  update frame 6
  update ld=2.95935e-05 ad=0.000302522
  Laser Pose= -0.0320253 -5.36882e-06 -3.14142
  

  设置太高，可能会显示Warning

  Scan Matching Failed, using odometry. Likelihood=0
  lp:-0.0306155 5.75314e-06 -3.14151
  op:-0.0306156 5.90277e-06 -3.14151
  

• linearUpdate：当机器人平移距离大于此值时，它将运行扫描过程。
• angularUpdate：当机器人旋转的速度超过此值时，它将运行扫描过程。 建议将此值设置为小于linearUpdate。

6.3 Navigation Simulation

接着上面的步骤，我们可以用来将我们之前保存的地图yaml数据读取出来。($HOME是根目录)

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
或者
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=~/Desktop/map.yaml

1. 接着我们点击2D Pose Estimate，然后点击机器人当前的位置，并将绿色箭头指向机器人的前方。
2. 重复操作1直到两个地图重合。
3. 然后它会用粒子（绿色的箭头）滤波把绿色的（SLAM）
4. 接着可以用2D Nav Goal指一个位置就可以把机器人移过去。

观察LaserScan的数据

我们可以用rostopic list查看我们当前的所有话题，其中有两个比较重要的话题，一个是/scan还有一个是cmd_vel。

/cmd_vel

rostopic info cmd_vel: geometry_msgs/Twist
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

rostopic echo /cmd_vel -n1: 显示出一个数据观察一下

linear: 
  x: 0.0
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 0.0
---

/scan

rostopic info /scan:
Type: sensor_msgs/LaserScan
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

rostopic echo /scan -n1:

header: 
  seq: 25854
  stamp: 
    secs: 5823
    nsecs: 115000000
  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: [...]
---

在这个例子中我们可以看见我们雷达的范围是0到2$\pi$,ranges给出了每一个角度上的距离，一共有$2\pi /0.0175019223243=360$个元素。

自己写一个漫游地图的node

1. 在ws的src中创建一个新的pkg
2. 新的pkg中创建一个文件夹scripts: mynavigation.py
3. chmod +x mynavigation.py

#! /usr/bin/env python

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

def callback(msg):
    vel_msg = Twist() #创建一个新的消息
    vel_msg.linear.x = 0
    vel_msg.linear.y = 0
    vel_msg.linear.z = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    vel_msg.angular.z = 0

    if(msg.ranges[0]<0.5): #如果机器人正前方的点很近，我们就转弯
        #print('Turn')
        vel_msg.angular.z=1.82
        pub.publish(vel_msg)
    else:
        #print('Go!')0.26
        vel_msg.linear.x=0.26
        pub.publish(vel_msg)
        

if __name__ == '__main__':
    rospy.init_node('navigation')
    rate = rospy.Rate(1)
    pub = rospy.Publisher('cmd_vel', Twist,queue_size=10) #控制机器人
    sub = rospy.Subscriber('/scan', LaserScan, callback)
    rospy.spin()

或者，设置某一个范围内如果有障碍就掉头跑

#! /usr/bin/env python

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

def toclose(data,tmin,tmax,threshold=0.5):
    # t [-180,180]
    for i in data[0:tmax]:
        if i<threshold:
            return True
    for i in data[360+tmin:]:
        if i<threshold:
            return True
    return False

def callback(msg):
    vel_msg = Twist()
    vel_msg.linear.x = 0
    vel_msg.linear.y = 0
    vel_msg.linear.z = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    vel_msg.angular.z = 0
    
    #if(msg.ranges[0]<0.5):
    if(toclose(msg.ranges,-30,30)):
        print('Turn')
        vel_msg.angular.z=1.82
        pub.publish(vel_msg)
    else:
        print('Go!')
        vel_msg.linear.x=0.26
        pub.publish(vel_msg)
        

if __name__ == '__main__':
    rospy.init_node('navigation')
    #rate = rospy.Rate(1)
    pub = rospy.Publisher('cmd_vel', Twist,queue_size=10)
    sub = rospy.Subscriber('/scan', LaserScan, callback)
    rospy.spin()