2"""cleaning_zoneをNavigateCompleteCoverageアクションへ中継するROS 2ノード。
4入力ポリゴンの検証、Nav2ゴール送信、進捗と終了状態の通知を行う。
5経路生成と追従はNav2のcoverage_serverとCoverageNavigatorが担当する。
8from __future__
import annotations
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
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'
43 """ROSパラメータ値をfloatへ変換し、変換不能時は既定値を返す。"""
45 if isinstance(value, str):
46 value = value.strip().strip(
'"').strip(
"'")
48 except (TypeError, ValueError):
53 """ROSパラメータ値を整数へ変換し、変換不能時は既定値を返す。"""
55 if isinstance(value, str):
56 value = value.strip().strip(
'"').strip(
"'")
57 return int(float(value))
58 except (TypeError, ValueError):
63 """ROSパラメータ値を文字列へ変換し、Noneには既定値を返す。"""
66 if isinstance(value, str):
72 """清掃ゾーンのNav2実行要求と状態通知を管理する。
74 ``/cleaning_zone`` の最新値を送信し、サーバ不在やゴール拒否時は再試行する。
75 新しいゾーンの受信時は実行中ゴールをキャンセルして最新値を優先する。
79 """topic、action client、再試行タイマーを初期化する。"""
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)
93 raw = {n: self.get_parameter(n).value
for n
in (
94 'zone_topic',
'state_topic',
'action_name',
'frame_id',
100 raw[
'action_name'],
'navigate_complete_coverage')
131 phase_qos = QoSProfile(
133 durability=DurabilityPolicy.TRANSIENT_LOCAL,
134 reliability=ReliabilityPolicy.RELIABLE,
137 String,
'/coverage/phase', phase_qos)
139 DiagnosticArray,
'/diagnostics', 10)
141 self.create_subscription(
145 map_qos = QoSProfile(
147 durability=DurabilityPolicy.TRANSIENT_LOCAL,
148 history=HistoryPolicy.KEEP_LAST,
149 reliability=ReliabilityPolicy.RELIABLE,
152 self.create_subscription(
163 clock=Clock(clock_type=ClockType.STEADY_TIME))
164 self.
_set_phase(
'IDLE',
'waiting for cleaning_zone')
166 self.get_logger().info(
167 f
'coverage_bridge started: zone={self._zone_topic} '
168 f
'action={self._action_name}'
172 """指定パラメータをboolとして取得する。"""
173 value = self.get_parameter(name).value
174 if isinstance(value, bool):
176 if isinstance(value, str):
177 return value.strip().lower()
in (
'true',
'1',
'yes')
178 return default
if value
is None else bool(value)
181 """指定パラメータをfloatとして取得する。"""
182 return _coerce_float(self.get_parameter(name).value, default)
185 """指定パラメータをintとして取得する。"""
186 return _coerce_int(self.get_parameter(name).value, default)
189 """再送上限と待機秒数 [s] をROSパラメータへ追従させる。"""
195 """coverage状態文字列をstate topicへpublishする。同一文言の連続送信は抑止する。"""
205 """最新の処理段階を保持し、変化時に/coverage/phaseへ通知する。"""
207 changed = phase != self.
_phase
210 if changed
or phase ==
'IDLE':
216 """bridge内部の判定値とgoal段階を/diagnosticsへ1.0s周期で出す。"""
232 failures = self.
_retry.failures
233 retry_limit = self.
_retry.limit
234 status = DiagnosticStatus()
235 status.name =
'coverage/bridge'
236 status.hardware_id =
'coverage'
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
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,
251 f
'{occupied_ratio:.4f}' if occupied_ratio
is not None else 'unknown'),
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}',
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]
275 def _on_map(self, msg: OccupancyGrid) ->
None:
276 """事前検証用に最新の地図を保持する。"""
281 """ゾーン内占有率が上限超ならTrueを返す。地図未受信時は検証しない。"""
287 self.get_logger().warn(
288 '地図未受信のため占有検証を省略します',
289 throttle_duration_sec=10.0,
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,
298 if occ.inside_cells == 0:
301 self.get_logger().warn(
'ゾーンが地図範囲外のため占有検証を省略します')
309 self.get_logger().info(
310 f
'ゾーン内占有率: {occ.occupied_ratio:.3f} '
311 f
'(unknown: {occ.unknown_ratio:.3f}, 上限: {limit:.3f})'
313 return occ.occupied_ratio > limit
316 """受信ゾーンを検証し、Nav2へ最新の実行要求を送る。"""
324 self.
_set_phase(
'ZONE_RECEIVED',
'validating polygon')
325 min_vertices = self.
_int_param(
'min_vertices', 3)
327 zone = NormalizedZone.from_points(
329 points=[(p.x, p.y)
for p
in msg.polygon.points],
330 min_vertices=min_vertices,
332 except ValueError
as e:
334 self.get_logger().error(f
'不正なゾーンのため無視します: {e}')
339 self.
_set_phase(
'INVALID_ZONE',
'occupied ratio exceeds limit')
340 self.get_logger().error(
341 'ゾーン内に静的障害物が多いため無視します'
358 self.
_set_phase(
'ZONE_READY',
'polygon accepted for goal submission')
362 """実行中要求がなく再試行時刻を過ぎた場合、保留ゾーンを再送する。"""
367 if pending
is not None and not executing
and self.
_retry.ready(time.monotonic()):
370 def _make_goal(self, zone: NormalizedZone) -> NavigateCompleteCoverage.Goal:
371 """正規化済みゾーンからCoverageNavigatorのgoalを組み立てる。"""
372 goal = NavigateCompleteCoverage.Goal()
373 goal.frame_id = zone.frame_id
375 for x, y
in zone.points:
379 poly.points.append(pt)
384 pt.x, pt.y = zone.points[0][0], zone.points[0][1]
386 poly.points.append(pt)
387 goal.polygons.append(poly)
393 """現在のgoalを無効化し、受理済みならNav2へcancelを要求する。"""
399 if handle
is not None:
401 handle.cancel_goal_async()
402 self.get_logger().info(
'実行中ゴールをキャンセルしました')
403 except Exception
as e:
404 self.get_logger().warn(f
'ゴールキャンセル失敗: {e}')
407 """action serverを確認し、ゾーンgoalを非同期で送信する。
409 server不在の確認は非ブロッキングで行い、1.0s周期タイマーが再送する。
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,
417 self.
_set_phase(
'WAITING_NAV_SERVER',
'action server unavailable')
429 self.
_set_phase(
'GOAL_REQUESTED',
'waiting for action response')
432 feedback_callback=functools.partial(self.
_on_feedback, gen),
437 """指定世代が最新でなく、結果を破棄すべきならTrueを返す。"""
442 """最新goalの残距離とリカバリ回数を記録する。"""
445 remaining = msg.feedback.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,
457 """goal受理結果を処理し、受理時は結果コールバックを登録する。"""
461 handle = future.result()
462 except Exception
as e:
463 self.get_logger().error(f
'ゴール送信エラー: {e}')
467 goal_rejected=
False, reason=f
'goal send error: {e}')
469 if handle
is None or not handle.accepted:
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()
482 """拒否・実行失敗を計数し、上限到達で諦めてterminal状態を通知する。
484 受理時のリセットは行わない。受理後に失敗が続けば上限で諦める。
487 give_up = self.
_retry.record_failure(time.monotonic())
489 'Nav2 rejected goal' if goal_rejected
else 'Nav2 execution failed')
499 self.get_logger().error(
500 f
'Nav2にゴールを{count}回拒否されました。諦めます。'
501 '初期位置(2D Pose Estimate)・lifecycle(bt_navigator active)・'
502 'TF(map->base_footprint)を確認してください。'
506 self.get_logger().warn(
507 f
'Nav2にゴールを拒否されました({count}/{limit})。'
508 f
'{self._retry.backoff_sec:.0f}s後に再送します。',
509 throttle_duration_sec=10.0,
514 self.get_logger().error(
515 f
'Nav2実行が{self._retry.failures}回失敗しました。諦めます。'
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,
527 """最新goalの終了状態をDONE、CANCELED、失敗として通知する。"""
533 action_result = future.result()
534 status = action_result.status
535 error_code = action_result.result.error_code
536 except Exception
as e:
537 self.get_logger().error(f
'Nav2実行エラー: {e}')
539 goal_rejected=
False, reason=f
'Nav2 result error: {e}')
543 if status == GoalStatus.STATUS_SUCCEEDED:
544 self.
_set_phase(
'DONE',
'Nav2 action succeeded')
545 self.get_logger().info(
'清掃完了(DONE)')
550 elif status == GoalStatus.STATUS_CANCELED:
551 self.
_set_phase(
'CANCELED',
'Nav2 action canceled')
552 self.get_logger().info(
'清掃キャンセル(CANCELED)')
558 self.get_logger().error(
559 f
'清掃失敗(status={status}, error_code={error_code})')
562 reason=f
'Nav2 action status={status}, error_code={error_code}')
566 """coverage_bridgeを初期化し、ROS 2イベントを処理する。"""
567 rclpy.init(args=args)
571 except KeyboardInterrupt:
578if __name__ ==
'__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を返す。地図未受信時は検証しない。
float|None _zone_received_at
None _try_send(self, NormalizedZone zone)
action serverを確認し、ゾーンgoalを非同期で送信する。
None __init__(self)
topic、action client、再試行タイマーを初期化する。
float|None _unknown_ratio
float _float_param(self, str name, float default)
指定パラメータをfloatとして取得する。
OccupancyGrid|None _latest_map
int|None _last_nav_error_code
float|None _feedback_received_at
None _sync_retry_params(self)
再送上限と待機秒数 [s] をROSパラメータへ追従させる。
NormalizedZone|None _pending_zone
float|None _occupied_ratio
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を返す。
float|None _occupancy_limit
None _set_phase(self, str phase, str reason='')
最新の処理段階を保持し、変化時に/coverage/phaseへ通知する。
float|None _remaining_distance
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非依存で求める。