通过Gazebo仿真学TurtleBot3（四）——简单的/cmd

1. 通过命令发送/cmd_vel控制底盘移动

- 启动tb3仿真：

　　启动：

$ roslaunch tb3_sim_bringup tb3_empty_world.launch

　　开启rviz显示：

$ rosrun rviz rviz -d `rospack find tb3_sim_bringup`/rviz/demo.rviz

- 通过命令发布/cmd_vel话题控制底盘：

$ rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'

　　tb3开始移动，效果如下：

　　要想停止移动，首先按ctrl+c终止rostopic发送，再输入：

$ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'

2. 通过node节点发送/cmd_vel控制底盘移动

- 停止之前的仿真：

　　分别按ctrl+c停止之前的节点。

- 从新启动仿真

　　启动：

$ roslaunch tb3_sim_bringup tb3_empty_world.launch

　　开启rviz显示：

$ rosrun rviz rviz -d `rospack find tb3_sim_bringup`/rviz/demo.rviz

　　运行timed_out_and_back.py开启一个新节点。

$ rosrun tb3_sim_nav timed_out_and_back.py

- 代码解析

#!/usr/bin/env pythonimport rospy
from geometry_msgs.msg import Twist
from math import piclass OutAndBack():def __init__(self):# Give the node a namerospy.init_node('out_and_back', anonymous=False)# Set rospy to execute a shutdown function when exiting       rospy.on_shutdown(self.shutdown)# Publisher to control the robot's speedself.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)# How fast will we update the robot's movement?rate = 50# Set the equivalent ROS rate variabler = rospy.Rate(rate)# Set the forward linear speed to 0.2 meters per second linear_speed = 0.2# Set the travel distance to 1.0 metersgoal_distance = 1.0# How long should it take us to get there?linear_duration = goal_distance / linear_speed# Set the rotation speed to 1.0 radians per secondangular_speed = 1.0# Set the rotation angle to Pi radians (180 degrees)goal_angle = pi# How long should it take to rotate?angular_duration = goal_angle / angular_speed# Loop through the two legs of the trip  for i in range(2):# Initialize the movement commandmove_cmd = Twist()# Set the forward speedmove_cmd.linear.x = linear_speed# Move forward for a time to go the desired distanceticks = int(linear_duration * rate)for t in range(ticks):self.cmd_vel.publish(move_cmd)r.sleep()# Stop the robot before the rotationmove_cmd = Twist()self.cmd_vel.publish(move_cmd)rospy.sleep(1)# Now rotate left roughly 180 degrees  # Set the angular speedmove_cmd.angular.z = angular_speed# Rotate for a time to go 180 degreesticks = int(goal_angle * rate)for t in range(ticks):           self.cmd_vel.publish(move_cmd)r.sleep()# Stop the robot before the next legmove_cmd = Twist()self.cmd_vel.publish(move_cmd)rospy.sleep(1)    # Stop the robotself.cmd_vel.publish(Twist())def shutdown(self):# Always stop the robot when shutting down the node.rospy.loginfo("Stopping the robot...")self.cmd_vel.publish(Twist())rospy.sleep(1)if __name__ == '__main__':try:OutAndBack()except:rospy.loginfo("Out-and-Back node terminated.")

　　该段代码主要实现前进一个指定量的距离，以及转动一个指定量的角度。具体实现思路是通过恒定线速度乘以时间来估计距离，通过恒定角速度乘以时间来估计转动角度。可以看出这是一种非常简单的开环控制，因此在物理机器人实际环境中的应用十分不可靠，但是仿真效果还行。要想实现精确的闭环控制还需要结合/odom话题来加以实现。

3. /odom与/cmd_vel控制的结合

- 停止之前的仿真：

　　分别按ctrl+c停止之前的节点。

- 从新启动仿真

　　启动：

$ roslaunch tb3_sim_bringup tb3_empty_world.launch

　　开启rviz显示：

$ rosrun rviz rviz -d `rospack find tb3_sim_bringup`/rviz/demo.rviz

　　运行odom_out_and_back.py开启一个新节点。

$ rosrun tb3_sim_nav odom_out_and_back.py

- 代码解析

