coverage
読み取り中…
検索中…
一致する文字列を見つけられません
coverage_monitor.py
[詳解]
1#!/usr/bin/env python3
2"""清掃ナビゲーションの入力、計画、速度指令とTFを診断情報へ集約する。"""
3
4from __future__ import annotations
5
6import time
7from collections import deque
8
9import rclpy
10from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
11from geometry_msgs.msg import PolygonStamped, PoseWithCovarianceStamped, Twist
12from nav_msgs.msg import OccupancyGrid, Odometry, Path
13from rclpy.clock import Clock
14from rclpy.clock_type import ClockType
15from rclpy.node import Node
16from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
17from rclpy.time import Time
18from rosgraph_msgs.msg import Clock as ClockMessage
19from sensor_msgs.msg import Imu, LaserScan, PointCloud2
20from std_msgs.msg import String
21from tf2_ros import Buffer, TransformListener
22
23
24# 最大30sの実時間サンプルからHzを求める。経路と速度指令はイベント型である。
25TOPICS = {
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}
35THREE_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}
46STAMP_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'}
50SENSOR_STAGES = ('scan', 'cloud', 'filtered', 'assembled', 'localization')
51
52
53class 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
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(
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
303def 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
316if __name__ == '__main__':
317 main()
主要トピックの実時間Hz、最終受信からの秒数、TFを発行する。
DiagnosticStatus _topic_status(self, str topic, float now)
一つのtopicについて実時間Hzと受信停止を表す。
None _on_topic(self, str topic, msg)
受信時刻、ヘッダー時刻の重複、点数を記録する。
None main(args=None)
coverage_monitorをROS 2ノードとして実行する。