-
Notifications
You must be signed in to change notification settings - Fork 7
/
Copy pathsystem_logger_node.py
executable file
·134 lines (112 loc) · 4.05 KB
/
system_logger_node.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
#!/usr/bin/env python
#encoding=utf-8
import rospy
from galileo_serial_server.msg import GalileoStatus
from xiaoqiang_log.msg import LogRecord
import time
import json
previous_status = None
log_pub = None
work_stamp_start = 0
pause_stamp_start = 0
test = GalileoStatus()
test.header.stamp.to_nsec() / 1000 / 1000
task_record = {
"events": [],
"results": ""
}
def galileo_status_logger(status):
global previous_status, work_stamp_start, pause_stamp_start, log_pub, task_record
if previous_status is None:
previous_status = status
return
# 从free状态到work状态
if previous_status.targetStatus == 0 and status.targetStatus == 1:
work_stamp_start = status.header.stamp.to_nsec() / 1000 / 1000 # ms单位时间戳
current_stamp = status.header.stamp.to_nsec() / 1000 / 1000
# 从work状态到free状态,正常完成后targetNumID不为,如果取消任务则为-1
if previous_status.targetStatus == 1 and status.targetStatus == 0 and status.targetNumID != -1:
task_record["events"].append({
"type": "WORKING",
"duration": current_stamp - work_stamp_start
})
task_record["results"] = "SUCCESS"
# pub log
log_record = LogRecord()
log_record.stamp = status.header.stamp
log_record.collection_name = "task"
log_record.record = json.dumps(task_record)
log_pub.publish(log_record)
# reset data
work_stamp_start = 0
pause_stamp_start = 0
task_record = {
"events": [],
"results": ""
}
rospy.loginfo("从work状态到free状态")
# 从working状态到pause状态
if previous_status.targetStatus == 1 and status.targetStatus == 2:
task_record["events"].append({
"type": "WORKING",
"duration": current_stamp - work_stamp_start,
})
pause_stamp_start = current_stamp
work_stamp_start = 0
# 从pause状态到working状态
if previous_status.targetStatus == 2 and status.targetStatus == 1:
task_record["events"].append({
"type": "PAUSED",
"duration": current_stamp - pause_stamp_start
})
work_stamp_start = current_stamp
pause_stamp_start = 0
# 从working状态到cancel状态
if previous_status.targetStatus == 1 and status.targetStatus == 0 and status.targetNumID == -1:
task_record["events"].append({
"type": "WORKING",
"duration": current_stamp - work_stamp_start,
})
task_record["results"] = "CANCEL"
# pub log
log_record = LogRecord()
log_record.stamp = status.header.stamp
log_record.collection_name = "task"
log_record.record = json.dumps(task_record)
log_pub.publish(log_record)
# reset data
work_stamp_start = 0
pause_stamp_start = 0
task_record = {
"events": [],
"results": ""
}
rospy.loginfo("从working状态到cancel状态")
# 从pause状态到cancel状态
if previous_status.targetStatus == 2 and status.targetStatus == 0:
task_record["events"].append({
"type": "PAUSED",
"duration": current_stamp - pause_stamp_start
})
task_record["results"] = "CANCEL"
# pub log
log_record = LogRecord()
log_record.stamp = status.header.stamp
log_record.collection_name = "task"
log_record.record = json.dumps(task_record)
log_pub.publish(log_record)
# reset data
work_stamp_start = 0
pause_stamp_start = 0
task_record = {
"events": [],
"results": ""
}
rospy.loginfo("从pause状态到cancel状态")
previous_status = status
if __name__ == "__main__":
rospy.init_node("system_logger")
sub = rospy.Subscriber("/galileo/status", GalileoStatus, galileo_status_logger, queue_size=10)
log_pub = rospy.Publisher("/xiaoqiang_log", LogRecord, queue_size=10)
while not rospy.is_shutdown():
time.sleep(1)