故障诊断模块的设计

最新推荐文章于 2024-07-25 00:36:27 发布

happymff

最新推荐文章于 2024-07-25 00:36:27 发布

阅读量121

点赞数 1

文章标签： linux 运维服务器

本文链接：https://blog.csdn.net/happymff/article/details/139737224

版权

如何做故障诊断模块的测试呢？

1.组装消息输入

2.进入消息topic的发布

3.查看诊断模块的输出

#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@文件        :control_topic.py
@说明        :
@时间        :2022/03/01 10:24:44
@作者        :mff
@版本        :1.0
'''

from diagnose_msgs.msg import ModuleStates, ModuleState
from diagnose_msgs.msg import ControlLimit
from diagnose_msgs.msg import SpeedLimit
from diagnose_msgs.msg import EmcLimit
from diagnose_msgs.msg import Float64Map, Int32Map, BoolMap

import rospy

#定时函数
def count_time(start_time, duration):
    duration1 = rospy.Time.now() - start_time
    if duration1 > duration:
        return True
    return False

def test_4801():
    topic_name = "/SY01/planning/diagnose_states"
    
    pub = rospy.Publisher(topic_name, ModuleStates, queue_size=1000)
    hz = rospy.Rate(10)
    
    msgs = ModuleStates()
    msg = ModuleState()
    msg.module_type="planning_module"
    msg.sub_module_type="sub_planning"
    msg.sub_module_id  =  "1"
    
    map_status = Int32Map()
    map_status.key = "map_load_status"
    map_status.value = -1

    start=rospy.Time.now()
    duration = rospy.Duration(3)
    while not rospy.is_shutdown():
        msg.int_states = []
        if count_time(start, duration):
            start = rospy.Time.now()
            if map_status.value == -1:
                map_status.value = 0
            else:
                map_status.value = -1
        msgs = ModuleStates()
        msg.int_states.append(map_status)
        msg.head.stamp = rospy.Time.now()
        msgs.states.append(msg)
        pub.publish(msgs)
        hz.sleep()

def test_4802():
    topic_name = "/SY01/planning/diagnose_states"
    pub = rospy.Publisher(topic_name, ModuleStates, queue_size=1000)
    hz = rospy.Rate(1)
    
    msgs = ModuleStates()
    msg = ModuleState()
    msg.module_type="planning_module"
    msg.sub_module_type="sub_planning"
    msg.sub_module_id  =  "1"
    

    map_status = Int32Map()
    map_status.key = "resolution_status"
    map_status.value = -1

    start=rospy.Time.now()
    duration = rospy.Duration(3)
    while not rospy.is_shutdown():
        msg.int_states = []
        if count_time(start, duration):
            start = rospy.Time.now()
            if map_status.value == -1:
                map_status.value = 0
            else:
                map_status.value = -1
        msgs = ModuleStates()
        msg.int_states.append(map_status)
        msg.head.stamp = rospy.Time.now()
        msgs.states.append(msg)
        pub.publish(msgs)
        hz.sleep()


if __name__ == "__main__":
    rospy.init_node("planning")
    test_4801()
    # test_4802()