（二）从零搭建unity3d机器人仿真:驱动turtlebot轮子运动

1.加载turtebot模型

在unity3d中加载turtlebot模型，注意不要直接拷贝Robotics-Nav2-SLAM-Example中的turtlebot中的文件夹，会污染工程导致使用urdf import 导入后的模型文件为红色。
从https://github.com/ROBOTIS-GIT/turtlebot3开源代码中重新下载文件夹好加入到unity3d工程文件夹下。

2.加入平面使机器人能在平面上运动

如果不加平面启动play后机器人会掉下去。右键Add->3Dobject->Plan加入平面后设置transform位置为（0，0，0）。点击Play，发现不掉下去了.

3.配置摄像头

play后发现很模糊 解决方法：选择主相机，勾选上physical camera就变清晰了。

4.加入脚本控制轮子运动

Unity3D中配置好轮子后可通过 WheelCollider、ArticulationBody 、 Rigidbody三种中的一种控制轮子，可以通过键盘按键控制机器人运动。
使用urdf导入默认使用ArticulationBody以下实现运动控制。

新建一个名字为AGVController.cs的文件，代码如下：

using UnityEngine;
// using Unity.Robotics.ROSTCPConnector;
//using RosMessageTypes.Geometry;
using Unity.Robotics.UrdfImporter.Control;

namespace RosSharp.Control
{
    public enum ControlMode { Keyboard, ROS};

    public class AGVController : MonoBehaviour
    {
        public GameObject wheel1;
        public GameObject wheel2;
        public ControlMode mode = ControlMode.ROS;

        private ArticulationBody wA1;
        private ArticulationBody wA2;

        public float maxLinearSpeed = 2; //  m/s
        public float maxRotationalSpeed = 1;//
        public float wheelRadius = 0.033f; //meters
        public float trackWidth = 0.288f; // meters Distance between tyres
        public float forceLimit = 10;
        public float damping = 10;

        // public float ROSTimeout = 0.5f;
       // private float lastCmdReceived = 0f;

        //ROSConnection ros;
        private RotationDirection direction;
        // private float rosLinear = 0f;
        // private float rosAngular = 0f;

        void Start()
        {
            wA1 = wheel1.GetComponent<ArticulationBody>();
            wA2 = wheel2.GetComponent<ArticulationBody>();
            SetParameters(wA1);
            SetParameters(wA2);
            // ros = ROSConnection.GetOrCreateInstance();
            // ros.Subscribe<TwistMsg>("cmd_vel", ReceiveROSCmd);
        }

        // void ReceiveROSCmd(TwistMsg cmdVel)
        // {
        //     rosLinear = (float)cmdVel.linear.x;
        //     rosAngular = (float)cmdVel.angular.z;
        //     lastCmdReceived = Time.time;
        // }

        void FixedUpdate()
        {
            if (mode == ControlMode.Keyboard)
            {
                KeyBoardUpdate();
            }
            // else if (mode == ControlMode.ROS)
            // {
            //     ROSUpdate();
            // }     
        }

        private void SetParameters(ArticulationBody joint)
        {
            ArticulationDrive drive = joint.xDrive;
            drive.forceLimit = forceLimit;
            drive.damping = damping;
            joint.xDrive = drive;
        }

        private void SetSpeed(ArticulationBody joint, float wheelSpeed = float.NaN)
        {
            ArticulationDrive drive = joint.xDrive;
            if (float.IsNaN(wheelSpeed))
            {
                drive.targetVelocity = ((2 * maxLinearSpeed) / wheelRadius) * Mathf.Rad2Deg * (int)direction;
            }
            else
            {
                drive.targetVelocity = wheelSpeed;
            }
            joint.xDrive = drive;
        }

        private void KeyBoardUpdate()
        {
            float moveDirection = Input.GetAxis("Vertical");
            float inputSpeed;
            float inputRotationSpeed;
            if (moveDirection > 0)
            {
                inputSpeed = maxLinearSpeed;
            }
            else if (moveDirection < 0)
            {
                inputSpeed = maxLinearSpeed * -1;
            }
            else
            {
                inputSpeed = 0;
            }

            float turnDirction = Input.GetAxis("Horizontal");
            if (turnDirction > 0)
            {
                inputRotationSpeed = maxRotationalSpeed;
            }
            else if (turnDirction < 0)
            {
                inputRotationSpeed = maxRotationalSpeed * -1;
            }
            else
            {
                inputRotationSpeed = 0;
            }
            RobotInput(inputSpeed, inputRotationSpeed);
        }


        // private void ROSUpdate()
        // {
        //     if (Time.time - lastCmdReceived > ROSTimeout)
        //     {
        //         rosLinear = 0f;
        //         rosAngular = 0f;
        //     }
        //     RobotInput(rosLinear, -rosAngular);
        // }

        private void RobotInput(float speed, float rotSpeed) // m/s and rad/s
        {
            if (speed > maxLinearSpeed)
            {
                speed = maxLinearSpeed;
            }
            if (rotSpeed > maxRotationalSpeed)
            {
                rotSpeed = maxRotationalSpeed;
            }
            float wheel1Rotation = (speed / wheelRadius);
            float wheel2Rotation = wheel1Rotation;
            float wheelSpeedDiff = ((rotSpeed * trackWidth) / wheelRadius);
            if (rotSpeed != 0)
            {
                wheel1Rotation = (wheel1Rotation + (wheelSpeedDiff / 1)) * Mathf.Rad2Deg;
                wheel2Rotation = (wheel2Rotation - (wheelSpeedDiff / 1)) * Mathf.Rad2Deg;
            }
            else
            {
                wheel1Rotation *= Mathf.Rad2Deg;
                wheel2Rotation *= Mathf.Rad2Deg;
            }
            SetSpeed(wA1, wheel1Rotation);
            SetSpeed(wA2, wheel2Rotation);
        }
    }
}

（1）在模型通过Add component加入以上脚本。然后设置wheel_right_link为wheel2。
play运行发现机器人直接翻过来了，重新修改加入重力就能用按键向前了。

（2）问题，启动后发现机器人向上走。
原因为加载的机器人base_footprint没有固定。
加入Urdf Joint Fixed脚本，并且勾上Is Base Link，并且加入Articulation body。
（3）这个代码是注释掉了ros相关代码。
可以发现最后通过得到joint（这里是轮子的joint），然后设置ArticulationDrive类的targetVelocity来设置速度，从而驱动轮子运动。

  private void SetSpeed(ArticulationBody joint, float wheelSpeed = float.NaN)
        {
            ArticulationDrive drive = joint.xDrive;
            if (float.IsNaN(wheelSpeed))
            {
                drive.targetVelocity = ((2 * maxLinearSpeed) / wheelRadius) * Mathf.Rad2Deg * (int)direction;
            }
            else
            {
                drive.targetVelocity = wheelSpeed;
            }
            joint.xDrive = drive;
        }

其他
（1）相机加入渲染脚本。
将UniversalAdditionalCameraData脚本加入到摄像机。
步骤1：打开Package Manager
在Unity编辑器中，点击顶部菜单栏的 Window。
选择 Package Manager。
步骤2：安装Universal RP包
在Package Manager窗口中，确保左上角的下拉菜单选择的是 Unity Registry。
在列表中找到 Universal RP（通常你可以使用搜索框输入“Universal RP”来快速查找）。
点击Universal RP包，然后在右下角点击 Install 按钮。等待安装完成。