升级记录说明：
------------------------------------------------------------------------------------------------------------------------------------
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
程序版本：V0.4
更新日期：2024/07/08 

(1) 更新原先的控制算法，解决直线容易跑偏的问题
(2) 依据速度、曲率、铰链角度等参数自动更新预瞄距离，提高跟踪精度
(3) 解决故障码一直存在一直报，不存在就不报
(4) 增加里程计定位
(5) 曲线行驶横向路径偏差计算方式更新
(6) 带包时（如何判断带包），脱轨不能直接发0，速度逐步减半
(7) 渣包位太偏时，允许车辆转弯到渣包附近，然后报故障码提示切换成遥操
(8) 对包完成后，虽然锁钩状态为1（全开）或0（未知），只要对中超声波有合理的数据，左右摇一摇方向
(9) 渣包太偏或两次对包未完成，报故障码提示跳转到遥操等待状态，同时发布遥操topic "/remote_ctrl"
(10) 修复快到终点时，偶尔发速度为0的bug
(11) 修复RTK节点偶尔挂掉的现象
(12) 修复刚开始节点未运行，后续正常运行后，频繁报故障码的问题
(13) 每隔10min执行一次rosnode cleanup自动清理一次，解决节点挂了之后，还假装没挂的问题。然后故障模块可以快速提示节点死掉
     的故障码
(14) 增加所有ROS节点正常运行的监测
(15) 出接渣车间，RTK定位与SLAM偏差太大时，短时间(200ms)靠odo定位
(16) 为了避免暂停距离太长，收到暂停后，减速度线性递增，若长时间未暂停，怠速以上的速度迅速拉入怠速，怠速以下的速度迅速拉为0
(17) 最后一段距离太短，倒数第二段的终点速度需要提前减速，距离太近，限制转向角
    （a）最后一段5.5m~10m，倒数第二段终点速度1.2m/s
     (b) 最后一段1.5m~2.5m，倒数第二段终点速度0.5m/s
     (c) 最后一段0.5m~1.5m，倒数第二段终点速度0.35m/s
     (d) 最后一段小于0.5m,倒数第二段终点速度0
(18) 从接渣口出来，最后一段距离，限制最大转向角
(19) 配置参数uncorrectedLatErrMax 0.15 -> 0.05,uncorrectedDiffHeading 1.5度 -> 1度
(20) 速度小于0.2m/s时，运控模块支腿太低不报故障
(21) 解决直线倒车和bezier倒车与对包的拼接，容易误报拼接
(22) 第三待命点附近，下发速度跳变
(23) 取包阶段，最后持续发一段速度，取放包，摇摆方向盘
(24) 减速阶段，修改为分阶段的线性减速
(25) 直角转弯，较长的直线段和转弯段分不同的跟踪方法（待测试）


------------------------------------------------------------------------------------------------------------------------------------
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
程序版本：V0.3
更新日期：2024/06/15

(1) 修复倒渣、巷道Bezier曲线、取放包等工况下，检测到支腿高度不对，自动抬支腿后未正常发0回写抬支腿
(2) 解决SLAM丢失后，车辆重新回到缓冷场，车辆的坐标丢失的问题
(3) 接渣点渣包位坐标更新
(4) 非自动模式下，除了定位节点外，其他节点没有正常启动，不报故障
(5) 日志保存时间由原来的1小时修改为24小时
(6) 将上传调度的坐标修改为保留小数点后三位(四舍五入)发布到topic
(7) 取包完成时，对中超声波的值不对，报异常给调度，暂停取包
(8) 下发速度和角度"/cmd/cmdvel"保留在小数点后三位
(9) 接渣巷道倒车最大速度调整为1.8m/s，前进最大速度调整为1.6m/s，取放包直线段最大速度调整为1m/s,转弯点前6.5m开始减速到0.45m/s
(10) 将取放包预瞄距离由3m调整为2.5m
(10) 增加故障发布节点，用于发布故障码(计划300个以上，已经完成100多个)和故障描述上传到相应的topic，同时车端保存相关故障日志，
    当前故障码存在误报和漏报的现象，待不断测试优化。
    故障码至少有7位数字组成，编码规则：
    (a) 个位、十位、百位：故障子码
    (b) 千位：故障等级【1：event(事件)、2：warning（警告或错误）、3：Alarm(限速)、4：Fault(刹车停)、5：Cirtical(急停)】
    (c) 万位、十万位：模块编码【4:定位、6:运动控制、7：调度任务、9：对包、10：局部路径规划、11：故障模块】
    (d) 百万位：车号
    故障码解释，如：6072030，6(百万位)：【6号车】、07(万和十万位)：【调度任务】、2(千位)：【warning(警告或错误)】、030(个十百位)：【故障子码】

    该版本故障码框架已经完成，主要包含：
    (a) 部分RTK定位和部分融合定位相关的故障码（待完善与测试优化）
    (b) 部分运动控制相关的故障码（待完善与测试优化）
    (c) 调度任务解析相关故障码(待完善与测试优化)
    (d) 对包相关故障码(待完善与测试优化)
    (e) 局部路径规划相关故障码(待完善与测试优化)

    额外增加以下故障相关逻辑,并报相关故障码
    (a) 发了速度不走，发角度不摆动，发升降支腿不动
    (b) 发支腿抬起后，长时间未抬起到位
    (c) 收到任务，但是属于非自动模式
    (d) 定位，调度任务解析、路径规划、运动控制等模块正常启动监测，后续可以考虑增加其他ROS节点的监测
------------------------------------------------------------------------------------------------------------------------------------
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
程序版本：V0.2
更新日期：2024/04/29

(1) 车上有渣包，支腿高度小于385mm，强制把支腿抬起，若无法抬起，报故障
(2) 长时间定位丢失，强制停车
(3) 当前处于暂停状态下，发继续，继续按照原先的继续执行，其他工况下发继续不理睬
------------------------------------------------------------------------------------------------------------------------------------
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
程序版本：V0.1
更新日期：2024/04/25

(1) 高速纠偏角度限制
(2) 对包距离带上正负号
(3) 对包预瞄距离由4m修改为3m