• 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  123410(失位)  10 11(位置继续变化),4(校正回来,上一个是11,如果当前校正完成就不在进行反复校正),56 继续检测
                    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))
  • 相关阅读:
    记长连接压测总结
    PHP装扩展
    LMbench安装&使用
    Scala学习笔记-2-(if、while、for、try、match)
    Gatling学习笔记-Scenario(场景)
    Java之路---Day05
    Java之路---Day04
    Java之路---Day03
    Java之路---Day02
    Java之路---Day01
  • 原文地址:https://www.cnblogs.com/sea-stream/p/10792013.html
Copyright © 2020-2023  润新知