一、机器人回充功能涉及到的知识

（1）TF坐标变换及数据处理

在回充导航工作中，需要涉及到多个坐标之间的转化，坐标与坐标之间的距离计算加速度计算。

现有一移动式机器人底盘，在底盘上安装了一雷达，雷达相对于底盘的偏移量已知，现雷达检测到一障碍物信息，获取到坐标分别为(x,y,z)，该坐标是以雷达为参考系的，如何将这个坐标转换成以小车为参考系的坐标呢？

        现在已知雷达（base_laser）相对于障碍物的距离，通过tf坐标变换可以转换为障碍物跟小车之间的距离。 

        其中所涉及到的TF转换中常用到的数据类型为以下这几种，查看rosmsg信息可以看到数据类型里面包含了什么信息方便我们获取和调用。

geometry_msg::TransfromStamped
geometry_msg::poseStamped
geometry_msg::pointStamped

        三个消息类型都包含有时间戳信息和"string frame_id"这个是参考坐标系，一般会根据实际情况选择坐标系，例如odom，base_link等。a数据类型描述了描述坐标系之间的变换关系，其中"string child_frame_id"是指被描述的坐标系，比如从将雷达的坐标系转换到base_link坐标系下。translation描述的是XYZ轴方向上64位的浮点数用于表示偏移量，rotation是描述欧拉角的，具体参数由四元数提供。

        个人理解：a数据类型里更多使用在对TF进行坐标系与坐标系之间的变换中，而b数据类型里通常能保存一个坐标点的位姿信息，当需要这个点的坐标信息和位姿信息的时候，a数据类型里不包含目标点的坐标信息，就像视觉识别停车点的时候，需要获得这个停车点在世界坐标下的坐标和位姿信息，停车点一般有yaw角的数据，所以需要提取b数据类型里面坐标点的信息和转换四元数信息以获得yaw信息。

在实际工作中还涉及到许多关于保存位姿信息的数据类型，一般是矩阵或者向量的形式。

（1）Eigen::Vector2f /Eigen::Vector3f +(向量名字)：一般用来存放二维点坐标和带有偏航角的三维向量。

（2）tf::Quaternion +（名字）：创建一个存放四元数的容器。

（3）tf::Matrix3x3（名字）:创建一个3x3的矩阵，一般与（2）一起使用，用于存放四元数提取出的欧拉角（roll、pitch、yaw）。

（4）geometry_msgs::TransformStamped：创建一个带有时间戳的坐标系转换关系，用于描述坐标系之间的变换关系。

        还有一些用来处理矩阵的数据类型，例如tf::getYaw(传入一个四元数的值)，可以把四元数的值转成yaw角并且保存到一个double类型的数据中，或者使用三维向量进行保存，用来描述平面坐标系下角度的偏航角。

        像tf::Quaternion这样的数据类型的还有其他的方法可以帮助我们处理数据，例如：.setRPY()和.convert()等操作，以此来达到计算相对应参数的目的。

（2）回充功能的创建

        在实际的工程中，回充功能往往以线程的形式被创建，若该工程文件是标准的C++文件的格式，则在.h文件中需要包含各种需要用到的头文件。

        在private内声明变量，将ROS的句柄（ros::NodeHanle nh_）、发布者(ros::Publisher)和订阅者(ros::Subscriber)都写在这个地方，除此之外，ROS中的service类型(ros::ServiceServer)和需要加载的参数都要在这里定义。

        pubilc下声明需要用到的函数，其中包括回调函数，定义线程，发布速度和发布点云的函数。

        在.cpp文件下，使用namespace + 名称(与.h文件下的namespace相同)。先声明一个默认函数，在默认函数中，将.h文件中定义的相关参数进行初始化。文件中主要初始化了一个接口/start_recharge，用来调用回充的线程。使用在.h文件中的声明的句柄nh_发布一个服务，并将指针指向一个发布回充状态的服务，以此来标记当前是否为回充状态。

         创建一个自动回充的线程，指针指向的是一个返回值void的"auto_charge_thread"函数，该函数使用一个循环来判断"/start_recharge"发布的回充或是下桩信息，以此来决定使用不同的步骤函数。

