【仿真】Ros by example1 控制底座、导航、路径规划、SLAM代码注释

一、控制底座

效果：让机器人花一段时间向前移动1m，再旋转180度，最后返回原点。

1、基于定时的timed_out_and_back.py

# -*- coding: utf-8 -*-    有中文注释
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from math import pi

class OutAndBack():
    def __init__(self):
        #1、初始化：给出节点名字，设置回调函数
        rospy.init_node('out_and_back', anonymous=False)
        rospy.on_shutdown(self.shutdown)
        
        #2、发布话题，机器人运动速度
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        rate = 50
        r = rospy.Rate(rate)
        #3、设定前进速度、距离；转动速度、距离
        linear_speed = 0.2
        goal_distance = 1.0
        linear_duration = goal_distance / linear_speed
        angular_speed = 1.0
        goal_angle = pi
        angular_duration = goal_angle / angular_speed
     
        #往返两次行程 
        for i in range(2):
            #4、前进1m
            move_cmd = Twist()
            move_cmd.linear.x = linear_speed
            ticks = int(linear_duration * rate)
            
            for t in range(ticks):
                self.cmd_vel.publish(move_cmd)
                r.sleep()
            
            # 旋转之前停下来
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
            # 5、转动180度
            move_cmd.angular.z = angular_speed
            ticks = int(goal_angle * rate)
            
            for t in range(ticks):           
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                
            # 下一步之前停下来
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)    
            
        # 发布空的Twist消息让机器人停下来
        self.cmd_vel.publish(Twist())
        
    def shutdown(self):
        # 发布空的Twist消息让机器人停下来
        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.")

OutAndBack类主要分为5部分：1、初始化：给出节点名字，设置回调函数；2、发布话题，机器人运动速度；3、设定前进速度、距离；转动速度、距离；4、循环部分，前进1m，旋转180度；5、主函数

接下来将详细介绍5部分内容

1、def __init__(self) 标准的类初始化，self类似于C++中this指针。

2、self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 发布Twist命令给/cmd_vel话题，且每秒50次更新机器人运动

4、前进1m

            # 初始化运动命令       
            move_cmd = Twist()
            # 设定前进速度
            move_cmd.linear.x = linear_speed
            # 在前进1米的这段时间控制命令更新的次数
            ticks = int(linear_duration * rate)
            for t in range(ticks):
                self.cmd_vel.publish(move_cmd)
                r.sleep()

 r.sleep（）是rospy.sleep(1/rate)的简单写法。

r=rospy.Rate(rate) 保持一定的速率来进行循环。

我们需要每1/rate秒发布一次move_cmd消息。一共发布linear_duration*rate次消息。

5、标准程序块，实例化OutAndBack类运行脚本。

if __name__ == '__main__':
    try:
        OutAndBack()
    except:
        rospy.loginfo("Out-and-Back node terminated.")

---

扫描二维码关注公众号，回复： 4082823 查看本文章

2、基于测量的odomz-out_and_back.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi

class OutAndBack():
    def __init__(self):
        # 1、初始化：给出节点名称，设置回调函数
        rospy.init_node('out_and_back', anonymous=False)      
        rospy.on_shutdown(self.shutdown)
        # 2、发布话题、前进速度和旋转速度
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        rate = 20
        r = rospy.Rate(rate)
        linear_speed = 0.15
        goal_distance = 1.0
        angular_speed = 0.5
        angular_tolerance = radians(1.0)
        goal_angle = pi
        # 3、新增的一系列功能：tf、odom、base_footprint、Point
        # 初始化tf监听器，填补缓冲区
        self.tf_listener = tf.TransformListener()
        rospy.sleep(2)
        # 设置odom坐标系
        self.odom_frame = '/odom'
        # 询问机器人使用/base_link坐标系还是/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")         
        # 初始化Point类型的变量
        position = Point()
        # 4、循环，前进1m，转动180度
        for i in range(2):
            # 前进1m
            move_cmd = Twist()
            move_cmd.linear.x = linear_speed
            # 得到开始的位姿信息
            (position, rotation) = self.get_odom()            
            x_start = position.x
            y_start = position.y  
            # 追踪行驶距离
            distance = 0  
            # 循环
            while distance < goal_distance and not rospy.is_shutdown():
                # 发布一次消息，睡眠      
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                # 得到当前位姿
                (position, rotation) = self.get_odom()
                # 计算位移
                distance = sqrt(pow((position.x - x_start), 2) + 
                                pow((position.y - y_start), 2))
            # 转动前停止
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
            # 给旋转配置运动命令
            move_cmd.angular.z = angular_speed
            # 跟踪最后角度
            last_angle = rotation
            # 跟踪转动的角度
            turn_angle = 0  
            while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
                # 发布一次消息，睡眠        
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                # 得到当前位姿，计算角度变换
                (position, rotation) = self.get_odom()
                delta_angle = normalize_angle(rotation - last_angle)
                # 添加到合计中
                turn_angle += delta_angle
                last_angle = rotation          
            # 转动前停止
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)   
        # 停止机器人
        self.cmd_vel.publish(Twist())
        
    def get_odom(self):
        # 得到现测量值与基框架间转换
        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.")

在上一节基础上增加了

3、新增的一系列功能：tf、odom、base_footprint、Point；4、对于前进直线和旋转的记录。

---

1、self.tf_listener = tf.TransformListener() 

      rospy.sleep(2)

我们创建一个TransformListener对象来监视坐标系转换（frame transforms）。 注意 tf需要一些时间去填补监听者(listener)的缓冲区，所以我们需要调用 rospy.sleep(2)。

2、get_odom()

(position, rotation) = self.get_odom()
    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)))

首先使用tf_listener对象查找当前 odom 坐标系和 base（基础）坐标系之间的变换。正常的话返回位置和旋转（四元数表示）

其中：return (Point(*trans), quat_to_angle(Quaternion(*rot))) 这段语句。在 python 中使用 “ * ” 修饰的trans 和 rot 变量，表示传给一个函数的形参是一个列表。我们这里的 trans 是 x、y、z 轴坐标值；rot是 x、y、z、w 四元数值。

---

总结：

上一篇文章核心思想是：通过前进1m这段时间控制命令更新的次数。

ticks = int(linear_duration * rate)
for t in range(ticks):
      self.cmd_vel.publish(move_cmd)
        r.sleep()

本文使用TransformListener 来访问 odom信息，使用 tf 去监视 /odom 坐标系与 /base_link 坐标系之间的转换要比依赖 /odom 话题要安全的多。