#!/usr/bin/env pythonimport rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, piclass OutAndBack():def __init__(self):# Give the node a namerospy.init_node('out_and_back', anonymous=False)# Set rospy to execute a shutdown function when exiting       rospy.on_shutdown(self.shutdown)# Publisher to control the robot's speedself.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)# How fast will we update the robot's movement?rate = 20# Set the equivalent ROS rate variabler = rospy.Rate(rate)# Set the forward linear speed to 0.15 meters per second linear_speed = 0.15# Set the travel distance in metersgoal_distance = 1.0# Set the rotation speed in radians per secondangular_speed = 0.5# Set the angular tolerance in degrees converted to radiansangular_tolerance = radians(1.0)# Set the rotation angle to Pi radians (180 degrees)goal_angle = pi# Initialize the tf listenerself.tf_listener = tf.TransformListener()# Give tf some time to fill its bufferrospy.sleep(2)# Set the odom frameself.odom_frame = '/odom'# Find out if the robot uses /base_link or /base_footprinttry:self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))self.base_frame = '/base_footprint'except (tf.Exception, tf.ConnectivityException, tf.LookupException):try:self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))self.base_frame = '/base_link'except (tf.Exception, tf.ConnectivityException, tf.LookupException):rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")rospy.signal_shutdown("tf Exception")  # Initialize the position variable as a Point typeposition = Point()# Loop once for each leg of the tripfor i in range(2):# Initialize the movement commandmove_cmd = Twist()# Set the movement command to forward motionmove_cmd.linear.x = linear_speed# Get the starting position values     (position, rotation) = self.get_odom()x_start = position.xy_start = position.y# Keep track of the distance traveleddistance = 0# Enter the loop to move along a sidewhile distance < goal_distance and not rospy.is_shutdown():# Publish the Twist message and sleep 1 cycle         self.cmd_vel.publish(move_cmd)r.sleep()# Get the current position(position, rotation) = self.get_odom()# Compute the Euclidean distance from the startdistance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2))# Stop the robot before the rotationmove_cmd = Twist()self.cmd_vel.publish(move_cmd)rospy.sleep(1)# Set the movement command to a rotationmove_cmd.angular.z = angular_speed# Track the last angle measuredlast_angle = rotation# Track how far we have turnedturn_angle = 0while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():# Publish the Twist message and sleep 1 cycle         self.cmd_vel.publish(move_cmd)r.sleep()# Get the current rotation(position, rotation) = self.get_odom()# Compute the amount of rotation since the last loopdelta_angle = normalize_angle(rotation - last_angle)# Add to the running totalturn_angle += delta_anglelast_angle = rotation# Stop the robot before the next legmove_cmd = Twist()self.cmd_vel.publish(move_cmd)rospy.sleep(1)# Stop the robot for goodself.cmd_vel.publish(Twist())def get_odom(self):# Get the current transform between the odom and base framestry:(trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))except (tf.Exception, tf.ConnectivityException, tf.LookupException):rospy.loginfo("TF Exception")returnreturn (Point(*trans), quat_to_angle(Quaternion(*rot)))def shutdown(self):# Always stop the robot when shutting down the node.rospy.loginfo("Stopping the robot...")self.cmd_vel.publish(Twist())rospy.sleep(1)if __name__ == '__main__':try:OutAndBack()except:rospy.loginfo("Out-and-Back node terminated.")

　　该段代码通过提取gazebo给出的base_frame与odom_frame之间的tf关系，给出机器人的实时位置和姿态角，再通过与起始时刻位置姿态进行对比，得到前进距离和转过角度的实时信息，将其反馈并与/cmd_vel结合，就形成了闭环控制。闭环控制的好处是可以较好的低效地面摩擦、转动轴承摩擦等阻力对于精确控制带来的影响，因此控制精度会有较大提升。但实际物理世界使用过程中odom里程计测量并不完美，会有随时间增长的不可逆的误差累计，因此实际使用过程中的效果也不会很理想，当然通过标定可以降低这些误差，但不能完全消除。
　　在gazebo仿真下，以上两个例子的效果区分并不明显，而且与实际物理机器人使用效果有较大不同，其原因是gazebo仿真中目前并没有细化考虑这些阻力和传感器误差。如果希望可以与真实机器人使用结果更为接近，可以在gazebo中加入较大的摩擦阻力和传感器误差。不过这并不是我们作为初学者的学习目的，因此不必过于纠结。

3. 控制命令组合

　　最后，我们再尝试一下上面运行命令的组合效果，跑一个正方形的轨迹。这基本上是之前运动命令的直接叠加，因此我们不再进行代码的解析，读者可以自行查看代码。

- 停止之前的仿真：

　　分别按ctrl+c停止之前的节点。

- 从新启动仿真

　　启动：

$ roslaunch tb3_sim_bringup tb3_empty_world.launch

　　开启rviz显示：

$ rosrun rviz rviz -d `rospack find tb3_sim_bringup`/rviz/demo.rviz

　　运行nav_square.py开启一个新节点。

$ rosrun tb3_sim_nav nav_square.py

　　运行效果：

参考资料：
[1] ROS官方wiki：http://wiki.ros.org/
[2] TurtleBot3电子手册：http://emanual.robotis.com/docs/en/platform/turtlebot3/overview/
[3] 《ROS by Example》 R.Patrick Goebel