#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))