coverage
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
coverage_monitor.py
[詳解]
1
#!/usr/bin/env python3
2
"""清掃ナビゲーションの入力、計画、速度指令とTFを診断情報へ集約する。"""
3
4
from
__future__
import
annotations
5
6
import
time
7
from
collections
import
deque
8
9
import
rclpy
10
from
diagnostic_msgs.msg
import
DiagnosticArray, DiagnosticStatus, KeyValue
11
from
geometry_msgs.msg
import
PolygonStamped, PoseWithCovarianceStamped, Twist
12
from
nav_msgs.msg
import
OccupancyGrid, Odometry, Path
13
from
rclpy.clock
import
Clock
14
from
rclpy.clock_type
import
ClockType
15
from
rclpy.node
import
Node
16
from
rclpy.qos
import
DurabilityPolicy, QoSProfile, ReliabilityPolicy
17
from
rclpy.time
import
Time
18
from
rosgraph_msgs.msg
import
Clock
as
ClockMessage
19
from
sensor_msgs.msg
import
Imu, LaserScan, PointCloud2
20
from
std_msgs.msg
import
String
21
from
tf2_ros
import
Buffer, TransformListener
22
23
24
# 最大30sの実時間サンプルからHzを求める。経路と速度指令はイベント型である。
25
TOPICS = {
26
'/cleaning_zone'
: (PolygonStamped,
None
),
27
'/map'
: (OccupancyGrid,
None
),
28
'/scan'
: (LaserScan, 2.0),
29
'/coverage_server/coverage_plan'
: (Path,
None
),
30
'/plan'
: (Path,
None
),
31
'/cmd_vel_nav'
: (Twist,
None
),
32
'/cmd_vel_smoothed'
: (Twist,
None
),
33
'/cmd_vel'
: (Twist,
None
),
34
}
35
THREE_D_TOPICS = {
36
'/scan_raw'
: (LaserScan, 2.0),
37
'/odom'
: (Odometry, 2.0),
38
'/imu/data_raw'
: (Imu, 2.0),
39
'/wheel/odometry'
: (Odometry, 2.0),
40
'/imu/data_fusion'
: (Imu, 2.0),
41
'/points'
: (PointCloud2, 2.0),
42
'/points_icp_filtered'
: (PointCloud2, 2.0),
43
'/points_assembled_icp'
: (PointCloud2, 15.0),
44
'/odometry/filtered'
: (Odometry, 2.0),
45
}
46
STAMP_TOPICS = {
'/scan'
,
'/scan_raw'
,
'/points'
,
'/points_icp_filtered'
,
47
'/points_assembled_icp'
,
'/odometry/filtered'
,
48
'/localization_pose'
,
'/odom'
,
'/imu/data_raw'
,
49
'/wheel/odometry'
,
'/imu/data_fusion'
}
50
SENSOR_STAGES = (
'scan'
,
'cloud'
,
'filtered'
,
'assembled'
,
'localization'
)
51
52
53
class
CoverageMonitor
(Node):
54
"""主要トピックの実時間Hz、最終受信からの秒数、TFを発行する。"""
55
56
def
__init__
(self) -> None:
57
super().
__init__
(
'coverage_monitor'
)
58
self.declare_parameter(
'profile'
,
'2d'
)
59
self.declare_parameter(
'zone_topic'
,
'/cleaning_zone'
)
60
self.declare_parameter(
'diagnostic_stage'
,
'scan'
)
61
profile = self.get_parameter(
'profile'
).value
62
self.
_sensor_profile
= profile ==
'sensor'
63
self.
_expect_clock
= profile
in
(
'2d'
,
'3d'
,
'sensor'
)
64
zone_topic = self.get_parameter(
'zone_topic'
).value
65
self.
_tf_target
=
'map'
if
profile !=
'sensor'
else
None
66
if
profile ==
'sensor'
:
67
stage = self.get_parameter(
'diagnostic_stage'
).value
68
if
stage
not
in
SENSOR_STAGES:
69
raise
ValueError(f
'Unknown diagnostic_stage: {stage}'
)
70
topics = {
'/scan_raw'
: THREE_D_TOPICS[
'/scan_raw'
],
71
'/scan'
: TOPICS[
'/scan'
]}
72
if
SENSOR_STAGES.index(stage) >= SENSOR_STAGES.index(
'cloud'
):
73
topics.update({
74
'/points'
: THREE_D_TOPICS[
'/points'
],
75
'/odom'
: THREE_D_TOPICS[
'/odom'
],
76
'/imu/data_raw'
: THREE_D_TOPICS[
'/imu/data_raw'
],
77
'/wheel/odometry'
: THREE_D_TOPICS[
'/wheel/odometry'
],
78
'/imu/data_fusion'
: THREE_D_TOPICS[
'/imu/data_fusion'
],
79
'/odometry/filtered'
: THREE_D_TOPICS[
'/odometry/filtered'
],
80
})
81
self.
_tf_target
=
'odom'
82
if
SENSOR_STAGES.index(stage) >= SENSOR_STAGES.index(
'filtered'
):
83
topics[
'/points_icp_filtered'
] = THREE_D_TOPICS[
'/points_icp_filtered'
]
84
if
SENSOR_STAGES.index(stage) >= SENSOR_STAGES.index(
'assembled'
):
85
topics[
'/points_assembled_icp'
] = THREE_D_TOPICS[
'/points_assembled_icp'
]
86
if
stage ==
'localization'
:
87
topics[
'/localization_pose'
] = (PoseWithCovarianceStamped, 15.0)
88
topics[
'/map'
] = (OccupancyGrid,
None
)
89
self.
_tf_target
=
'map'
90
else
:
91
topics = dict(TOPICS)
92
topics[zone_topic] = topics.pop(
'/cleaning_zone'
)
93
if
profile ==
'3d'
:
94
topics.update(THREE_D_TOPICS)
95
self.
_samples
= {topic: deque(maxlen=600)
for
topic
in
topics}
96
self.
_last_seen
= {topic:
None
for
topic
in
topics}
97
self.
_thresholds
= {topic: threshold
for
topic, (_, threshold)
in
topics.items()}
98
self.
_received_since_zone
= {topic: 0
for
topic
in
topics}
99
self.
_last_stamp_ns
= {topic:
None
for
topic
in
topics}
100
self.
_repeated_stamps
= {topic: 0
for
topic
in
topics}
101
self.
_backward_stamps
= {topic: 0
for
topic
in
topics}
102
self.
_last_stamp_problem
= {topic:
None
for
topic
in
topics}
103
self.
_last_point_count
= {topic:
None
for
topic
in
topics}
104
self.
_last_scan_timing
= {topic:
None
for
topic
in
topics}
105
self.
_zone_topic
= zone_topic
106
self.
_zone_received
=
False
107
self.
_phase
=
'IDLE'
108
self.
_clock_sample
: tuple[float, float] |
None
=
None
109
self.
_clock_last_seen
: float |
None
=
None
110
self.
_sim_rate
: float |
None
=
None
111
self.
_clock_backward_events
= 0
112
113
# BEST_EFFORT購読はRELIABLE/BEST_EFFORTの両publisherから受信できる。
114
data_qos = QoSProfile(
115
depth=1, reliability=ReliabilityPolicy.BEST_EFFORT,
116
durability=DurabilityPolicy.VOLATILE)
117
for
topic, (msg_type, _)
in
topics.items():
118
self.create_subscription(
119
msg_type, topic,
120
lambda
msg, name=topic: self.
_on_topic
(name, msg), data_qos)
121
phase_qos = QoSProfile(
122
depth=1, reliability=ReliabilityPolicy.RELIABLE,
123
durability=DurabilityPolicy.TRANSIENT_LOCAL)
124
self.create_subscription(
125
String,
'/coverage/phase'
, self.
_on_phase
, phase_qos)
126
if
self.
_expect_clock
:
127
self.create_subscription(
128
ClockMessage,
'/clock'
, self.
_on_clock
, data_qos)
129
self.
_tf_buffer
= Buffer()
130
self.
_tf_listener
= TransformListener(self.
_tf_buffer
, self)
131
self.
_diagnostics_pub
= self.create_publisher(
132
DiagnosticArray,
'/diagnostics'
, 10)
133
# Gazebo停止中も最終受信時刻を更新するため実時間1.0s周期を使う。
134
self.create_timer(
135
1.0, self.
_publish_diagnostics
,
136
clock=Clock(clock_type=ClockType.STEADY_TIME))
137
138
def
_on_topic
(self, topic: str, msg) ->
None
:
139
"""受信時刻、ヘッダー時刻の重複、点数を記録する。"""
140
now = time.monotonic()
141
self.
_samples
[topic].append(now)
142
self.
_last_seen
[topic] = now
143
if
topic
in
STAMP_TOPICS:
144
stamp = msg.header.stamp
145
stamp_ns = stamp.sec * 1_000_000_000 + stamp.nanosec
146
previous = self.
_last_stamp_ns
[topic]
147
if
previous
is
not
None
and
stamp_ns == previous:
148
self.
_repeated_stamps
[topic] += 1
149
self.
_last_stamp_problem
[topic] = now
150
elif
previous
is
not
None
and
stamp_ns < previous:
151
self.
_backward_stamps
[topic] += 1
152
self.
_last_stamp_problem
[topic] = now
153
self.
_last_stamp_ns
[topic] = stamp_ns
154
if
isinstance(msg, PointCloud2):
155
self.
_last_point_count
[topic] = msg.width * msg.height
156
elif
isinstance(msg, LaserScan):
157
self.
_last_scan_timing
[topic] = (msg.time_increment, msg.scan_time)
158
if
topic == self.
_zone_topic
:
159
self.
_zone_received
=
True
160
self.
_received_since_zone
= {
161
name: 0
for
name
in
self.
_received_since_zone
}
162
self.
_received_since_zone
[topic] += 1
163
164
def
_on_phase
(self, msg) -> None:
165
"""bridgeが発行した処理段階を保持する。"""
166
self.
_phase
= msg.data
167
168
def
_on_clock
(self, msg: ClockMessage) ->
None
:
169
"""シミュレーション時間と実時間の比を計算する。"""
170
now = time.monotonic()
171
self.
_clock_last_seen
= now
172
sim = msg.clock.sec + msg.clock.nanosec * 1e-9
173
if
self.
_clock_sample
is
not
None
:
174
previous_wall, previous_sim = self.
_clock_sample
175
wall_delta = now - previous_wall
176
sim_delta = sim - previous_sim
177
if
wall_delta >= 0.5
and
sim_delta >= 0:
178
self.
_sim_rate
= sim_delta / wall_delta
179
self.
_clock_sample
= (now, sim)
180
elif
sim_delta < 0:
181
self.
_sim_rate
=
None
182
self.
_clock_backward_events
+= 1
183
self.
_clock_sample
= (now, sim)
184
else
:
185
self.
_clock_sample
= (now, sim)
186
187
def
_topic_status
(self, topic: str, now: float) -> DiagnosticStatus:
188
"""一つのtopicについて実時間Hzと受信停止を表す。"""
189
samples = self.
_samples
[topic]
190
while
samples
and
now - samples[0] > 30.0:
191
samples.popleft()
192
last_seen = self.
_last_seen
[topic]
193
age = now - last_seen
if
last_seen
is
not
None
else
None
194
threshold = self.
_thresholds
[topic]
195
publishers = self.count_publishers(topic)
196
hz = (
197
(len(samples) - 1) / (samples[-1] - samples[0])
198
if
len(samples) >= 2
and
samples[-1] > samples[0]
else
0.0)
199
status = DiagnosticStatus()
200
status.name = f
'coverage/topic{topic}'
201
status.hardware_id =
'coverage'
202
if
publishers == 0:
203
status.level = DiagnosticStatus.WARN
204
status.message =
'NO_PUBLISHER'
205
elif
topic
in
STAMP_TOPICS
and
publishers > 1:
206
status.level = DiagnosticStatus.WARN
207
status.message =
'MULTIPLE_PUBLISHERS'
208
elif
age
is
None
:
209
status.level = (
210
DiagnosticStatus.WARN
if
threshold
is
not
None
211
else
DiagnosticStatus.OK)
212
status.message =
'NO_MESSAGE'
213
elif
threshold
is
not
None
and
age > threshold:
214
status.level = DiagnosticStatus.WARN
215
status.message =
'STALE'
216
elif
(self.
_last_stamp_problem
[topic]
is
not
None
and
217
now - self.
_last_stamp_problem
[topic] <= 30.0):
218
status.level = DiagnosticStatus.WARN
219
status.message =
'NON_MONOTONIC_STAMP'
220
else
:
221
status.level = DiagnosticStatus.OK
222
status.message =
'RECEIVING'
if
threshold
is
not
None
else
'OBSERVED'
223
status.values = [
224
KeyValue(key=
'hz_wall'
, value=f
'{hz:.3f}'
),
225
KeyValue(key=
'publisher_count'
, value=str(publishers)),
226
KeyValue(key=
'age_wall_sec'
, value=f
'{age:.1f}'
if
age
is
not
None
else
'never'
),
227
KeyValue(key=
'messages_since_zone'
, value=str(
228
self.
_received_since_zone
[topic])
if
self.
_zone_received
else
'no_zone'
),
229
]
230
if
self.
_last_stamp_ns
[topic]
is
not
None
:
231
status.values.extend([
232
KeyValue(key=
'last_stamp_ns'
, value=str(self.
_last_stamp_ns
[topic])),
233
KeyValue(key=
'repeated_stamps'
, value=str(self.
_repeated_stamps
[topic])),
234
KeyValue(key=
'backward_stamps'
, value=str(self.
_backward_stamps
[topic])),
235
])
236
if
self.
_last_point_count
[topic]
is
not
None
:
237
status.values.append(KeyValue(
238
key=
'last_point_count'
, value=str(self.
_last_point_count
[topic])))
239
if
self.
_last_scan_timing
[topic]
is
not
None
:
240
increment, duration = self.
_last_scan_timing
[topic]
241
status.values.extend([
242
KeyValue(key=
'time_increment_sec'
, value=f
'{increment:.9f}'
),
243
KeyValue(key=
'scan_time_sec'
, value=f
'{duration:.9f}'
),
244
])
245
return
status
246
247
def
_publish_diagnostics
(self) -> None:
248
"""Foxglove Diagnosticsパネル向けに各監視項目を1.0s周期で発行する。"""
249
now = time.monotonic()
250
statuses = [self.
_topic_status
(topic, now)
for
topic
in
self.
_samples
]
251
if
self.
_tf_target
is
not
None
:
252
tf_status = DiagnosticStatus()
253
tf_status.name = f
'coverage/tf/{self._tf_target}_to_base_footprint'
254
tf_status.hardware_id =
'coverage'
255
available = self.
_tf_buffer
.can_transform(
256
self.
_tf_target
,
'base_footprint'
, Time())
257
tf_status.level = DiagnosticStatus.OK
if
available
else
DiagnosticStatus.WARN
258
tf_status.message =
'AVAILABLE'
if
available
else
'UNAVAILABLE'
259
statuses.append(tf_status)
260
if
self.
_expect_clock
:
261
clock_status = DiagnosticStatus()
262
clock_status.name =
'coverage/simulation_clock'
263
clock_status.hardware_id =
'coverage'
264
clock_age = (
265
now - self.
_clock_last_seen
266
if
self.
_clock_last_seen
is
not
None
else
None
)
267
clock_status.level = (
268
DiagnosticStatus.OK
if
clock_age
is
not
None
and
clock_age <= 2.0
269
else
DiagnosticStatus.WARN)
270
clock_status.message = (
271
'RECEIVING'
if
clock_age
is
not
None
and
clock_age <= 2.0
272
else
'NO_MESSAGE'
if
clock_age
is
None
else
'STALE'
)
273
clock_status.values = [
274
KeyValue(
275
key=
'age_wall_sec'
,
276
value=f
'{clock_age:.1f}'
if
clock_age
is
not
None
else
'never'
),
277
KeyValue(
278
key=
'simulation_speed'
,
279
value=f
'{self._sim_rate:.2f}x'
280
if
self.
_sim_rate
is
not
None
else
'unknown'
),
281
KeyValue(
282
key=
'backward_clock_events'
,
283
value=str(self.
_clock_backward_events
)),
284
]
285
statuses.append(clock_status)
286
if
not
self.
_sensor_profile
:
287
flow = DiagnosticStatus()
288
flow.name =
'coverage/flow'
289
flow.hardware_id =
'coverage'
290
flow.level = DiagnosticStatus.OK
291
flow.message = self.
_phase
292
flow.values = [KeyValue(key=
'bridge_phase'
, value=self.
_phase
)]
293
if
self.
_sim_rate
is
not
None
:
294
flow.values.append(KeyValue(
295
key=
'simulation_speed'
, value=f
'{self._sim_rate:.2f}x'
))
296
statuses.append(flow)
297
msg = DiagnosticArray()
298
msg.header.stamp = self.get_clock().now().to_msg()
299
msg.status = statuses
300
self.
_diagnostics_pub
.publish(msg)
301
302
303
def
main
(args=None) -> None:
304
"""coverage_monitorをROS 2ノードとして実行する。"""
305
rclpy.init(args=args)
306
node =
CoverageMonitor
()
307
try
:
308
rclpy.spin(node)
309
except
KeyboardInterrupt:
310
pass
311
finally
:
312
node.destroy_node()
313
rclpy.shutdown()
314
315
316
if
__name__ ==
'__main__'
:
317
main
()
coverage.coverage_monitor.CoverageMonitor
主要トピックの実時間Hz、最終受信からの秒数、TFを発行する。
定義
coverage_monitor.py:53
coverage.coverage_monitor.CoverageMonitor._topic_status
DiagnosticStatus _topic_status(self, str topic, float now)
一つのtopicについて実時間Hzと受信停止を表す。
定義
coverage_monitor.py:187
coverage.coverage_monitor.CoverageMonitor._on_clock
_on_clock
定義
coverage_monitor.py:128
coverage.coverage_monitor.CoverageMonitor._tf_listener
_tf_listener
定義
coverage_monitor.py:130
coverage.coverage_monitor.CoverageMonitor._last_scan_timing
dict _last_scan_timing
定義
coverage_monitor.py:104
coverage.coverage_monitor.CoverageMonitor._tf_buffer
_tf_buffer
定義
coverage_monitor.py:129
coverage.coverage_monitor.CoverageMonitor._clock_last_seen
float|None _clock_last_seen
定義
coverage_monitor.py:109
coverage.coverage_monitor.CoverageMonitor._sim_rate
float|None _sim_rate
定義
coverage_monitor.py:110
coverage.coverage_monitor.CoverageMonitor._on_phase
_on_phase
定義
coverage_monitor.py:125
coverage.coverage_monitor.CoverageMonitor._on_topic
None _on_topic(self, str topic, msg)
受信時刻、ヘッダー時刻の重複、点数を記録する。
定義
coverage_monitor.py:138
coverage.coverage_monitor.CoverageMonitor._zone_topic
_zone_topic
定義
coverage_monitor.py:105
coverage.coverage_monitor.CoverageMonitor._last_stamp_problem
dict _last_stamp_problem
定義
coverage_monitor.py:102
coverage.coverage_monitor.CoverageMonitor._sensor_profile
str _sensor_profile
定義
coverage_monitor.py:62
coverage.coverage_monitor.CoverageMonitor._clock_backward_events
int _clock_backward_events
定義
coverage_monitor.py:111
coverage.coverage_monitor.CoverageMonitor._thresholds
dict _thresholds
定義
coverage_monitor.py:97
coverage.coverage_monitor.CoverageMonitor._backward_stamps
dict _backward_stamps
定義
coverage_monitor.py:101
coverage.coverage_monitor.CoverageMonitor._diagnostics_pub
_diagnostics_pub
定義
coverage_monitor.py:131
coverage.coverage_monitor.CoverageMonitor._clock_sample
tuple[float, float]|None _clock_sample
定義
coverage_monitor.py:108
coverage.coverage_monitor.CoverageMonitor._received_since_zone
dict _received_since_zone
定義
coverage_monitor.py:98
coverage.coverage_monitor.CoverageMonitor._last_stamp_ns
dict _last_stamp_ns
定義
coverage_monitor.py:99
coverage.coverage_monitor.CoverageMonitor._tf_target
str _tf_target
定義
coverage_monitor.py:65
coverage.coverage_monitor.CoverageMonitor._expect_clock
_expect_clock
定義
coverage_monitor.py:63
coverage.coverage_monitor.CoverageMonitor.__init__
None __init__(self)
定義
coverage_monitor.py:56
coverage.coverage_monitor.CoverageMonitor._phase
str _phase
定義
coverage_monitor.py:107
coverage.coverage_monitor.CoverageMonitor._last_seen
dict _last_seen
定義
coverage_monitor.py:96
coverage.coverage_monitor.CoverageMonitor._repeated_stamps
dict _repeated_stamps
定義
coverage_monitor.py:100
coverage.coverage_monitor.CoverageMonitor._samples
dict _samples
定義
coverage_monitor.py:95
coverage.coverage_monitor.CoverageMonitor._publish_diagnostics
_publish_diagnostics
定義
coverage_monitor.py:135
coverage.coverage_monitor.CoverageMonitor._last_point_count
dict _last_point_count
定義
coverage_monitor.py:103
coverage.coverage_monitor.CoverageMonitor._zone_received
bool _zone_received
定義
coverage_monitor.py:106
coverage.coverage_monitor.main
None main(args=None)
coverage_monitorをROS 2ノードとして実行する。
定義
coverage_monitor.py:303
coverage
coverage_monitor.py
構築:
1.18.0