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 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.")该段代码主要实现前进一个指定量的距离，以及转动一个指定量的角度。具体实现思路是通过恒定线速度乘以时间来估计距离，通过恒定角速度乘以时间来估计转动角度。可以看出这是一种非常简单的开环控制，因此在物理机器人实际环境中的应用十分不可靠，但是仿真效果还行。要想实现精确的闭环控制还需要结合/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 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.") 该段代码通过提取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
扫一扫
645
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
