3.1-里程计

1. 实训目的

基于里程计构建闭环的机器人运动控制系统，即通过机器人的自体运动测量来估计机器人的位移，使机器人在gazebo仿真环境完成特定动作。

1. 实训操作步骤

我们需要完成以下步骤：

• 使用RViz显示机器人模型，实时显示雷达扫描和里程计坐标系的变化。

• 监听/base_footprint与/odom之间的tf变换关系

• 计算相对起点的位姿变化，控制机器人完成指定动作

1. 搭建RViz仿真环境

进入自己工作空间的src目录下，通过如下命令创建一个名为proj03_odom的功能包：

$ catkin_create_pkg proj03_odom std_msgs rospy

进入功能包目录下，创建/scripts目录用于存放Python脚本，创建/rviz目录用于存放RViz文件，创建/launch目录用于存放launch文件。

在/launch目录下，创建odom_rviz.launch，用于启动机器人状态和关节状态发布节点，将机器人模型导入RViz中。

odom_rviz.launch：

<launch>

    <arg name="model" default="waffle_pi" />

   

    <!-- 在参数服务器载入xacro文件 -->

    <param name="robot_description" command="$(find xacro)/xacro $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

 

    <!-- 启动机器人状态和关节状态发布节点 -->

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />

    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />

 

    <!-- 启动rviz，或启动已保存配置的rviz文件 -->

    <node name="rviz" pkg="rviz" type="rviz" />

    <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find proj03_odom)/rviz/odom.rviz" required="true" /> -->

</launch>

使用如下命令启动gazebo仿真环境：

$ export TURTLEBOT3_MODEL=waffle_pi

$ roslaunch  turtlebot3_gazebo  turtlebot3_house.launch

此时tf树如下图所示：

使用如下命令监听odom话题：

$ rostopic echo odom

可以看到里程计信息，里程计消息结构如下图所示：

其中pose代表机器人当前的位置和姿态，而 twist代表机器人当前速度信息。

启动odom_rviz.launch，分别添加机器人模型、雷达话题、里程计话题、摄像头话题，在Fixed Frame下拉列表中选择odom。如不存在odom坐标系，重启仿真环境。

$ source devel/setup.bash

$ roslaunch proj03_odom odom_rviz.launch

修改Fixed Frame，添加机器人模型：

添加摄像头、里程计、雷达话题：

取消勾选里程计话题的Cov可视化，将雷达点云的size修改为0.03：

配置完成后，可选择保存配置文件odom.rviz到功能包目录的rviz目录下，修改odom_rviz.launch，即可通过roslaunch直接打开配置好的RViz界面。

1. 编写TF监听脚本

TF是一个让用户实时跟踪多个坐标系的功能包，它使用树形数据结构，实时维护多个坐标系之间的坐标变换关系，我们需要监听TF变换来获取机器人相对起始位置的位置与姿态关系，即载体坐标系与里程计坐标系之间的旋转与平移关系。

我们仿真机器人的载体坐标系名称为/base_footprint，里程计坐标系名称为/odom，因此我们需要监听的是/base_footprint到/odom之间的TF变换。

进入scripts文件夹下，通过如下命令创建TF监听脚本：

$ gedit get_pose.py

get_pose.py内容如下：

#!/usr/bin/env python

# -*- coding: UTF-8 -*-

import rospy

from tf_conversions import transformations

from math import pi

import tf

 

class Robot:

    def __init__(self):

        self.tf_listener = tf.TransformListener()

        try:

            # 等待tf消息

            self.tf_listener.waitForTransform('/odom', '/base_footprint', rospy.Time(),rospy.Duration(1.0))

        except (tf.Exception, tf.ConnectivityException, tf.LookupException):

            return

    def get_pose(self):

        try:

            # 获取当前时刻/odom到/base_footprint之间的旋转平移关系

            (trans, rot) = self.tf_listener.lookupTransform('/odom', '/base_footprint',rospy.Time(0))

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):

            rospy.loginfo("tf Error")

            return None

        # 将四元数形式的旋转转化为欧拉角形式，euler[2]为yaw角

        euler = transformations.euler_from_quaternion(rot)

        #print (euler[2] / pi * 180)

        x = trans[0]

        y = trans[1]

        th = euler[2] / pi * 180

        return (x, y, th)

 

