前言

  上一篇文章 “多机器人协同控制①——仿真平台搭建” 里已经介绍了多台Turtlebot3在Gazebo中的显示,这篇文章将写一个简单的程序实现单小车的轨迹运动,把整个流程打通一下,为后续的算法验证做一个准备。  

1.单机运动

  参考上一篇文章中的2.多机仿真,我们已经可以实现多台Turtlebot在Gazebo中运行,此时通过命令查看Topic:  
rostopic list

可以看到     此时的四辆小车分别以 tb3_x 作为前缀,其中   我们先尝试着对其中一辆小车进行运动控制:   首先我们知道,和ROS里的小乌龟例程一样,Turtlebot3通过 cmd_vel 话题发送指令就可以对小车进行运动控制。  
  • 我们可以简单的通过命令行模拟数据发送:

rostopic pub /tb3_0/cmd_vel geometry_msgs/Twist "linear:
  x: 0.5
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.2"

  • 同样也可以通过 rqt_publisher 工具发送数据

rosrun rqt_publisher rqt_publisher

  • Python程序在后面会详细给出
因为Turtlebot3是两轮差动驱动的轮式移动机器人,在轮子不打滑的情况下,没有y轴和z轴的位移,因此只需要输入 linear xangular z 两个数据分别控制小车的线速度和角速度即可。  

2.到达指定点

  在上一篇文章中我们可以使用demo进行键盘控制小车运动、SLAM建图后进行自主运动等。但是在我们做实验验证的时候,有些情况需要让小车沿着给定轨迹运动,下面就针对这种情况分析一下。   首先我们可以通过 odom 这个话题接收小车的里程计信息,进而获取小车自身的坐标位置,因为仿真环境中环境影响较小,且我们目前的研究重点在小车的编队上,因此我们暂时可以将小车的 odom 信息视作小车的真实位姿。   接下来我们可以给定坐标点,控制小车到达目标点,由于程序比较简单这里直接放出Python代码:  
import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion

# 定义全局变量
x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0

X_t = 0
Y_t = 0
X_sim = 5        # 目标点x坐标
Y_sim = 5        # 目标点y坐标


def Trans_robot_pose(msg):  # 回调函数
    # 位置坐标声明
    global x
    global y
    global w_o    # 当前小车位姿的四元数信息
    global x_o
    global y_o
    global z_o
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    w_o = msg.pose.pose.orientation.w
    x_o = msg.pose.pose.orientation.x
    y_o = msg.pose.pose.orientation.y
    z_o = msg.pose.pose.orientation.z
    return w_o, y_o, z_o, x_o, x, y


if __name__ == '__main__':
    rospy.init_node('item1')

    turtle_vel = rospy.Publisher('/tb3_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    rate = rospy.Rate(10.0)

    while not rospy.is_shutdown():
        msg = geometry_msgs.msg.Twist()
        (roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])  # 将四元数转化为roll, pitch, yaw
        if yaw < 0:
            yaw = yaw + 2 * math.pi

        X_t = X_sim
        Y_t = Y_sim

        D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))

        # 判断坐标象限
        if (Y_t - y) == 0 and (X_t - x) > 0:
            yaw_t = 0
        if (Y_t - y) > 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x))
        if (Y_t - y) > 0 and (X_t - x) == 0:
            yaw_t = 0.5 * math.pi
        if (Y_t - y) > 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) == 0 and (X_t - x) < 0:
            yaw_t = math.pi
        if (Y_t - y) < 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) < 0 and (X_t - x) == 0:
            yaw_t = 1.5 * math.pi
        if (Y_t - y) < 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi

        Theta_err = yaw_t - yaw
        if Theta_err < -math.pi:
            Theta_err = Theta_err + 2 * math.pi
        if Theta_err > math.pi:
            Theta_err = Theta_err - 2 * math.pi

        # 目前先只使用最简单的比例控制
        liner_speed = 0.1 * D_err
        angular_speed = 0.7 * Theta_err

        msg.linear.x = liner_speed
        msg.angular.z = angular_speed

        liner_speed_old = liner_speed
        angular_speed_old = angular_speed
        turtle_vel.publish(msg)    # 发布运动指令
        rospy.Subscriber('/tb3_0/odom', nav_msgs.msg.Odometry,  Trans_robot_pose) # 获取位姿信息

        rate.sleep()
    rospy.spin()

