ROS 第四讲 让小车在RViz与Gazebo模拟器中跑起来

使用RViz 模拟turtlebot3

roslaunch turtlebot3_fake turtlebot3_fake.launch

就会弹出rviz的窗口:

查看下topic:

rostopic list

显示:

/clicked_point
/cmd_vel
/initialpose
/joint_states
/move_base_simple/goal
/odom
/rosout
/rosout_agg
/tf
/tf_static

其中有/cmd_vel 这个topic, 再查看下后台结构图:

rqt_graph

显示topic /cmd_vel由/turtlebot3_fake_node订阅, 而其暂没有发布者,因此小车在模拟器中是静止的. 让小车动起来可用键盘:

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

也可以用程序控制:

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
"""
@file: turtlebot1.py
@description: move turtlebot3 in rviz
"""

import sys
import rospy as ros
from geometry_msgs.msg import Twist


def move_turtle(lin_vel, ang_vel):
    ros.init_node('move_turtlebot1', anonymous=False)
    pub = ros.Publisher('/cmd_vel', Twist, queue_size=10)
    rate = ros.Rate(10)
    vel = Twist()
    while not ros.is_shutdown():
        vel.linear.x = lin_vel
        vel.linear.y = 0
        vel.linear.z = 0
        vel.angular.x = 0
        vel.angular.y = 0
        vel.angular.z = ang_vel
        ros.loginfo("Linear Vel = %f: Angular Vel = % f", lin_vel, ang_vel)
        pub.publish(vel)
        rate.sleep()


if __name__ == '__main__':
    try:
        move_turtle(float(sys.argv[1]), float(sys.argv[2]))
    except ros.ROSInterruptException:
        pass

运行类似命令:

rosrun practice1 move_turtlebot1.py 0.1 0. 8

小车即可在rviz中动起来,类似:

刚刚, roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 命令, 可以让我们通过键盘来控制小车的运动. 其实我们也可以不调用这个为我们准备好的roslauch命令, 我们完全可以使用程序实现一样的功能.

实时抓取用户键入信息

首先需要解决的是如何通过键盘的输入将其转化为对应topic所需的msg, 可以使用termios包来实现

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
"""
@file: keyboard_publisher.py 
@description: 
"""

import sys, select, tty, termios
import rospy as ros
from std_msgs.msg import String


def keyboard_capture():
    ros.init_node("keyboard_driver")
    key_pub = ros.Publisher('keyin', String, queue_size=1)
    rate = ros.Rate(1000)
    # 控制台是回车"Enter"后接收用户的输入的, 下面两句是将此模式替换成,
    # 用户输入立即接入用户输入信息,而不用等待回车输入
    prev_attrib = termios.tcgetattr(sys.stdin)
    tty.setcbreak(sys.stdin.fileno())
    print "starting capture key in,immediately, press Ctrl-z to exit..."
    while not ros.is_shutdown():
        if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]:
            key_pub.publish(sys.stdin.read(1))
        rate.sleep()
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, prev_attrib)


if __name__ == '__main__':
    keyboard_capture()

通过上面的程序即可捕获键盘实时键入的信息. 可先在一个terminal窗口中使用:

rostopic echo /keyin

然后再启动:

rosrun practice2 keyboard_publisher.py

那么上一个terminal中会显示类似如下信息,则说明我们成功了:

data: "w"
---
data: "a"
---
data: "d"
---
data: "s"
---

接下来,我们只需将用户键入的信息映射成速度信息并发布即可:

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
"""
@file: key2vel_map.py 
@description: 
"""

import rospy as ros
from std_msgs.msg import String
from geometry_msgs.msg import Twist


class Key2Vel_Map(object):
   def __init__(self, mode='uniform'):
       """

       :param mode: 速度模式还是匀速(default)模式
       """
       ros.init_node('key2vel')
       self.vel_pub = ros.Publisher('/cmd_vel', Twist, queue_size=1)
       if mode == 'uniform':
           self.map_func = self.uniform_map
       elif mode == 'accelerate':
           self.map_func = self.accelerate_map
       else:
           ValueError('bad mode: `%s` value, please check!' % mode)
       self.mode = mode
       self.vel = [0, 0]
       self.twist = Twist()
       self.kmap = {'a': [0, 0.1], 's': [0, 0], 'd': [0, -0.1], 'w': [0.1, 0], 'x': [-0.1, 0]}

   def key2map(self, key):
       if key in self.kmap:
           return self.kmap[key]
       else:
           return None

   def uniform_map(self, key, multiple=10):
       """

       :param key:
       :param multiple: 0.1匀速度的倍数
       :return:
       """
       res = self.key2map(key)
       if res:
           return [x * multiple for x in res]

   def accelerate_map(self, key):
       res = self.key2map(key)
       if res:
           if key == 's':
               self.vel = [0, 0]
           else:
               self.vel = [x + y for x, y in zip(res, self.vel)]
           return self.vel

   def key_callback(self, msg):
       if len(msg.data) == 0 or not self.kmap.get(msg.data[0]):
           return  # unknown key

       vels = self.map_func(msg.data[0])
       self.twist.linear.x = vels[0]
       self.twist.angular.z = vels[1]

   def capture(self):
       ros.Subscriber('keyin', String, self.key_callback)
       while not ros.is_shutdown():
           self.vel_pub.publish(self.twist)
       ros.spin()


