差速小车DiffCart

我们想在Gazebo的仿真环境中运行Cartographer进行激光建图，虽然已经构建了一个多线激光雷达的模型并成功的获得了激光点云。 但是我们还需要一个可以移动的东西带着雷达到处溜达，本着简单实用的原则，我们先构建一个差速小车来进行激光建图。

本文中，我们将介绍差速驱动的基本原理，并根据官方教程创建一个差速小车DiffCart。 然后为之写一个插件，订阅ROS的控制指令，使得我们可以像turtlebot那样通过键盘来控制它。

1. 差速驱动原理

差速驱动是目前市面上比较常见的一种移动机器人驱动方式，Turtlebot就是一个典型的例子。如右图所示， 一个差速小车主要由一对共轴的轮子提供前进和转向的动力，这两个轮子分别由两个电机独立驱动，它们的转速组合决定了机器人的运动轨迹。

如右图所示，差速小车的运动涉及到\(O\)系和\(A\)系两个坐标系。其中\(O\)系是一个全局坐标系描述了小车的运动空间，\(A\)系是一个跟随小车一起运动的局部坐标系。 此外，局部坐标系的原点\(A\)是两个轮子的中点。我们假设小车是一个刚体，并且左右两个轮子只有纯滚动运动，不会打滑。在\(A\)系下观察小车以以线速度\(v\)角速度\(\omega\)运动。 此时，点\(C\)是点\(A\)瞬时的转动中心，\(R = |AC|\)是小车的转弯半径。 根据质点运动学，有：

$$ R = \cfrac{v}{\omega} $$

通过分析质点的转动，我们知道，点\(A\)的运动轨迹可以由一系列的圆弧拼接而成。 而这些圆弧的半径都是由上式决定的，所以控制小车在\(O\)系下按照指定的轨迹运动，只需要给定线速度和角速度就可以了。对于差速小车而言，还需要进一步的， 将线速度\(v\)和角速度\(\omega\)转换成左右两个轮子的转速\(n_L, n_R\)。

一般我们说轮子或者电机的转速时，默认的量纲是转每分。这里我们采用Gazebo仿真所用的量纲——弧度每秒。由于小车可以看做是绕着瞬时的旋转中心\(C\)转动， 那么左右轮转速与机器人的转弯半径和角速度之间存在如下的几何关系：

$$ \begin{array}{rcl} n_R · r = \omega \left(R + l/2\right) \\ n_L · r = \omega \left(R - l/2\right) \end{array} \Rightarrow \begin{array}{rcl} n_R = (v + 0.5 \omega l) / r \\ n_L = (v - 0.5 \omega l) / r \end{array} $$

其中，\(l\)是两个轮子之间的距离，\(r\)则是轮半径。这俩都是机械参数可以通过测量得到。上式给出了小车的线速度和角速度到两轮转速之间的变换关系。

2. DiffCart的模型描述

我们在models子目录下新建一个目录diffcart，并在其中创建了一个xml文件model.config。 根据Gazebo的模型目录约定，这个model.config是必须要有的，它和ROS包中的pacakge.xml的作用类似， 记录了关于模型的各种描述信息(meta information)，包括模型名称和简述、作者信息、依赖关系等。

实际描述DiffCart模型的是一个称为model.sdf的文件。 这是一个xml文件，它通过SDF描述机器人模型。下面的左图是折叠之后的model.sdf文件，可以看到DiffCart由三个link构成， 其中左右轮(left_wheel, right_wheel)分别通过两个"revolute"类型的joint与基座(base)连接。

左右轮是两个半径为0.1m高0.05m的圆柱，两轮相距0.26m。这里的轮间距和轮半径是控制插件中需要用到的两个重要机械参数。 基座是由一个长宽高(xyz)分别为0.4m, 0.2m, 0.1m的长方体和一个半径为0.05m的球体组合而成的。这里的长方体扮演者小车的车身，我们通过pose标签设定它离地0.1m。 球体只是用来支撑车身的，为了降低动力学分析的复杂度，我们认为它是光滑的，设定摩擦系数为0。上面的右图是Gazebo根据model.sdf文件渲染的DiffCart，图中红绿蓝三色箭头分别表示xyz三个坐标轴。

3. DiffCart的控制插件

我们在demos子目录下新建一个目录diffcart， 并在其中创建了diffcart_plugin.cc用于实现DiffCart的控制插件， 通过CMake文件指引编译。下表中列举了插件类DiffCartPlugin中定义的成员变量。 这些成员变量全部在Load函数中完成初始化工作。

        public: virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
            mWheelDiff = 0.26;
            mWheelRadius = 0.1;
            mLeftWheelCmd = 0;
            mRightWheelCmd = 0;

            mWorld = model->GetWorld();
            mModel = model;
            mLeftWheel = model->GetJoint("diffcart::base_2_left_wheel");
            mRightWheel = model->GetJoint("diffcart::base_2_right_wheel");

            // ROS 配置
            this->mRosNode.reset(new ros::NodeHandle("~"));
            mRosCmdVelSub = mRosNode->subscribe("/cmd_vel", 100,
                    &DiffCartPlugin::OnCmdVelFromRos, this);

            mUpdateEndCnt = event::Events::ConnectWorldUpdateEnd(
                    boost::bind(&DiffCartPlugin::OnWorldUpdateEnd, this));
        }

右侧是Load函数的实现，它有两个输入参数，其中model是将要控制的DiffCart对象，sdf则是SDF模型文件解析器对象。

