simulation
読み取り中…
検索中…
一致する文字列を見つけられません
home_pose_saver.py
[詳解]
1#!/usr/bin/env python3
2"""シミュレーションの位置推定が安定してから、機体の初期位置と向きを保存する。
3
4清掃走行のBehavior Treeは、この保存処理が完了してから走行を開始する。
5ほかの走行目標や手動操作も、保存が完了するまで待つ必要がある。
6"""
7
8from __future__ import annotations
9
10import math
11
12import rclpy
13from geometry_msgs.msg import PoseStamped
14from rclpy.node import Node
15from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
16from rclpy.time import Time
17from tf2_ros import Buffer, TransformException, TransformListener
18
19
20class HomePoseSaver(Node):
21 """機体が静止し、地図上の位置が安定した時点の姿勢を一度だけ保存する。後から接続した購読側にも配信する。"""
22
23 MAX_TF_AGE_SEC = 0.5
24 FUTURE_TF_TOLERANCE_SEC = 0.5 # Allow localization TF publication tolerance.
25 MAX_SPAWN_DRIFT_METERS = 0.05
26 MAX_SPAWN_DRIFT_RADIANS = 0.05
27 STABLE_POSE_DRIFT_METERS = 0.02
28 STABLE_POSE_DRIFT_RADIANS = 0.02
29 STABLE_FOR_SEC = 2.0
30
31 def __init__(self) -> None:
32 super().__init__('home_pose_saver')
33 self.declare_parameter('global_frame', 'map')
34 self.declare_parameter('robot_frame', 'base_footprint')
35 self.declare_parameter('odom_frame', 'odom')
36 self.declare_parameter('home_pose_topic', '/simulation/home_pose')
37 self.declare_parameter('publish_period_sec', 1.0)
38 self.declare_parameter('check_period_sec', 0.1)
39
40 self._global_frame = str(self.get_parameter('global_frame').value).strip('/')
41 self._robot_frame = str(self.get_parameter('robot_frame').value).strip('/')
42 self._odom_frame = str(self.get_parameter('odom_frame').value).strip('/')
43 topic = str(self.get_parameter('home_pose_topic').value)
44 publish_period = float(self.get_parameter('publish_period_sec').value)
45 check_period = float(self.get_parameter('check_period_sec').value)
46 if not self._global_frame or not self._robot_frame or not self._odom_frame or not topic:
47 raise ValueError('frame names and home_pose_topic must not be empty')
48 if not math.isfinite(publish_period) or publish_period <= 0.0:
49 raise ValueError('publish_period_sec must be positive and finite')
50 if not math.isfinite(check_period) or check_period <= 0.0:
51 raise ValueError('check_period_sec must be positive and finite')
52
53 qos = QoSProfile(
54 history=HistoryPolicy.KEEP_LAST,
55 depth=1,
56 reliability=ReliabilityPolicy.RELIABLE,
57 durability=DurabilityPolicy.TRANSIENT_LOCAL,
58 )
59 self._publisher = self.create_publisher(PoseStamped, topic, qos)
60 self._tf_buffer = Buffer()
61 self._tf_listener = TransformListener(self._tf_buffer, self)
62 self._home_pose: PoseStamped | None = None
63 self._odom_anchor: tuple[float, float, float] | None = None
64 self._stable_pose: tuple[float, float, float] | None = None
65 self._stable_since_ns: int | None = None
66 self._failed = False
67 self._check_timer = self.create_timer(check_period, self._check_pose)
68 self._publish_timer = self.create_timer(publish_period, self._publish_home_pose)
69 self.get_logger().info(
70 f'Waiting for spawn anchor TF {self._odom_frame} -> {self._robot_frame} '
71 f'and stable localization TF {self._global_frame} -> {self._odom_frame}; '
72 f'home pose topic is {topic}. Wait for capture before sending external motion commands.')
73
74 def _lookup_fresh(self, target: str, source: str, now_ns: int):
75 """コールバック内で待機せずに、現在取得できる座標変換を返す。"""
76 try:
77 transform = self._tf_buffer.lookup_transform(target, source, Time())
78 except TransformException:
79 return None
80
81 stamp_ns = (
82 transform.header.stamp.sec * 1_000_000_000 +
83 transform.header.stamp.nanosec)
84 age_ns = now_ns - stamp_ns
85 if (stamp_ns <= 0 or
86 age_ns > self.MAX_TF_AGE_SEC * 1_000_000_000 or
87 age_ns < -self.FUTURE_TF_TOLERANCE_SEC * 1_000_000_000):
88 return None
89 if transform.header.frame_id.lstrip('/') != target or \
90 transform.child_frame_id.lstrip('/') != source:
91 return None
92 if not self._valid_transform(transform):
93 return None
94 return transform
95
96 @staticmethod
97 def _valid_transform(transform) -> bool:
98 t = transform.transform.translation
99 q = transform.transform.rotation
100 values = (t.x, t.y, t.z, q.x, q.y, q.z, q.w)
101 norm = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w
102 return all(math.isfinite(value) for value in values) and abs(norm - 1.0) < 1e-3
103
104 @staticmethod
105 def _yaw(rotation) -> float:
106 sin_yaw = 2.0 * (rotation.w * rotation.z + rotation.x * rotation.y)
107 cos_yaw = 1.0 - 2.0 * (rotation.y * rotation.y + rotation.z * rotation.z)
108 return math.atan2(sin_yaw, cos_yaw)
109
110 @staticmethod
111 def _angle_distance(a: float, b: float) -> float:
112 return abs(math.atan2(math.sin(a - b), math.cos(a - b)))
113
114 @staticmethod
115 def _planar_pose(transform) -> tuple[float, float, float]:
116 translation = transform.transform.translation
117 return (translation.x, translation.y, HomePoseSaver._yaw(transform.transform.rotation))
118
119 @classmethod
120 def _pose_distance(cls, a: tuple[float, float, float],
121 b: tuple[float, float, float]) -> tuple[float, float]:
122 return (math.hypot(a[0] - b[0], a[1] - b[1]),
123 cls._angle_distance(a[2], b[2]))
124
125 def _fail_closed(self, reason: str) -> None:
126 self._failed = True
127 self.get_logger().error(
128 f'Home pose capture failed closed: {reason}; no pose will be published')
129
130 def _reset_stability(self) -> None:
131 self._stable_pose = None
132 self._stable_since_ns = None
133
134 def _check_pose(self) -> None:
135 if self._failed or self._home_pose is not None:
136 return
137
138 now_ns = self.get_clock().now().nanoseconds
139 # Simulation time must be running; freshness and the two-second hold
140 # are measured on the same clock as Gazebo.
141 if now_ns <= 0:
142 self._reset_stability()
143 return
144
145 odom_base = self._lookup_fresh(self._odom_frame, self._robot_frame, now_ns)
146 if odom_base is None:
147 self._reset_stability()
148 return
149 current_odom_pose = self._planar_pose(odom_base)
150 if self._odom_anchor is None:
151 self._odom_anchor = current_odom_pose
152 self.get_logger().info(
153 f'Established spawn odometry anchor in {self._odom_frame}: '
154 f'x={current_odom_pose[0]:.3f}, y={current_odom_pose[1]:.3f}, '
155 f'yaw={current_odom_pose[2]:.3f}')
156 else:
157 drift_m, drift_rad = self._pose_distance(self._odom_anchor, current_odom_pose)
158 if (drift_m > self.MAX_SPAWN_DRIFT_METERS or
159 drift_rad > self.MAX_SPAWN_DRIFT_RADIANS):
160 self._fail_closed(
161 f'robot moved {drift_m:.3f}m / {drift_rad:.3f}rad from its initial '
162 f'odometry anchor before localization capture')
163 return
164
165 map_odom = self._lookup_fresh(self._global_frame, self._odom_frame, now_ns)
166 map_base = self._lookup_fresh(self._global_frame, self._robot_frame, now_ns)
167 if map_odom is None or map_base is None:
168 self._reset_stability()
169 return
170
171 current_map_pose = self._planar_pose(map_base)
172 if self._stable_pose is None or self._stable_since_ns is None:
173 self._stable_pose = current_map_pose
174 self._stable_since_ns = now_ns
175 return
176 if now_ns < self._stable_since_ns:
177 self._stable_pose = current_map_pose
178 self._stable_since_ns = now_ns
179 return
180
181 stable_drift_m, stable_drift_rad = self._pose_distance(
182 self._stable_pose, current_map_pose)
183 if (stable_drift_m > self.STABLE_POSE_DRIFT_METERS or
184 stable_drift_rad > self.STABLE_POSE_DRIFT_RADIANS):
185 self._stable_pose = current_map_pose
186 self._stable_since_ns = now_ns
187 return
188
189 if now_ns - self._stable_since_ns < self.STABLE_FOR_SEC * 1_000_000_000:
190 return
191
192 pose = PoseStamped()
193 pose.header = map_base.header
194 pose.pose.position.x = map_base.transform.translation.x
195 pose.pose.position.y = map_base.transform.translation.y
196 pose.pose.position.z = map_base.transform.translation.z
197 pose.pose.orientation = map_base.transform.rotation
198 self._home_pose = pose
199 self.get_logger().info(
200 f'Captured stable stationary spawn pose in {self._global_frame}: '
201 f'x={pose.pose.position.x:.3f}, y={pose.pose.position.y:.3f}, '
202 f'yaw={self._yaw(pose.pose.orientation):.3f}; '
203 f'stationary and localized for {self.STABLE_FOR_SEC:.1f}s')
204
205 def _publish_home_pose(self) -> None:
206 if self._home_pose is None or self._failed:
207 return
208 # Re-publish the frozen value so the transient-local sample remains
209 # available to BT subscribers that start later.
210 self._publisher.publish(self._home_pose)
211
212
213def main(args=None) -> None:
214 rclpy.init(args=args)
215 node = HomePoseSaver()
216 try:
217 rclpy.spin(node)
218 finally:
219 node.destroy_node()
220 rclpy.shutdown()
221
222
223if __name__ == '__main__':
224 main()
機体が静止し、地図上の位置が安定した時点の姿勢を一度だけ保存する。後から接続した購読側にも配信する。
tuple[float, float, float]|None _stable_pose
tuple[float, float, float]|None _odom_anchor
float _angle_distance(float a, float b)
tuple[float, float] _pose_distance(cls, tuple[float, float, float] a, tuple[float, float, float] b)
_lookup_fresh(self, str target, str source, int now_ns)
コールバック内で待機せずに、現在取得できる座標変換を返す。
tuple[float, float, float] _planar_pose(transform)