if __name__ == '__main__':
   k2v = Key2Vel_Map(mode='accelerate')
   k2v.capture()

此时我们就可以分别在不同的terminal中开启两个程序了,也可以写一个.launch文件,将两个程序同时启动. 首先,我们在 practice2包中创建launch目录, 然后在其中创建名为key2map.launch文件, 并在其中加入:

<launch>
    <node name ="key2vel" pkg="practice2" type="key2vel_map.py" output="screen"/>
    <node name ="keyboard_driver" pkg="practice2" type="keyboard_publisher.py" output="screen"/>
</launch>

启动:

roslaunch practice2 key2map.launch

然后我们就可以在当前的terminal中键入WASDX来操作小车了!

让小车在gazebo中跑起来

首先启动带有turtlebot的模拟环境:

roslaunch turtlebot3_gazebo turtlebot3_world.launch

会弹出如下类似的窗口:

可以使用

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

然后使用键盘来操作. terminal上面会出现类似信息,可知使用WADXS五键来控制:

... logging to /home/[username]/.ros/log/8498294a-f078-11eb-9bc2-2cfda1be536f/roslaunch-prince_pc-26012.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://[user_name]_pc:38397/

SUMMARY
========

PARAMETERS
 * /model: burger
 * /rosdistro: melodic
 * /rosversion: 1.14.11

NODES
  /
    turtlebot3_teleop_keyboard (turtlebot3_teleop/turtlebot3_teleop_key)

ROS_MASTER_URI=http://localhost:11311

process[turtlebot3_teleop_keyboard-1]: started with pid [26061]

Control Your TurtleBot3!
---------------------------
Moving around:
        w
   a    s    d
        x

w/x : increase/decrease linear velocity (Burger : ~ 0.22, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)

space key, s : force stop

CTRL-C to quit

也可以使用程序来控制小车, 同样的,先看下当前的topic 有哪些:

# 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

现在我们已经熟悉了/cmd_vel 这个topic的msg可以控制小车,比如写一个控制小车,走走停停的程序:

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
"""
@file: run_or_wait.py 
@description: 
"""

import rospy as ros
from geometry_msgs.msg import Twist

def red_green_light():
    cmd_vel_pub = ros.Publisher('/cmd_vel', Twist, queue_size=1)
    ros.init_node('red_light_green_light', anonymous=False)
    red_light_twist = Twist()
    green_light_twist = Twist()
    green_light_twist.linear.x = 0.5
    driving_forward = True
    light_change_time = ros.Time.now()
    rate = ros.Rate(10)

    while not ros.is_shutdown():
        if driving_forward:
            cmd_vel_pub.publish(green_light_twist)
            print('go forward...', light_change_time)
        else:
            cmd_vel_pub.publish(red_light_twist)
            print('stop...', light_change_time)
        if light_change_time < ros.Time.now():
            driving_forward = not driving_forward
            light_change_time = ros.Time.now() + ros.Duration(3)
        rate.sleep()


if __name__ == '__main__':
    red_green_light()

运行后,小车就会走一会停一会. 我们可以控制gazebo中的小车了!

获取小车位置信息

上面的程序会使用小车很容易就卡在某个地方. 因为他漫无目的的走,根本不在意前方的障碍物. 我们慢慢来, 我们先让小车走固定距离, 这个咱们之前控制小乌龟时尝试过,这次咱们按类似的方法控制小车. 小车的位置信息topic是/odom, 其type与msg:

# rostopic type /odom
nav_msgs/Odometry


# rosmsg show nav_msgs/Odometry
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance

从上面略显复杂的数据结构中, 稍加寻找,发现其位置信息在:Odometry.pose.pose.position中, 其它的与小乌龟移动固定距离类似:

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
"""
@file: run_or_wait.py
@description: 
"""

import sys
import rospy as ros
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

ORIGIN_POSE = 0
CURRENT_POSE = 0