在函数的一开始，我们先设定了轮间距和轮半径分别为0.26m和0.1m， 这种参数的硬编码方式不是很好，以后会考虑使用配置文件来替代它。同时还为左右轮的速度指令赋予了初值0。

接着，通过Gazebo的API获取了仿真世界和模型对象，分别用mWorld和mModel记录。同时，还根据SDF文件中为Joint定义的名称获取了左右轮对象。 以后我们将通过这两个对象实现机器人的运动控制和里程计反馈。

然后，我们构建了ROS的节点句柄，并以传统的方式订阅了ROS主题"/cmd_vel"。 这里我们并没有像Velodyne的仿真插件那样，专门在一个线程中处理ROS的回调任务。 主要还是希望尽可能的减少线程的数量，降低系统的并发特性，减少BUG出现的几率。

最后，我们注册了仿真更新结束事件的回调函数OnWorldUpdateEnd。下面是该函数的实现，目前只有一句话，用于处理ROS的回调任务。以后我们将在这里添加机器人状态的更新任务。

        private: void OnWorldUpdateEnd() {
            ros::spinOnce();
        }

我们订阅的ROS主题"/cmd_vel"的消息类型是"geometry_msgs/Twist"，使用了6个float64的字段分别描述了机器人在xyz三轴上的线速度和绕这三个轴转动的角速度。 我们可以通过指令$ rosmsg show geometry_msgs/Twist来具体的查看消息内容。根据差速小车的运动学模型， 我们在ROS消息的回调函数OnCmdVelFromRos中，将机器人的线速度和角速度转换为左右电机的转速，并控制机器人运动。其函数实现如下面代码所示。

        private: void OnCmdVelFromRos(const geometry_msgs::TwistConstPtr & msg)
        {
            mLeftWheelCmd = msg->linear.x - msg->angular.z * mWheelDiff * 0.5;
            mRightWheelCmd = msg->linear.x + msg->angular.z * mWheelDiff * 0.5;

            mLeftWheel->SetVelocity(0, mLeftWheelCmd / mWheelRadius);
            mRightWheel->SetVelocity(0, mRightWheelCmd / mWheelRadius);
        }

4. 运行脚本

现在，我们还需要一个launch文件和一个bash脚本来运行gazebo并加载仿真世界和插件。 在Velodyne的仿真插件中，我们曾出于统一Gazebo和ROS仿真时间的目的，提供了一组脚本方案。 但这个脚本方案还存在一些问题需要改进：

• 在运行仿真demo之前，需要先source在gazebo_demos根目录下的setup.sh脚本，添加一些环境变量。这个过程很繁琐，一不注意就忘了。 我们完全可以在运行gazebo的脚本文件中添加这些环境变量，从而省掉这个过程。
• 在原先运行gazebo的脚本文件中，我们把需要加载的世界描述文件硬编码在里面了。这样改动起来就比较麻烦，当需要更换世界文件的时候， 我们还需要修改gazebo的脚本文件，或者重新提供一个。我们可以在调用gazebo脚本文件的时候把世界文件以运行参数的形式传递过去， 这样就不需要过多的修改gazebo脚本文件了。

出于以上两点考虑，我们提供了一个脚本文件"bringup_gazebo.sh"用于添加环境变量，运行gazebo加载仿真世界文件。文件内容如下面的代码片段所示， 在第2、3行中获取脚本"bringup_gazebo.sh"的目录，用变量SCRIPT_DIR记录。接着在第5、6行中，获取gazebo_demos的根目录以及模型文件的路径， 添加环境变量GAZEBO_MODEL_PATH供Gazebo查找DiffCart模型文件。最后的8到11行用于检查输入的运行参数${1}，如果${1}中的内容是一个文件的话就运行gazebo并加载该文件。

        #!/bin/sh
        [ -L ${0} ] && SCRIPT_DIR=$(readlink ${0}) || SCRIPT_DIR=${0}
        SCRIPT_DIR=$(dirname ${SCRIPT_DIR})
        
        export GAZEBO_DEMO_PATH=$(dirname ${SCRIPT_DIR})
        export GAZEBO_MODEL_PATH=${GAZEBO_DEMO_PATH}/models:$GAZEBO_MODEL_PATH
        
        if [ -f ${1} ]
        then
            gazebo ${1} -s `catkin_find --first-only libgazebo_demos_ros_plugin.so`
        fi

相应的，我们在launch子目录下添加"diffcart.launch"文件， 在该文件中，我们以如下的形式运行bringup_gazebo.sh脚本。其中args中记录的就是运行脚本时的输入参数，我们直接将需要加载的世界文件填写在这里。

        <node name="gazebo" pkg="gazebo_demos" type="bringup_gazebo.sh"
            args="$(find gazebo_demos)/worlds/diffcart.world" output="screen"/>

如果一切顺利，我们就可以直接通过如下语句运行demo了。

        $ roslaunch gazebo_demos diffcart.launch

为了方便控制DiffCart在仿真世界中运动，我们添加了diffcart_keyteleop.launch文件， 将turtlebot的键盘遥控接到了DiffCart上，这样就可以通过键盘控制DiffCart了。

        $ roslaunch gazebo_demos diffcart_keyteleop.launch

5. 完

本文中，我们根据官方教程创建了一个差速驱动的小车DiffCart，并为之编写了一个控制插件通过订阅ROS消息接收控制指令。而且，我们还将turtlebot的键盘遥控器与DiffCart对接上了， 可以方便的控制机器人运动。此外，我们对运行demo的脚本进行了优化可以一键运行demo了。