coverage
読み取り中…
検索中…
一致する文字列を見つけられません
coverage_bridge.py
[詳解]
1#!/usr/bin/env python3
2"""cleaning_zoneをNavigateCompleteCoverageアクションへ中継するROS 2ノード。
3
4入力ポリゴンの検証、Nav2ゴール送信、進捗と終了状態の通知を行う。
5経路生成と追従はNav2のcoverage_serverとCoverageNavigatorが担当する。
6"""
7
8from __future__ import annotations
9
10import functools
11import threading
12import time
13from typing import Any
14
15import rclpy
16from action_msgs.msg import GoalStatus
17from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
18from geometry_msgs.msg import Point32, Polygon, PolygonStamped
19from nav_msgs.msg import OccupancyGrid
20from opennav_coverage_msgs.action import NavigateCompleteCoverage
21from rclpy.action import ActionClient
22from rclpy.clock import Clock
23from rclpy.clock_type import ClockType
24from rclpy.node import Node
25from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
26from std_msgs.msg import String
27
28from coverage.zone_field import NormalizedZone
29from coverage.zone_map import zone_occupancy
30from coverage.retry_policy import RetryPolicy
31
32
33# /coverage/state 通知の terminal 状態文字列。
34STATE_DONE = 'DONE'
35STATE_CANCELED = 'CANCELED'
36STATE_FAILED = 'FAILED'
37STATE_FAILED_INVALID_ZONE = 'FAILED_INVALID_ZONE'
38STATE_FAILED_REJECTED = 'FAILED_REJECTED'
39STATE_WAITING_NAV_SERVER = 'WAITING_NAV_SERVER'
40
41
42def _coerce_float(value: Any, default: float) -> float:
43 """ROSパラメータ値をfloatへ変換し、変換不能時は既定値を返す。"""
44 try:
45 if isinstance(value, str):
46 value = value.strip().strip('"').strip("'")
47 return float(value)
48 except (TypeError, ValueError):
49 return default
50
51
52def _coerce_int(value: Any, default: int) -> int:
53 """ROSパラメータ値を整数へ変換し、変換不能時は既定値を返す。"""
54 try:
55 if isinstance(value, str):
56 value = value.strip().strip('"').strip("'")
57 return int(float(value))
58 except (TypeError, ValueError):
59 return default
60
61
62def _coerce_str(value: Any, default: str) -> str:
63 """ROSパラメータ値を文字列へ変換し、Noneには既定値を返す。"""
64 if value is None:
65 return default
66 if isinstance(value, str):
67 return value
68 return str(value)
69
70
71class CoverageBridge(Node):
72 """清掃ゾーンのNav2実行要求と状態通知を管理する。
73
74 ``/cleaning_zone`` の最新値を送信し、サーバ不在やゴール拒否時は再試行する。
75 新しいゾーンの受信時は実行中ゴールをキャンセルして最新値を優先する。
76 """
77
78 def __init__(self) -> None:
79 """topic、action client、再試行タイマーを初期化する。"""
80 super().__init__('coverage_bridge')
81 self.declare_parameter('zone_topic', '/cleaning_zone')
82 self.declare_parameter('state_topic', '/coverage/state')
83 self.declare_parameter('action_name', 'navigate_complete_coverage')
84 self.declare_parameter('frame_id', 'map')
85 self.declare_parameter('behavior_tree', '')
86 self.declare_parameter('min_vertices', 3)
87 self.declare_parameter('retry_limit', 3)
88 self.declare_parameter('backoff_sec', 5.0)
89 self.declare_parameter('check_map_occupancy', True)
90 self.declare_parameter('max_occupied_ratio', 0.05)
91 # use_sim_timeはrclpyが自動宣言するため、ここでは重複して宣言しない。
92
93 raw = {n: self.get_parameter(n).value for n in (
94 'zone_topic', 'state_topic', 'action_name', 'frame_id',
95 'behavior_tree',
96 )}
97 self._zone_topic = _coerce_str(raw['zone_topic'], '/cleaning_zone')
98 self._state_topic = _coerce_str(raw['state_topic'], '/coverage/state')
100 raw['action_name'], 'navigate_complete_coverage')
101 self._default_frame = _coerce_str(raw['frame_id'], 'map')
102 self._behavior_tree = _coerce_str(raw['behavior_tree'], '')
103
104 self._pending_zone: NormalizedZone | None = None
105 self._lock = threading.Lock()
106 self._goal_handle = None
107 self._executing = False
108 # 最新世代以外の非同期応答を破棄し、goal切替後の状態上書きを防ぐ。
109 self._generation = 0
110 # 拒否・実行失敗の連続計数。受理ではリセットしない。
112 self._sync_retry_params()
113 # 同一state文字列の連続publishを抑止する。
114 self._last_state = ''
115 self._phase = 'IDLE'
116 self._phase_reason = 'waiting for cleaning_zone'
118 self._zone_received_at: float | None = None
119 self._occupancy_check = 'NOT_RUN'
120 self._occupied_ratio: float | None = None
121 self._unknown_ratio: float | None = None
122 self._occupancy_limit: float | None = None
123 self._goal_id = ''
124 self._last_nav_error_code: int | None = None
125 self._feedback_received_at: float | None = None
126 self._remaining_distance: float | None = None
127 self._recoveries = 0
128
129 # state通知はQoS depth 10でpublishする。
130 self._state_pub = self.create_publisher(String, self._state_topic, 10)
131 phase_qos = QoSProfile(
132 depth=1,
133 durability=DurabilityPolicy.TRANSIENT_LOCAL,
134 reliability=ReliabilityPolicy.RELIABLE,
135 )
136 self._phase_pub = self.create_publisher(
137 String, '/coverage/phase', phase_qos)
138 self._diagnostics_pub = self.create_publisher(
139 DiagnosticArray, '/diagnostics', 10)
140 # 入力ゾーンをQoS depth 10で受信する。
141 self.create_subscription(
142 PolygonStamped, self._zone_topic, self._on_zone, 10
143 )
144 # 静的障害物の事前検証用に/mapをtransient localで受信する。
145 map_qos = QoSProfile(
146 depth=1,
147 durability=DurabilityPolicy.TRANSIENT_LOCAL,
148 history=HistoryPolicy.KEEP_LAST,
149 reliability=ReliabilityPolicy.RELIABLE,
150 )
151 self._latest_map: OccupancyGrid | None = None
152 self.create_subscription(
153 OccupancyGrid, '/map', self._on_map, map_qos
154 )
155 self._nav_client: ActionClient = ActionClient(
156 self, NavigateCompleteCoverage, self._action_name
157 )
158 # 保留ゾーンを1.0s周期で再送可能か確認する。
159 self.create_timer(1.0, self._on_retry_timer)
160 # 診断はGazebo停止中も1.0s周期で更新する。
161 self.create_timer(
163 clock=Clock(clock_type=ClockType.STEADY_TIME))
164 self._set_phase('IDLE', 'waiting for cleaning_zone')
165
166 self.get_logger().info(
167 f'coverage_bridge started: zone={self._zone_topic} '
168 f'action={self._action_name}'
169 )
170
171 def _bool_param(self, name: str, default: bool) -> bool:
172 """指定パラメータをboolとして取得する。"""
173 value = self.get_parameter(name).value
174 if isinstance(value, bool):
175 return value
176 if isinstance(value, str):
177 return value.strip().lower() in ('true', '1', 'yes')
178 return default if value is None else bool(value)
179
180 def _float_param(self, name: str, default: float) -> float:
181 """指定パラメータをfloatとして取得する。"""
182 return _coerce_float(self.get_parameter(name).value, default)
183
184 def _int_param(self, name: str, default: int) -> int:
185 """指定パラメータをintとして取得する。"""
186 return _coerce_int(self.get_parameter(name).value, default)
187
188 def _sync_retry_params(self) -> None:
189 """再送上限と待機秒数 [s] をROSパラメータへ追従させる。"""
190 with self._lock:
191 self._retry.limit = self._int_param('retry_limit', 3)
192 self._retry.backoff_sec = self._float_param('backoff_sec', 5.0)
193
194 def _publish_state(self, text: str) -> None:
195 """coverage状態文字列をstate topicへpublishする。同一文言の連続送信は抑止する。"""
196 with self._lock:
197 if text == self._last_state:
198 return
199 self._last_state = text
200 msg = String()
201 msg.data = text
202 self._state_pub.publish(msg)
203
204 def _set_phase(self, phase: str, reason: str = '') -> None:
205 """最新の処理段階を保持し、変化時に/coverage/phaseへ通知する。"""
206 with self._lock:
207 changed = phase != self._phase
208 self._phase = phase
209 self._phase_reason = reason
210 if changed or phase == 'IDLE':
211 msg = String()
212 msg.data = phase
213 self._phase_pub.publish(msg)
214
215 def _publish_diagnostics(self) -> None:
216 """bridge内部の判定値とgoal段階を/diagnosticsへ1.0s周期で出す。"""
217 with self._lock:
218 phase = self._phase
219 reason = self._phase_reason
220 vertices = self._zone_vertices
221 received_at = self._zone_received_at
222 occupancy_check = self._occupancy_check
223 occupied_ratio = self._occupied_ratio
224 unknown_ratio = self._unknown_ratio
225 occupancy_limit = self._occupancy_limit
226 goal_id = self._goal_id
227 last_nav_error_code = self._last_nav_error_code
228 feedback_received_at = self._feedback_received_at
229 remaining_distance = self._remaining_distance
230 recoveries = self._recoveries
231 pending = self._pending_zone is not None
232 failures = self._retry.failures
233 retry_limit = self._retry.limit
234 status = DiagnosticStatus()
235 status.name = 'coverage/bridge'
236 status.hardware_id = 'coverage'
237 status.level = (
238 DiagnosticStatus.ERROR if phase in ('INVALID_ZONE', 'FAILED')
239 else DiagnosticStatus.WARN if phase in ('WAITING_NAV_SERVER', 'RETRYING')
240 else DiagnosticStatus.OK)
241 status.message = f'{phase}: {reason}' if reason else phase
242 values = {
243 'phase': phase,
244 'reason': reason,
245 'zone_vertices': vertices,
246 'zone_age_wall_sec': (
247 f'{time.monotonic() - received_at:.1f}'
248 if received_at is not None else 'never'),
249 'occupancy_check': occupancy_check,
250 'occupied_ratio': (
251 f'{occupied_ratio:.4f}' if occupied_ratio is not None else 'unknown'),
252 'unknown_ratio': (
253 f'{unknown_ratio:.4f}' if unknown_ratio is not None else 'unknown'),
254 'max_occupied_ratio': (
255 f'{occupancy_limit:.4f}' if occupancy_limit is not None else 'unknown'),
256 'goal_id': goal_id or 'none',
257 'last_nav_error_code': (
258 last_nav_error_code if last_nav_error_code is not None else 'unknown'),
259 'feedback_age_wall_sec': (
260 f'{time.monotonic() - feedback_received_at:.1f}'
261 if feedback_received_at is not None else 'never'),
262 'distance_remaining_m': (
263 f'{remaining_distance:.2f}'
264 if remaining_distance is not None else 'unknown'),
265 'number_of_recoveries': recoveries,
266 'pending_zone': str(pending).lower(),
267 'retry_failures': f'{failures}/{retry_limit}',
268 }
269 status.values = [KeyValue(key=k, value=str(v)) for k, v in values.items()]
270 msg = DiagnosticArray()
271 msg.header.stamp = self.get_clock().now().to_msg()
272 msg.status = [status]
273 self._diagnostics_pub.publish(msg)
274
275 def _on_map(self, msg: OccupancyGrid) -> None:
276 """事前検証用に最新の地図を保持する。"""
277 with self._lock:
278 self._latest_map = msg
279
280 def _zone_blocked_by_static_obstacles(self, zone: NormalizedZone) -> bool:
281 """ゾーン内占有率が上限超ならTrueを返す。地図未受信時は検証しない。"""
282 with self._lock:
283 grid = self._latest_map
284 if grid is None:
285 with self._lock:
286 self._occupancy_check = 'NO_MAP'
287 self.get_logger().warn(
288 '地図未受信のため占有検証を省略します',
289 throttle_duration_sec=10.0,
290 )
291 return False
292 info = grid.info
293 occ = zone_occupancy(
294 info.width, info.height, info.resolution,
295 info.origin.position.x, info.origin.position.y,
296 grid.data, zone.points,
297 )
298 if occ.inside_cells == 0:
299 with self._lock:
300 self._occupancy_check = 'OUTSIDE_MAP'
301 self.get_logger().warn('ゾーンが地図範囲外のため占有検証を省略します')
302 return False
303 limit = self._float_param('max_occupied_ratio', 0.05)
304 with self._lock:
305 self._occupancy_check = 'CHECKED'
306 self._occupied_ratio = occ.occupied_ratio
307 self._unknown_ratio = occ.unknown_ratio
308 self._occupancy_limit = limit
309 self.get_logger().info(
310 f'ゾーン内占有率: {occ.occupied_ratio:.3f} '
311 f'(unknown: {occ.unknown_ratio:.3f}, 上限: {limit:.3f})'
312 )
313 return occ.occupied_ratio > limit
314
315 def _on_zone(self, msg: PolygonStamped) -> None:
316 """受信ゾーンを検証し、Nav2へ最新の実行要求を送る。"""
317 with self._lock:
318 self._zone_received_at = time.monotonic()
319 self._zone_vertices = len(msg.polygon.points)
320 self._occupancy_check = 'NOT_RUN'
321 self._occupied_ratio = None
322 self._unknown_ratio = None
323 self._occupancy_limit = None
324 self._set_phase('ZONE_RECEIVED', 'validating polygon')
325 min_vertices = self._int_param('min_vertices', 3)
326 try:
327 zone = NormalizedZone.from_points(
328 frame_id=msg.header.frame_id or self._default_frame,
329 points=[(p.x, p.y) for p in msg.polygon.points],
330 min_vertices=min_vertices,
331 )
332 except ValueError as e:
333 self._set_phase('INVALID_ZONE', str(e))
334 self.get_logger().error(f'不正なゾーンのため無視します: {e}')
335 self._publish_state(STATE_FAILED_INVALID_ZONE)
336 return
337 if self._bool_param('check_map_occupancy', True):
339 self._set_phase('INVALID_ZONE', 'occupied ratio exceeds limit')
340 self.get_logger().error(
341 'ゾーン内に静的障害物が多いため無視します'
342 )
343 self._publish_state(STATE_FAILED_INVALID_ZONE)
344 return
345 else:
346 with self._lock:
347 self._occupancy_check = 'DISABLED'
349 with self._lock:
350 self._pending_zone = zone
351 self._goal_id = ''
352 self._last_nav_error_code = None
353 self._feedback_received_at = None
354 self._remaining_distance = None
355 self._recoveries = 0
356 self._sync_retry_params()
357 self._retry.reset()
358 self._set_phase('ZONE_READY', 'polygon accepted for goal submission')
359 self._try_send(zone)
360
361 def _on_retry_timer(self) -> None:
362 """実行中要求がなく再試行時刻を過ぎた場合、保留ゾーンを再送する。"""
363 with self._lock:
364 pending = self._pending_zone
365 executing = self._executing
366 self._sync_retry_params()
367 if pending is not None and not executing and self._retry.ready(time.monotonic()):
368 self._try_send(pending)
369
370 def _make_goal(self, zone: NormalizedZone) -> NavigateCompleteCoverage.Goal:
371 """正規化済みゾーンからCoverageNavigatorのgoalを組み立てる。"""
372 goal = NavigateCompleteCoverage.Goal()
373 goal.frame_id = zone.frame_id
374 poly = Polygon()
375 for x, y in zone.points:
376 pt = Point32()
377 pt.x, pt.y = x, y
378 pt.z = 0.0
379 poly.points.append(pt)
380 # Fields2Coverは閉路(始点==終点)を要求するため、始点を末尾へ追加する。
381 # 未閉形のまま送るとcoverage_serverがINVALID_COORDS(803)で破棄する。
382 if zone.points:
383 pt = Point32()
384 pt.x, pt.y = zone.points[0][0], zone.points[0][1]
385 pt.z = 0.0
386 poly.points.append(pt)
387 goal.polygons.append(poly)
388 if self._behavior_tree:
389 goal.behavior_tree = self._behavior_tree
390 return goal
391
392 def _cancel_current_goal(self) -> None:
393 """現在のgoalを無効化し、受理済みならNav2へcancelを要求する。"""
394 with self._lock:
395 self._generation += 1
396 handle = self._goal_handle
397 self._goal_handle = None
398 self._executing = False
399 if handle is not None:
400 try:
401 handle.cancel_goal_async()
402 self.get_logger().info('実行中ゴールをキャンセルしました')
403 except Exception as e: # noqa: BLE001
404 self.get_logger().warn(f'ゴールキャンセル失敗: {e}')
405
406 def _try_send(self, zone: NormalizedZone) -> None:
407 """action serverを確認し、ゾーンgoalを非同期で送信する。
408
409 server不在の確認は非ブロッキングで行い、1.0s周期タイマーが再送する。
410 """
411 if not self._nav_client.wait_for_server(timeout_sec=0.0):
412 self.get_logger().warn(
413 f'{self._action_name}サーバ不在のため再試行します',
414 throttle_duration_sec=10.0,
415 )
416 self._publish_state(STATE_WAITING_NAV_SERVER)
417 self._set_phase('WAITING_NAV_SERVER', 'action server unavailable')
418 return
419 with self._lock:
420 self._generation += 1
421 gen = self._generation
422 self._executing = True
423 self._goal_handle = None
424 self._goal_id = ''
425 self._last_nav_error_code = None
426 self._feedback_received_at = None
427 self._remaining_distance = None
428 self._recoveries = 0
429 self._set_phase('GOAL_REQUESTED', 'waiting for action response')
430 send_future = self._nav_client.send_goal_async(
431 self._make_goal(zone),
432 feedback_callback=functools.partial(self._on_feedback, gen),
433 )
434 send_future.add_done_callback(functools.partial(self._on_goal_response, gen))
435
436 def _stale(self, gen: int) -> bool:
437 """指定世代が最新でなく、結果を破棄すべきならTrueを返す。"""
438 with self._lock:
439 return gen != self._generation
440
441 def _on_feedback(self, gen: int, msg) -> None:
442 """最新goalの残距離とリカバリ回数を記録する。"""
443 if self._stale(gen):
444 return
445 remaining = msg.feedback.distance_remaining
446 with self._lock:
447 self._feedback_received_at = time.monotonic()
448 self._remaining_distance = remaining
449 self._recoveries = msg.feedback.number_of_recoveries
450 self.get_logger().info(
451 f'清掃中: 残り約{remaining:.1f}m '
452 f'(recoveries={msg.feedback.number_of_recoveries})',
453 throttle_duration_sec=10.0,
454 )
455
456 def _on_goal_response(self, gen: int, future) -> None:
457 """goal受理結果を処理し、受理時は結果コールバックを登録する。"""
458 if self._stale(gen):
459 return
460 try:
461 handle = future.result()
462 except Exception as e: # noqa: BLE001
463 self.get_logger().error(f'ゴール送信エラー: {e}')
464 with self._lock:
465 self._executing = False
467 goal_rejected=False, reason=f'goal send error: {e}')
468 return
469 if handle is None or not handle.accepted:
470 with self._lock:
471 self._executing = False
472 self._register_setback(goal_rejected=True)
473 return
474 with self._lock:
475 self._goal_handle = handle
476 self._goal_id = bytes(handle.goal_id.uuid).hex()
477 self._set_phase('GOAL_ACCEPTED', 'Nav2 action accepted')
478 result_future = handle.get_result_async()
479 result_future.add_done_callback(functools.partial(self._on_nav_result, gen))
480
481 def _register_setback(self, goal_rejected: bool, reason: str = '') -> None:
482 """拒否・実行失敗を計数し、上限到達で諦めてterminal状態を通知する。
483
484 受理時のリセットは行わない。受理後に失敗が続けば上限で諦める。
485 """
486 self._sync_retry_params()
487 give_up = self._retry.record_failure(time.monotonic())
488 detail = reason or (
489 'Nav2 rejected goal' if goal_rejected else 'Nav2 execution failed')
490 if give_up:
491 with self._lock:
492 self._pending_zone = None
493 self._set_phase('FAILED', detail)
494 else:
495 self._set_phase('RETRYING', detail)
496 if goal_rejected:
497 count, limit = self._retry.failures, self._retry.limit
498 if give_up:
499 self.get_logger().error(
500 f'Nav2にゴールを{count}回拒否されました。諦めます。'
501 '初期位置(2D Pose Estimate)・lifecycle(bt_navigator active)・'
502 'TF(map->base_footprint)を確認してください。'
503 )
504 self._publish_state(STATE_FAILED_REJECTED)
505 return
506 self.get_logger().warn(
507 f'Nav2にゴールを拒否されました({count}/{limit})。'
508 f'{self._retry.backoff_sec:.0f}s後に再送します。',
509 throttle_duration_sec=10.0,
510 )
511 self._publish_state(self._retry.state_text())
512 return
513 if give_up:
514 self.get_logger().error(
515 f'Nav2実行が{self._retry.failures}回失敗しました。諦めます。'
516 )
517 self._publish_state(STATE_FAILED)
518 return
519 self.get_logger().warn(
520 f'Nav2実行が失敗しました({self._retry.state_text()})。'
521 f'{self._retry.backoff_sec:.0f}s後に再送します。',
522 throttle_duration_sec=10.0,
523 )
524 self._publish_state(self._retry.state_text())
525
526 def _on_nav_result(self, gen: int, future) -> None:
527 """最新goalの終了状態をDONE、CANCELED、失敗として通知する。"""
528 if self._stale(gen):
529 return
530 with self._lock:
531 self._executing = False
532 try:
533 action_result = future.result()
534 status = action_result.status
535 error_code = action_result.result.error_code
536 except Exception as e: # noqa: BLE001
537 self.get_logger().error(f'Nav2実行エラー: {e}')
539 goal_rejected=False, reason=f'Nav2 result error: {e}')
540 return
541 with self._lock:
542 self._last_nav_error_code = error_code
543 if status == GoalStatus.STATUS_SUCCEEDED:
544 self._set_phase('DONE', 'Nav2 action succeeded')
545 self.get_logger().info('清掃完了(DONE)')
546 self._publish_state(STATE_DONE)
547 with self._lock:
548 self._pending_zone = None
549 self._retry.reset()
550 elif status == GoalStatus.STATUS_CANCELED:
551 self._set_phase('CANCELED', 'Nav2 action canceled')
552 self.get_logger().info('清掃キャンセル(CANCELED)')
553 with self._lock:
554 self._pending_zone = None
555 self._goal_handle = None
556 self._publish_state(STATE_CANCELED)
557 else:
558 self.get_logger().error(
559 f'清掃失敗(status={status}, error_code={error_code})')
561 goal_rejected=False,
562 reason=f'Nav2 action status={status}, error_code={error_code}')
563
564
565def main(args=None) -> None:
566 """coverage_bridgeを初期化し、ROS 2イベントを処理する。"""
567 rclpy.init(args=args)
568 node = CoverageBridge()
569 try:
570 rclpy.spin(node)
571 except KeyboardInterrupt:
572 pass
573 finally:
574 node.destroy_node()
575 rclpy.shutdown()
576
577
578if __name__ == '__main__':
579 main()
清掃ゾーンのNav2実行要求と状態通知を管理する。
None _register_setback(self, bool goal_rejected, str reason='')
拒否・実行失敗を計数し、上限到達で諦めてterminal状態を通知する。
bool _bool_param(self, str name, bool default)
指定パラメータをboolとして取得する。
NavigateCompleteCoverage.Goal _make_goal(self, NormalizedZone zone)
正規化済みゾーンからCoverageNavigatorのgoalを組み立てる。
bool _zone_blocked_by_static_obstacles(self, NormalizedZone zone)
ゾーン内占有率が上限超ならTrueを返す。地図未受信時は検証しない。
None _try_send(self, NormalizedZone zone)
action serverを確認し、ゾーンgoalを非同期で送信する。
None __init__(self)
topic、action client、再試行タイマーを初期化する。
float _float_param(self, str name, float default)
指定パラメータをfloatとして取得する。
None _sync_retry_params(self)
再送上限と待機秒数 [s] をROSパラメータへ追従させる。
None _on_feedback(self, int gen, msg)
最新goalの残距離とリカバリ回数を記録する。
None _publish_state(self, str text)
coverage状態文字列をstate topicへpublishする。同一文言の連続送信は抑止する。
int _int_param(self, str name, int default)
指定パラメータをintとして取得する。
None _cancel_current_goal(self)
現在のgoalを無効化し、受理済みならNav2へcancelを要求する。
bool _stale(self, int gen)
指定世代が最新でなく、結果を破棄すべきならTrueを返す。
None _set_phase(self, str phase, str reason='')
最新の処理段階を保持し、変化時に/coverage/phaseへ通知する。
連続失敗を数え、再送可否と待機期限を判定する。
None main(args=None)
coverage_bridgeを初期化し、ROS 2イベントを処理する。
float _coerce_float(Any value, float default)
ROSパラメータ値をfloatへ変換し、変換不能時は既定値を返す。
int _coerce_int(Any value, int default)
ROSパラメータ値を整数へ変換し、変換不能時は既定値を返す。
str _coerce_str(Any value, str default)
ROSパラメータ値を文字列へ変換し、Noneには既定値を返す。
Nav2ゴール再送の連続失敗計数と待機時刻をROS非依存で管理する。
清掃ゾーンの点列をROSメッセージに依存せず検証・正規化する。
清掃ゾーン内セルの地図占有率をROS非依存で求める。