在打开launch文件后直接rosrun 该py文件即可看到小车朝目标点移动。   此时我们可以通过rviz工具来查看小车的运动轨迹:  

3.给定轨迹

  接着我们可以想到,如果连续给定不同的点,岂不就可以让小车沿着轨迹运动了,因此我们只需要将上面的程序稍加改动:将 X_simY_sim 变为数组即可。   之后再进行简单修改就能让Turtlebot沿着给定轨迹运动(因为这里没有考虑时间因素,因此不能算作轨迹跟踪)   下面直接放出Python代码:  
import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion

# 定义全局变量
x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0

X_t = 0
Y_t = 0
X_t_Pre = 0
Y_t_Pre = 0
X_sim = [5,  1, 2.5,  4, 0,   0,   5,  5,  0, 0]        # 目标点x坐标
Y_sim = [0, -4, 2.5, -4, 0, 2.5, 2.5, -4, -4, 0]        # 目标点y坐标
r = 0


def Trans_robot_pose(msg):  # 回调函数
    # 位置坐标声明
    global x
    global y
    global w_o    # 当前小车位姿的四元数信息
    global x_o
    global y_o
    global z_o
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    w_o = msg.pose.pose.orientation.w
    x_o = msg.pose.pose.orientation.x
    y_o = msg.pose.pose.orientation.y
    z_o = msg.pose.pose.orientation.z
    return w_o, y_o, z_o, x_o, x, y


if __name__ == '__main__':
    rospy.init_node('item1')

    turtle_vel = rospy.Publisher('/tb3_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    rate = rospy.Rate(10.0)

    while not rospy.is_shutdown():
        msg = geometry_msgs.msg.Twist()
        (roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])  # 将四元数转化为roll, pitch, yaw
        if yaw < 0:
            yaw = yaw + 2 * math.pi

        X_t = X_sim[r]
        Y_t = Y_sim[r]

        D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))

        # 判断坐标象限
        if (Y_t - y) == 0 and (X_t - x) > 0:
            yaw_t = 0
        if (Y_t - y) > 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x))
        if (Y_t - y) > 0 and (X_t - x) == 0:
            yaw_t = 0.5 * math.pi
        if (Y_t - y) > 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) == 0 and (X_t - x) < 0:
            yaw_t = math.pi
        if (Y_t - y) < 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) < 0 and (X_t - x) == 0:
            yaw_t = 1.5 * math.pi
        if (Y_t - y) < 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi

        Theta_err = yaw_t - yaw

        if Theta_err < -math.pi:
            Theta_err = Theta_err + 2 * math.pi
        if Theta_err > math.pi:
            Theta_err = Theta_err - 2 * math.pi

        if D_err < 0.3:        # 当距当前点小于一定值的时候去下一个点
            X_t_Pre = X_t
            Y_t_Pre = Y_t
            r = r + 1
            print r
            if r == 10:
                r = 0

            # 仍然先只使用最简单的比例控制
        liner_speed = 0.1 * D_err
        angular_speed = 0.7 * Theta_err

        msg.linear.x = liner_speed
        msg.angular.z = angular_speed

        liner_speed_old = liner_speed
        angular_speed_old = angular_speed
        turtle_vel.publish(msg)    # 发布运动指令
        rospy.Subscriber('/tb3_0/odom', nav_msgs.msg.Odometry,  Trans_robot_pose) # 获取位姿信息

        rate.sleep()
    rospy.spin()

在打开launch文件后直接rosrun该py文件即可看到小车沿着轨迹运动。   同样可以使用rviz工具来查看小车的运动轨迹  

4.mat文件生成轨迹

  有些时候我们会先使用Matlab软件进行仿真,众所周知,Matlab的数据存储标准格式为mat格式,如果我们要使用mat里的数据生成轨迹,这个时候我们就需要使用Python来读取mat格式的文件了。     采用 Python 读取 matlab 中 .mat文件的方法有很多,中外文的论坛上都不少,这里我们采用h5py来读物.mat文件。  
