通过Gazebo仿真学TurtleBot3（四）——简单的/cmd_vel控制_turtlebot3 odom话题

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

- 启动tb3仿真：

　　启动：

$ roslaunch tb3_sim_bringup tb3_empty_world.launch
1

　　开启rviz显示：

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

- 通过命令发布/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}}'
1

　　tb3开始移动，效果如下：

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

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

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

- 停止之前的仿真：

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

- 从新启动仿真

　　启动：

$ roslaunch tb3_sim_bringup tb3_empty_world.launch
1

　　开启rviz显示：

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

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

$ rosrun tb3_sim_nav timed_out_and_back.py
1

- 代码解析

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from math import pi

class OutAndBack():
    def __init__(self):
        # Give the node a name
        rospy.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 speed
        self.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 variable
        r = 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 meters
        goal_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 second
        angular_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 command
            move_cmd = Twist()

            # Set the forward speed
            move_cmd.linear.x = linear_speed

            # Move forward for a time to go the desired distance
            ticks = int(linear_duration * rate)

            for t in range(ticks):
                self.cmd_vel.publish(move_cmd)
                r.sleep()

            # Stop the robot before the rotation
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

            # Now rotate left roughly 180 degrees  

            # Set the angular speed
            move_cmd.angular.z = angular_speed

            # Rotate for a time to go 180 degrees
            ticks = int(goal_angle * rate)

            for t in range(ticks):           
                self.cmd_vel.publish(move_cmd)
                r.sleep()

            # Stop the robot before the next leg
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)    

        # Stop the robot
        self.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.")
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

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

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

- 停止之前的仿真：

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

- 从新启动仿真

　　启动：

$ roslaunch tb3_sim_bringup tb3_empty_world.launch
1

　　开启rviz显示：

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

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

$ rosrun tb3_sim_nav odom_out_and_back.py
1

- 代码解析

#!/usr/bin/env python

import 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, pi

class OutAndBack():
    def __init__(self):
        # Give the node a name
        rospy.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 speed
        self.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 variable
        r = rospy.Rate(rate)

        # Set the forward linear speed to 0.15 meters per second 
        linear_speed = 0.15

        # Set the travel distance in meters
        goal_distance = 1.0

        # Set the rotation speed in radians per second
        angular_speed = 0.5

        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(1.0)

        # Set the rotation angle to Pi radians (180 degrees)
        goal_angle = pi

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Set the odom frame
        self.odom_frame = '/odom'

        # Find out if the robot uses /base_link or /base_footprint
        try:
            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 type
        position = Point()

        # Loop once for each leg of the trip
        for i in range(2):
            # Initialize the movement command
            move_cmd = Twist()

            # Set the movement command to forward motion
            move_cmd.linear.x = linear_speed

            # Get the starting position values     
            (position, rotation) = self.get_odom()

            x_start = position.x
            y_start = position.y

            # Keep track of the distance traveled
            distance = 0

            # Enter the loop to move along a side
            while 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 start
                distance = sqrt(pow((position.x - x_start), 2) + 
                                pow((position.y - y_start), 2))

            # Stop the robot before the rotation
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

            # Set the movement command to a rotation
            move_cmd.angular.z = angular_speed

            # Track the last angle measured
            last_angle = rotation

            # Track how far we have turned
            turn_angle = 0

            while 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 loop
                delta_angle = normalize_angle(rotation - last_angle)

                # Add to the running total
                turn_angle += delta_angle
                last_angle = rotation

            # Stop the robot before the next leg
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)

        # Stop the robot for good
        self.cmd_vel.publish(Twist())

    def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (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")
            return

        return (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.")
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153

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

3. 控制命令组合

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

- 停止之前的仿真：

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

- 从新启动仿真

　　启动：

$ roslaunch tb3_sim_bringup tb3_empty_world.launch
1

　　开启rviz显示：

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

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

$ rosrun tb3_sim_nav nav_square.py
1

　　运行效果：

---

参考资料：
[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