第七天：越疆小车的建图和导航，修改相关参数使得建图无重影无空缺更完整，使用python代码实现自动导航功能

这里的gmapping算法只能理解，不能深入学习算法本身的知识，感兴趣或专业可以深入学习，因为它已经写好了launch可直接调用文件完成建图工作。

HectorSLAM 是一种结合鲁棒性好的扫描匹配方法 2D SLAM 采用惯性传感系统的导航技术。 优点：不依赖里程计，适应无人机和地面不平坦区域 缺点：传感器性能要求很高，激光雷达必须是高频(40HZ 以上)并测量噪声很小的噪声

Gmapping 是基于激光的 SLAM 它已经集成了算法 ROS 在移动机器人中人中使用最多的SLAM 算法。这个算法是由的 Grisetti 等人提出的基础 Rao-Blackwellized 的粒子滤波 SLAM 方法。 优点：激光雷达频率要求低，鲁棒性高，能有效利用车轮里程计信息，速度快，计算量小 缺点：严重依赖里程计，无法适应无人机和地面不平坦区域，无回环(激光) SLAM 很难做回环检测)，大场景，粒子多，特别消耗资源。

KartoSLAM 以图优化为基础，采用高度优化和非迭代的方法 cholesky 矩阵解耦稀疏系统。图优化方法使用图的平均值表示地图，每个节点表示机器人轨迹的位置点和传感器测量数据集，箭头的连接表示连续机器人位置点的运动，并根据空间中节点箭头的约束计算和更新地图。 优点:对传感器要求低，相对 gmapping 算法，构建具有回环检测功能的大场景地图 缺点:计算量大，内存需求大，稳定性不够。构建大地图时，算法程序可能会死亡

cartographer 是 Google 基于图纸优化的实时室内建图项目 SLAM 该方法可以生分辨率 5cm 的 2D 格网地图。每一帧 laser scan 数据，利用 scan match 将子图插入最佳估计位置（submap）中，且 scan matching 只跟当前 submap 有关。在生成一个 submap 之后，将进行局部回环（loop close），利用分支定位和预先计算的网格，所有 submap 完成后，将进行全局回环，相对 KartoSLAM，Cartographer 采用优化库的是 google 的 ceres 构建 problem 优化。 优点：适用于大场景地图，可进行闭环检测，稳定性好 缺点：计算量大，内存需求大

<launch>  <arg name="scan_topic" default="scan" />//指定雷达消息主题为/scan
 <arg name="base_frame" default="base_footprint"/> //指定底盘坐标系
 <arg name="odom_frame" default="odom_combined"/> //指定里程计坐标系
 <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" respawn="true" >
 <param name="base_frame" value="$(arg base_frame)"/>
 <param name="odom_frame" value="$(arg odom_frame)"/>
 <param name="map_update_interval" value="1"/> //地图更新的时间间隔
 <param name="maxUrange" value="8.0"/> //雷达可用的最大有效测距值，
//一般情况下设置 maxUrange < 雷达的现实实际测距值 <= maxRange
 <param name="maxRange" value="10.0"/> //maxRange 是雷达的理论最大测距值
120
 <param name="sigma" value="0.05"/> //endpoint 匹配标准差
 <param name="kernelSize" value="3"/> //最多使用内核数
 <param name="lstep" value="0.05"/> //优化机器人移动的初始值（距离）
 <param name="astep" value="0.05"/> //优化 optimize 机器人移动的初始值（角度）
 <param name="iterations" value="5"/> //扫描匹配器的迭代次数
 <param name="lsigma" value="0.075"/> //用于计算的波束可能性的 sigma
 <param name="ogain" value="3.0"/> //在评估可能性时使用的增益，用于平滑重采样效果
 <param name="lskip" value="0"/> //为 0,表示所有的激光都处理
 <param name="minimumScore" value="30"/> //最小匹配得分，这个参数很重要，它决定了对激光的一个置信度，
//越高说明对激光匹配算法的要求越高，激光的匹配也越容易失败而
//转去使用里程计数据，而设的太低又会使地图中出现大量噪声，
//所以需要权衡调整
 <param name="srr" value="0.01"/> //运动模型的噪声参数
 <param name="srt" value="0.02"/> //运动模型的噪声参数
 <param name="str" value="0.01"/> //运动模型的噪声参数
 <param name="stt" value="0.02"/> //运动模型的噪声参数
 <param name="linearUpdate" value="0.05"/> //机器人移动 linearUpdate 距离，进行 scanmatch
 <param name="angularUpdate" value="0.0436"/> //机器人选装 angularUpdate 角度，进行 scanmatch 
 <param name="temporalUpdate" value="-1.0"/> //如果上次扫描处理的时间早于更新时间（秒），则处理扫描。