使用h5py库可以读写超过内存的大数据 。在简单数据的读操作中,我们通常一次性把数据全部读入到内存中。读写超过内存的大数据时,有别于简单数据的读写操作,受限于内存大小,通常需要指定位置、指定区域读写操作,避免无关数据的读写,h5py库刚好可以实现这一功能。

  直接pip安装即可  
pip install h5py

  之后的我们可以将上文的代码进行简单的修改,将 X_tY_t 改为去读取mat文件的数组,再简单修改即可,Python代码如下:  
import rospy
import math
import h5py
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion

fix_data_file_size_path = "/home/wca/data/car0.mat"  # mat文件的绝对路径
fixdata_size_file = h5py.File(fix_data_file_size_path, "r")
sizeData = fixdata_size_file["an"]

# 定义全局变量
x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
i = 0
X_t = 0
Y_t = 0
X_t_Pre = 0
Y_t_Pre = 0
r = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0


def Trans_robot_pose(msg):
    # 位置坐标声明
    global x
    global y
    global w_o
    global x_o
    global y_o
    global z_o
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    w_o = msg.pose.pose.orientation.w
    x_o = msg.pose.pose.orientation.x
    y_o = msg.pose.pose.orientation.y
    z_o = msg.pose.pose.orientation.z
    return w_o, y_o, z_o, x_o, x, y


if __name__ == '__main__':
    rospy.init_node('item0')
    turtle_vel = rospy.Publisher('/tb3_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    rate = rospy.Rate(10.0)

    while not rospy.is_shutdown():
        msg = geometry_msgs.msg.Twist()

        while i < 1000:     # 根据自己mat数据的大小决定
            (roll, pitch, yaw) = euler_from_quaternion([x_o, y_o, z_o, w_o])
            if yaw < 0:
                yaw = yaw + 2 * math.pi

            X_t = sizeData[0][i]
            Y_t = sizeData[1][i]

            D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))

            if (Y_t - y) == 0 and (X_t - x) > 0:
                yaw_t = 0
            if (Y_t - y) > 0 and (X_t - x) > 0:
                yaw_t = math.atan((Y_t - y) / (X_t - x))
            if (Y_t - y) > 0 and (X_t - x) == 0:
                yaw_t = 0.5 * math.pi
            if (Y_t - y) > 0 and (X_t - x) < 0:
                yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
            if (Y_t - y) == 0 and (X_t - x) < 0:
                yaw_t = math.pi
            if (Y_t - y) < 0 and (X_t - x) < 0:
                yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
            if (Y_t - y) < 0 and (X_t - x) == 0:
                yaw_t = 1.5 * math.pi
            if (Y_t - y) < 0 and (X_t - x) > 0:
                yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi

            Theta_err = yaw_t - yaw
            if Theta_err < -math.pi:
                Theta_err = Theta_err + 2 * math.pi
            if Theta_err > math.pi:
                Theta_err = Theta_err - 2 * math.pi

            if D_err < 0.5:
                i = i + 1
                print i
                if i >= 1000:
                    i = 1000

            liner_speed = 0.2 * D_err
            angular_speed = 0.6 * Theta_err
            if liner_speed > 0.4:   # 对小车进行限制最高速度
                liner_speed = 0.4
            if liner_speed < 0.2:   # 对小车进行限制最低速度
                liner_speed = 0.2
            if angular_speed > 1:
                angular_speed = 1

            msg.linear.x = liner_speed
            msg.angular.z = angular_speed

            liner_speed_old = liner_speed
            angular_speed_old = angular_speed

            turtle_vel.publish(msg)                 # 向/cmd_vel话题发布数据
            rospy.Subscriber('/tb3_0/odom', nav_msgs.msg.Odometry, Trans_robot_pose)

            rate.sleep()  # 以固定频率执行

        # 如果到达最后一个点,让小车停下来
        msg.linear.x = 0.0
        msg.angular.z = 0.0
        print msg.linear.x

        turtle_vel.publish(msg)                 # 向/cmd_vel话题发布数据
        rospy.Subscriber('/tb3_0/odom', nav_msgs.msg.Odometry, Trans_robot_pose)

        rate.sleep()  # 以固定频率执行
    rospy.spin()  # 保持节点运行,直到节点关闭

  在打开launch文件后直接rosrun该py文件即可看到小车沿着Matlab数据文件中的轨迹运动。   同样可以使用rviz工具来查看小车的运动轨迹