z

 

#miss part
    begin_last = 0.0
    k=0.1
    timer = threading.Timer(2, flag_true)
    if(time_last!=0.0):
        time_now = datetime.datetime.now()
        k = (time_now - time_last).total_seconds()

    speed_d = 0.7 * 10 * k
    speed_h = 3.5 * 2 * k

    time_last = datetime.datetime.now()
    #rospy.logwarn("=============")
    #rospy.logwarn(flag)
    if(flag):
        if(i==0):
            last_x = round(data.position.x, 4)
            last_y = round(data.position.y, 4)
            last_z = round(data.position.z, 4)
            last_pose = data
        else:
            p_x = round(data.position.x, 4)
            p_y = round(data.position.y, 4)
            p_z = round(data.position.z, 4)

            if (init_x == p_x and init_x == p_y and init_x == p_z):
                # flag = False  1，2，3，4， 10（失位）  10 11(位置继续变化)，4（校正回来，上一个是11，如果当前校正完成就不在进行反复校正），5，6 继续检测
                rospy.logwarn("init position success")
                return
            elif(last_x == p_x and last_y == p_y and last_z == p_z):
                #rospy.logwarn("stop")
                #flag = False
                return
            else:
                differ_d = math.sqrt(math.pow(last_x-p_x, 2)+math.pow(last_y-p_y, 2))
                differ_h = abs(last_z-p_z)

                if(differ_d>speed_d or differ_h>speed_h ): #miss and initpose
                    rospy.logwarn("change line:%f,z:%f", differ_d, differ_h)
                    #先报miss,通知停止导航
                    pub_miss.publish(String("miss"))

                    if(num==0):
                        begin = datetime.datetime.now()
                    end = datetime.datetime.now()
                    #如果一分钟之内校正三次，上报异常
                    if(num<3):
                        num=num+1
                    else: #每三次一判断
                        k2 = end - begin
                        num = 0
                        if (k2.total_seconds() < 60):
                            #rospy.logwarn("begin :%f , end :%f", begin.total_seconds(), end.total_seconds())
                            rospy.logwarn("Loss of position 3 times in 60 seconds ")
                            flag = False
                            return

                    # 10秒内不能校正两次
                    if (begin_last != 0.0):
                        k2 = end - begin_last
                        if (k2.total_seconds() < 10):
                            rospy.logwarn("Loss of position 2 times in 10 seconds")
                            flag = False
                            return
                    
                    init_x = last_x
                    init_y = last_y
                    init_z = last_z
                    rospy.logwarn("miss current position(x:%f,y:%f,z:%f)", p_x, p_y, p_z)
                    rospy.logwarn("miss times %f",num)
                    
                    #导航停止会有延迟，设置延迟0.5s,停止后在校正
                    #time.sleep(0.5)
                    #开始校正
                    pub_initpose.publish(msg_construct(last_pose))
                    flag = False
                    timer.start()
                    #rospy.logwarn(flag)
                    begin_last = datetime.datetime.now()
                else:#recode last data
                    pub_miss.publish(String("right"))
                    #rospy.logwarn("right")
                    last_x = p_x
                    last_y = p_y
                    last_z = p_z
                    last_pose = data

                #rospy.logwarn("current position(x:%f,y:%f,z:%f)", p_x, p_y, p_z)
            #rospy.logwarn("last position(x:%f,y:%f,z:%f)", last_x, last_y, last_z)

        i=i+1
    #rospy.logwarn("current position(x:%f,y:%f,z:%f)",round(data.position.x, 4),round(data.position.y, 4), round(data.position.z, 4))