//小于零的值将关闭基于时间的更新
 <param name="resampleThreshold" value="0.5"/> //基于 Neff 的重采样阈值
 <param name="particles" value="50"/> //粒子个数,用于粒子滤波算法
 <param name="xmin" value="-1.0"/> //map 初始化的大小
 <param name="ymin" value="-1.0"/>
 <param name="xmax" value="1.0"/>
 <param name="ymax" value="1.0"/>
 <param name="delta" value="0.05"/> //地图的分辨率
 <param name="llsamplerange" value="0.01"/> //可能性的平移采样范围
 <param name="llsamplestep" value="0.01"/> //可能性的平移采样步骤
 <param name="lasamplerange" value="0.005"/> //角度采样范围的可能性
 <param name="lasamplestep" value="0.005"/> //角度采样步骤的可能性
 <remap from="scan" to="$(arg scan_topic)"/>
 </node>
</launch>

主要可以修改以下两个参数，优化我们的建图，至于其它参数的额详解可以自行网上搜索学习。 name=“minimumScore” value=“30”//最小匹配得分，这个参数很重要，它决定了对激光的一个置信度， name=“particles” value=“50” //粒子个数,用于粒子滤波算法

这里就不做详细解释了，直接上代码，具体可以参看相关注释。

先简单说一下这里的思路。
首先我们要明白要去哪些点，根据比赛要求，是15个快递的，然后是10个投递目标点，以及1个起始点。
所以需要确定的是26个点。但是这个点属实有点多，不可能一个个去测，所以首先可以确定10个投递点，
这10个点事一定要测试的，根据前面的博文，测得10个点以后，只需要修改程序中对应的数组中10个省份。
加上起始点，11个点是一定要测试的。其次是邮件的抓取点，由于要求是邮件基本均匀摆放，
根据场地和数量可以算出邮件中心之间间距15CM，所以这11个点我们只需要改变Y轴坐标，
进行累加，其它的值包括X轴，Z轴，以及另外四个位姿值不变。
但是这里可能会出现误差，所以我们导航到下一个点是，可能邮件并不在我们的镜头中，所以需要进行调整。
我的调整方案是，左右依次旋转一定角度，在导航误差不大的情况下，就可以正常找到邮件并识别，
当然我也提到是误差不大，误差大了相当与直接放弃抓取，去往下一个，由于赛题是对抗赛，所以放弃抓取未尝不可，
可以更快的进入下一个邮件的识别抓取。
其次，我这个方案还有一个比较大的问题就是有点浪费时间，我是左右分别等待了10~15S左右，
要等待镜头和识别稳定下来，所以万一这一次完全没识别失败了，那么可能就会浪费30s。
这里我也简单的做了一个假设方案，我们可以采取识别，使用颜色分割，上面提到邮件不显示完全，
就无法正常识别，往往我们可以在图像中找到对应的黄色区域（赛题中做了要求，背景为蓝色，邮件为黄色）
然后我们可以根据黄色区域在我们画面的位置，来控制机械臂往对应方向移动。
但是我们最终没有去实现，一个是比较麻烦，得去做图像这一块的处理，加上机械臂的运动范围有限，可能目的范围会超出机械臂的范围，
而且这个范围是不好确定的，具体的可以参考越疆机械臂具体资料。
其次是再进行图像处理可能又会增加硬件的压力，耗时会大大加大，毕竟是部署在单片机上，性能远不比电脑（文字识别用的最小模型，帧率上都比较低，具体后面说）
所以综上所述，还是采取左右固定移动的方式，希望建图和导航不会出现较大的误差！

正式上代码（我不列出全部代码，以防影响大家，有疑问可交流，主要介绍一下各个功能函数）

''' ------------------------------------------------------------------------------------------------- 功能：导航停止 参数：无 讲解： 导航结束停止或者出现异常停止 作者：瓜洲 ------------------------------------------------------------------------------------------------- '''
def shutdown():
    rospy.loginfo("Stopping the robot...")
    # Cancel any active goals
    move_base.cancel_goal()
    rospy.sleep(2)
    # Stop the robot
    cmd_vel_pub.publish(Twist())
    rospy.sleep(1)
