前言
上一篇文章 “多机器人协同控制①——仿真平台搭建” 里已经介绍了多台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 x 和 angular z 两个数据分别控制小车的线速度和角速度即可。
2.到达指定点
在上一篇文章中我们可以使用demo进行键盘控制小车运动、SLAM建图后进行自主运动等。但是在我们做实验验证的时候，有些情况需要让小车沿着给定轨迹运动，下面就针对这种情况分析一下。 首先我们可以通过 odom 这个话题接收小车的里程计信息，进而获取小车自身的坐标位置，因为仿真环境中环境影响较小，且我们目前的研究重点在小车的编队上，因此我们暂时可以将小车的 odom 信息视作小车的真实位姿。 接下来我们可以给定坐标点，控制小车到达目标点，由于程序比较简单这里直接放出Python代码：
- ROS操作的详细步骤可以移步ROS入门21讲或者我的其他博客
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_sim 和 Y_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_t 和 Y_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工具来查看小车的运动轨迹
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明多机器人协同控制②——简单的轨迹控制！