autoChargeThread = new boost::thread(&Recharge::auto_charge_thread, this);

        在该线程中，回充的主要运动控制函数是"MoveToTarget()",该函数中将回充的每个步骤都单独写成一个函数，这样调用起来更加方便，而且可以调用不同的运动顺序，满足各种场景。

（3）回充功能关键—如何识别停车点 

        使用激光雷达实现回充功能需要利用激光点云信息，对点云信息进行过滤和判断。使用一个函数来对激光点云来进行处理，一般采用的是异形的充电桩在点云数据上的特殊性或使用反光条在激光雷达的作用下反射光的强度差异来判断是否为目标点。再使用回调函数去输出一个停车位的位姿信息。

        一般先使用Move_base导航到充电桩的附近，再进入回充的线程，因为考虑到激光雷达的扫描范围差异，可能存在一些死角或者盲区，所以需要使用常规的导航方式对机器人的位姿进行一个初步的运动，使得机器人的朝向可以顺利扫描到特征点。

        至于这一部分的算法实现，大部分都已经开源，需要知道这个函数需要什么参数，输入和输出的值，并且编写Rviz中的可视化信息，将提取出来的停车点位姿输出在Rviz中。

        个人总结：无论是采用激光雷达还是视觉，都需要一个能反映目标点位姿的信息例如：反光条、二维码或者一些比较特殊的外观，回充的步骤也是利用获取目标点的位姿等信息进行不断的调整，使得机器人的yaw角和目标位置的yaw角不断重合，通过使用一个accelerate的变量来控制机器人的速度，使得机器人离目标点较远时速度较快，邻近目标点时速度逐渐降低，以实现机器人的速度动态控制。

（4）回充动作的分解

        下图为机器人回充简图，双划线名称是回充动作组的成员，可以根据具体情况增加或调整相对应的步骤。

Step1. 首先使用一个回调函数获取机器人当前位姿，一般该数据由"/odom"话题保存格式为nav_msgs::Odometry数据类型的消息，可以通过一个回调函数将机器人当前位姿获取出来。

        然后再获取目标点的位姿信息，与获取机器人位姿方法相同。当获取了机器人位姿和目标点位姿之后，就可以编写一个函数，将机器人的朝向转向目标点的位置。这里需要对机器人的角度和目标点的角度进行处理（原本的角度范围是在-PI到PI之间需要转化到0-2PI之间）。再计算两个角度的差值来决定选择顺逆时针旋转到目标点。

        两个点的差值和中间值得出一个加速度，使机器人在相差角度较大的时候会以较快的角速度进行旋转，靠近目标点时速度下降，使得机器人最终停在一个合理的范围内。

Step2. 旋转到目标点后需要移动到目标位置，通过计算两点之间的距离，引入一个较长的时间作为判断是由超时的阈值，若时间内仍未返回值则报错。与上述使用"accelerate"变量一致，用来控制机器人的线速度。当机器人移动到目标点之后进入下一步骤。

Step3. 将机器人的yaw角与停车点的yaw角转到同一方向。

该步骤与上面旋转到目标朝向的核心思想基本相同，先处理机器人与目标点的yaw角度，使他们都处于(0~2PI)中。再将这两个角度进行比较判断顺逆时针旋转，再计算需要旋转的角度，加上"accelerate"的变量来控制旋转速度即可。

Step4. 机器人以低速靠近充电桩

        经过上述的运动之后，机器人的充电触点与充电桩的触电已经在可顺利对接的角度了，此时只需要让机器人匀速的靠近目标点，通过接受电池回调函数改变充电状态标志位来检测是否需要停车。其中可以引入停车距离以此来判断是否处于可进行上桩操作，计算两个位置之间的距离的函数可以在外面写好，这里直接调用就可以。当所有的步骤完成之后，该线程将会跳出，意味着回充功能顺利完成。

个人总结：机器人回充是一个已经很成熟的技术，使用的传感器也很广泛，面对不同的使用场景可以选择不同的回充解决方案，但是大致的回充实现流程都是如此，通过学习该回充的功能，以此来加深ROS的相关知识。