''' ------------------------------------------------------------------------------------------------- 功能：读取导航点 参数：txt文件名 讲解： 将10个导航点存储在txt中，第一个是起始点，剩余十个对应邮递箱的点，其顺序和 代码数组中的10个省份对应，这样方便修改和写入，不改动代码 作者：瓜洲 ------------------------------------------------------------------------------------------------- '''
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
def read_goals(filename):
    # 定义导航点信息
    global nav_pointcnt
    quaternions = []		#坐标点
    Position = []			#位姿点
    data = []
    # 读入txt数据
    with open(filename) as f:
        for line in f.readlines():
            temp = line.split()
            data.append(temp)
    # 把数据解析存入目标点列表
    for n in data:
        x, y, z = float(n[0]), float(n[1]), float(n[2])
        x1, y1, z1, w = float(n[3]), float(n[4]), float(n[5]), float(n[6])
        data1 = Point(x, y, z)
        data2 = Quaternion(x1, y1, z1, w)
        Position.append(data1)
        quaternions.append(data2)
    nav_pointcnt = len(Position)
    for i in range(0, nav_pointcnt):
        waypoints.append(Pose(Position[i], quaternions[i])) #pose功能函数自带，使用即可
    print"read goals finish!!"
    return nav_pointcnt



''' ------------------------------------------------------------------------------------------------- 功能：导航移动 参数：目标点 讲解： 使用move_base.send_goal(goal)函数，实现导航功能，这里的自动导航已经用此函数进行封装了，所以我们直接使用就行 至于实现方法有兴趣可以找到源文件学习，这里不做讲解 作者：瓜洲 ------------------------------------------------------------------------------------------------- '''
def move(goal):
    # 发送移动目标点
    move_base.send_goal(goal)
    #等待超时，单位 秒
    finished_within_time = move_base.wait_for_result(rospy.Duration(70))
    # 如果超时取消本目标点的导航，进入下一步
    if not finished_within_time:
        move_base.cancel_goal()
        rospy.loginfo("Timed out achieving goal")
    else:
        # 否则导航完成，获取状态
        state = move_base.get_state()
        # 如果返回GoalStatus.SUCCEEDED，则导航成功
        if state == GoalStatus.SUCCEEDED:
            state = True
            rospy.loginfo("Goal succeeded!")
            return state


''' ------------------------------------------------------------------------------------------------- 功能：机械臂状态回调函数 参数：状态值 讲解： 本函数主要接收抓取结束时和投递结束时的状态，以此来判断是移动向投递箱还是分拣台 作者：瓜洲 ------------------------------------------------------------------------------------------------- '''
def dobot_statecallback(data):
    global dobot_grap_state, dobot_deliver_state, move_state, box_index
    try:
        dobot_grap_state = data.dobot_grap_flag
        dobot_deliver_state = data.dobot_deliver_flag
        box_index = data.deliver_box_index
        move_state = True
        #print("dobot_grap_state: ", dobot_grap_state,"deliver_state: ", dobot_deliver_state)
        print("box_index: ", box_index, "move_state:", move_state)
        return dobot_deliver_state, dobot_deliver_state, move_state, box_index
    except:
        print("receive dobot_state error")

''' ------------------------------------------------------------------------------------------------- 功能：导航主函数 参数：无 讲解： 只展示初始化和话题的接收和创建，只有后面的导航逻辑这里省略，主要是更加上面的回调函数判断过程，然后执行相应的操作 作者：瓜洲 ------------------------------------------------------------------------------------------------- '''
def main():
    global dobot_grap_state, dobot_deliver_state ,move_state, box_index, \
        nav_grap_state, nav_deliver_state, nav_pointcnt
    pointcnt = 0
    para_move = 0.13			#邮件移动的常数值
    rospy.init_node('send_goal', anonymous=False)
    rospy.on_shutdown(shutdown)
    rospy.loginfo("Waiting for move_base action server...")
    pub_nav_state = rospy.Publisher('/navigational_state', navigational_state, queue_size=10)  # 发布数据
    rospy.Subscriber('/dobot_state', dobot_state, dobot_statecallback)
    # 等待服务器
    move_base.wait_for_server(rospy.Duration(20))
    rospy.loginfo("Connected to move base server")
    nav_pointcnt = read_goals("/home/eaibot/moveit_ws/goals.txt")

    rate = rospy.Rate(10)

以下代码省略，主要就是调用上面的函数，判断小车的移动逻辑。

这一篇的核心在导航这一块，但是建图却是关键，图建不好就会严重影响导航的准确率，也就对全过程产生影响，所以一点要注意建图的方式方法。其次是检查小车的硬件，因为我遇到了小伙伴，他们建图的时候始终有缺陷，地图建出来严重变形，有比较大的发散，应该是激光雷达上翘了，最终连续厂家换了一台就好了。 其次就是虽然涉及到建图算法和导航算法，但是根据上面的内容，我们并没有去深究它，因为已经做了封装，直接拿来用就可以。当然要改进其性能，那肯定要熟读学习； 最后是可以参考我上面提到的一些问题，自己做一些改进，毕竟本人所学尚浅，还有很大的进步学习空间。下一章分享文字识别相关知识。