def pose_callback(msg):
    global CURRENT_POSE, ORIGIN_POSE
    CURRENT_POSE = msg.pose.pose.position.x
    ros.loginfo('Robot X=%f
', CURRENT_POSE - ORIGIN_POSE)


def move_turtlebot(lin_vel, ang_vel, distance):
    global CURRENT_POSE, ORIGIN_POSE
    ros.init_node('move_turtlebot', anonymous=False)
    pub = ros.Publisher('/cmd_vel', Twist, queue_size=1)
    ros.Subscriber('/odom', Odometry, pose_callback)
    rate = ros.Rate(1)
    rate.sleep()
    vel = Twist()
    ORIGIN_POSE = CURRENT_POSE
    while not ros.is_shutdown():
        vel.linear.x = lin_vel
        vel.linear.y = 0
        vel.linear.z = 0
        vel.angular.x = 0
        vel.angular.y = 0
        vel.angular.z = ang_vel
        # rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel, ang_vel)

        if ((CURRENT_POSE - ORIGIN_POSE) >= distance):
            ros.loginfo("Robot Reached destination")
            ros.logwarn("Stopping robot")
            vel.linear.x = 0.
            vel.angular.z = 0.
            pub.publish(vel)
            rate.sleep()
            break
        pub.publish(vel)
        rate.sleep()


if __name__ == '__main__':
    try:
        move_turtlebot(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]))
    except ros.ROSInterruptException:
        pass

这样小车跑一会就停下来了.

使用sensor laser

我们限定了小车运动的方式以及运动范围避免他撞到周围的物体. 其实通过传感器,小车可以及时避障,然后, 修改路线.

之前的包practice1 没有传感器相关的依赖包, 这里我们重新创建一个practice2的包,加入sensor 相关的依赖包:

cd ~/catkin_ws/src
catkin_create_pkg practice2 rospy roscpp standard_msgs geometry_msgs sensor_msgs
cd ~/catkin_ws
catkin_make --pkg practice2
cd ~/catkin_ws/src/practice2
mkdir -p script

使用rostopic list, 展示的列表中, /scan 是关于激光扫描的topic:

# rostopic type /scan
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

如果此时打印相关msg的信息,大约会不断打印类似下面的数据(Ctrl-c 可以停下):

#  rostopic echo /scan
header: 
  seq: 91
  stamp: 
    secs: 62
    nsecs: 152000000
  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: [inf, inf, inf, inf, inf, inf, inf, 3.073126792907715, 3.0457253456115723, 3.0183537006378174, 2.03729248046875, 1.973899245262146, 1.955241322517395, 1.9441207647323608, 1.9433691501617432, ...,inf]
intensities: [0.0, 0.0, ..., 0.0]
---

其中的range 表示在传感器可扫描角度中,各个方向上面的物体与小车距离. 要获得小车正前方障碍物与小车的距离可以这样计算:

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

其中msg表示msg时实例名.

取得以上信息后,就可以让小车自动避障了, 比如当前的障碍距离小车20cm时, 就让小车暂停, 并旋转一定角度, 后继承前进.

#!/usr/bin/env python  
# -*- coding: utf-8 -*-
"""
@file: run_or_wait.py
@description: 
"""

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


class WanderBot(object):
    def __init__(self):
        ros.init_node('wanderBot')
        self.ahead_distance = 1e10
        self.ahead_threshold = 0.3
        self.scan_sub = ros.Subscriber('/scan', LaserScan, callback=self.scan_callback)
        self.cmd_vel_pub = ros.Publisher('/cmd_vel', Twist, queue_size=1)
        self.rate = ros.Rate(1)

    def scan_callback(self, msg):
        self.ahead_distance = msg.ranges[len(msg.ranges) / 2]
        ros.loginfo('ahead_distance: %f
' % self.ahead_distance)

    def wander(self):
        vel = Twist()
        while not ros.is_shutdown():
            if self.ahead_distance < 0.4:
                ros.loginfo('stop and turn around...')
                vel.linear.x = 0
                vel.angular.z = 0.2
            else:
                ros.loginfo('go ahead...')
                # 注意这个方向才是与laser sensor 一致的方向, 
                # 实验一下即可得知
                vel.linear.x = -0.2 
                vel.angular.z = 0
            self.cmd_vel_pub.publish(vel)
            self.rate.sleep()


if __name__ == '__main__':
    wb = WanderBot()
    wb.wander()

先打开rivz, 再执行上面的程序,会对小车的运动更有帮助:

roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch
# another new terminal
rosrun practice2 wanderbot.py

...
[INFO] [1627714129.363296, 348.200000]: ahead_distance: 1.163768

[INFO] [1627714129.421784, 348.264000]: go ahead...
[INFO] [1627714129.559045, 348.401000]: ahead_distance: 1.122308

[INFO] [1627714129.760064, 348.601000]: ahead_distance: 1.084583

[INFO] [1627714129.960899, 348.802000]: ahead_distance: 1.060412

[INFO] [1627714130.162832, 349.002000]: ahead_distance: 1.005350

[INFO] [1627714130.365322, 349.202000]: ahead_distance: 0.954956
...