在大多数时间都是依靠ROS节点来发布恰当的Twist消息。举个简单的例子,假设我们想编程使机器人向前移动1m,旋转180度,然后返回开始的位置。我们会尝试用不同的方法来完成这个任务,这些方法很好地表现了ROS不同层次的运动控制。
1、通过定时和定速估计距离和角度
我们第一个尝试是用定时的Twist命令去让机器人花一定时间向前移动一定的距离,旋转180度后,然后以相同的速度进行相同时长的向前移动,不出意外的话它会回到开始的位置。最后我们会让机器人旋转180度回到最初的方向。
这段脚本可以在子目录rbx1_nav/nodes下的timed_out_and_back.py中找到。
2、在ArbotiX模拟器上进行计时前进并返回运动
为了保证模拟的TurtleBot回到开始的位置,按下“Ctrl-C”让模拟的TurtleBot中正在运行的启动文件停止运行,然后用以下命令让它重新运行:
roslaunch rbx1_bringup fake_turtlebot.launch
如果你愿意的话,可以用对应Pi Robot或者你自己的机器人的文件替换掉fake_turtlebot.launch,这不会使结果有差别。
如果RViz并不是正在运行,那么就让它开始运行:
rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz
或者按下Reset按钮删除掉在上一部分留下的Odometry箭头。
最后运行timed_out_and_back.py节点:
rosrun rbx1_nav timed_out_and_back.py
3、计时前进并返回运动的脚本
以下是计时前进并返回节点的完整脚本。
![](https://i-blog.csdnimg.cn/blog_migrate/8f900a89c6347c561fdf2122f13be562.gif)
![](https://i-blog.csdnimg.cn/blog_migrate/961ddebeb323a10fe0623af514929fc1.gif)
#!/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.")
#!/usr/bin/env python
import rospy
如果阅读了ROS Beginner Tutorials in Python,你会知道所有ROS节点都是以这两句开头。第一行确保了这个脚本会被看作Python程序脚本,而第二行引用ROS的核心Python库。
from geometry_msgs.msg import Twist
from math import pi
这里我们引用了其他一些我们在脚本中需要用到的组件,在当前的例子中,我们需要用到ROS的geometry_msgs包中的Twist消息类型,和Python的math模块中的常数pi。请注意一个常见的引用错误,即忘记在你的包中的package.xml文件中写上必要的ROS<run_depend>。在当前的例子中,我们的package.xml文件必须要有这行才能正确从geometry_msgs.msg中引用Twist:
<run_depend>geometry_msgs</run_depend>
class OutAndBack():
def __init__(self):
这里ROS节点主要部分的开头把它自己定义成了一个Python类,并加上一行标准的类初始化。
#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)
每个ROS节点都被要求调用rospy.init_node(),我们也可以设置一个回调函数on_shutdown(),这样我们就可以在脚本结束的时候,比如用户按下ctrl-C的时候,进行必要的清理操作。对于一个移动机器人,最重要的清理操作就是让机器人停下来。我们可以在后面的脚本中看如何做到这点。
#Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel',Twist) #How fast will we update the robot's movement? rate=50 #set the equivalent ROS rate variable r=rospy.Rate(rate)
这里定义了我们用来发布Twist命令给/cmd_vel话题的ROS发布者。还设定了以每秒50次的频率来更新机器人的运动
#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=linear_distance/linear_speed
以相对安全的0.2米/秒的速度初始化前进速度,并把目标距离设定为1.0米,接着计算运动需要多长时间。
# Set the rotation speed to 1.0 radians per second angular_speed=1.0 #Ser the rotation angle to Pi radians(180 degrees) goal_angle=pi #How long should it take to rotate? angular_duration=angular_distance/angular_speed
设定旋转速度为1.0弧度/秒,目标角度为180度或Pi弧度。
#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()
正是这里的循环使机器人运动起来,每循环一次就运动一段。因为一些机器人要求不断发布Twist消息来使它持续运动,所以为了让机器人以linear_speed米/秒的速度向前移动linear_distance米,我们需要在恰当的时长内每1/rate秒发布一次move_cmd消息。语句r.sleep()是rospy.sleep(1/rate)的简写。因为我们把变量赋值为r=rospy.Rate(rate)。
#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()
在循环的第二部分,我们让机器人在恰当时间段内以angular_speed弧度/秒的速度旋转,最终一共旋转180度。
#Stop the robot self.cmd_vel.publish(Twist())
当机器人完成整个计时前进并返回的过程后,我们发布一条空的Twist消息(所有项都设为0)让它停止运动
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)
这个是我们的停机回调函数,如果脚本因为任何原因停止运行,我们就会通过发布一条空的Twist消息去停止机器人
if __name__ == '__main__' try OutAndBack() except rospy.ROSInterruptException: rospy.loginfo("Out-and-Back node terminated.")
最后这段是运行一段Python脚本的标准程序块。我们实例化OutAndBack类来使脚本(和机器人)运行。