if __name__ == "__main__":

    rospy.init_node('get_pose',anonymous=True)

    robot = Robot()

    r = rospy.Rate(100)

    r.sleep()

    while not rospy.is_shutdown():

        print(robot.get_pose())

        r.sleep()

在scripts文件夹下使用如下命令给get_pose.py脚本添加可执行权限：

$ chmod 777 get_pose.py

修改功能包下的CmakeLists，添加如下几行：

catkin_install_python(PROGRAMS

  scripts/get_pose.py

  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

)

编译工作空间，如上一节打开仿真环境后，另起一个终端，输入如下命令运行脚本：

$ source devel/setup.bash

$ rosrun proj03_odom get_pose.py

可以看到脚本将获取到的里程计信息打包成(x，y，yaw)的数组形式打印出来，如下图。其中x 代表 ros 坐标系下机器人前后位置变化(前正后负)，y代表 ros 坐标系下机器人左右位置变化(左正右负)，yaw 代表机器人机头朝向 (左正右负)。

另起一个终端，输入如下命令打开键盘控制节点：

$ export TURTLEBOT3_MODEL=waffle_pi

$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

控制机器人在地图中行走，可以看到前述里程计信息也发生了变化。

同时可以在RViz中看到如下效果：

1. 编写里程计运动控制脚本

上一节实现了对TF变换的监听，这一节使用监听到的TF变换来估计机器人相对起点的位置和姿态，从而使机器人完成我们指定的行走动作。

在scripts文件夹下通过如下命令创建里程计运动控制脚本：

$ gedit move_with_odom.py

move_with_odom.py内容如下：

#!/usr/bin/env python

# -*- coding: UTF-8 -*-

import rospy

from geometry_msgs.msg import Twist, Point, Quaternion

import tf

from angles import normalize_angle

from tf.transformations import euler_from_quaternion

from math import radians, copysign, sqrt, pow, pi

 

class alongTheRectangle():

    def __init__(self):

        rospy.init_node('along_the_rectangle', anonymous=False)   

        rospy.on_shutdown(self.shutdown)

        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

 

        # 更新频率

        rate = 20

        r = rospy.Rate(rate)

        # 前进速度

        linear_speed = 0.2

        # 目标距离

        goal_distance = 1.5

        # 旋转速度

        angular_speed = 0.5

        # 角度容忍度，这里设置0.5°的角度容忍度，转换成弧度单位

        angular_tolerance = radians(0.5)

        # 一程运动结束后旋转的角度

        goal_angle = pi/2

 

        # 初始化tf监听器

        self.tf_listener = tf.TransformListener()

        rospy.sleep(2)

        self.odom_frame = '/odom'

        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):

            rospy.loginfo("Cannot find transform between /odom and or /base_footprint")

            rospy.signal_shutdown("tf Exception") 

        position = Point()

       

        # 一次循环完成一程行走

        for i in range(4):

            move_cmd = Twist()

            move_cmd.linear.x = linear_speed  

            rospy.loginfo("Run!")

            (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()

                # 将角度转换到[-pi,pi]区间

                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

        (r,p,y) =  tf.transformations.euler_from_quaternion(rot)

        return (Point(*trans), y)

 

    def shutdown(self):

        # 当节点关闭时使机器人停止运动

        rospy.loginfo("Stopping the robot...")

        self.cmd_vel.publish(Twist())

        rospy.sleep(1)

 

if __name__ == '__main__':

    try:

        alongTheRectangle()

    except:

        rospy.loginfo("along-the-rectangle node terminated.")

同上一节添加脚本可执行权限、修改CmakeLists及运行脚本，实现里程计运动控制。

RViz中显示效果如下：

1. 实训结果

1. 了解RViz的配置，学习从launch文件启动RViz。

2. 学习TF变换监听，获取两个坐标系间的位置姿态变换关系。

3. 学习根据里程计信息控制机器人完成指定动作。