simulation
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
home_pose_saver.py
[詳解]
1
#!/usr/bin/env python3
2
"""シミュレーションの位置推定が安定してから、機体の初期位置と向きを保存する。
3
4
清掃走行のBehavior Treeは、この保存処理が完了してから走行を開始する。
5
ほかの走行目標や手動操作も、保存が完了するまで待つ必要がある。
6
"""
7
8
from
__future__
import
annotations
9
10
import
math
11
12
import
rclpy
13
from
geometry_msgs.msg
import
PoseStamped
14
from
rclpy.node
import
Node
15
from
rclpy.qos
import
DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
16
from
rclpy.time
import
Time
17
from
tf2_ros
import
Buffer, TransformException, TransformListener
18
19
20
class
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
213
def
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
223
if
__name__ ==
'__main__'
:
224
main
()
home_pose_saver.HomePoseSaver
機体が静止し、地図上の位置が安定した時点の姿勢を一度だけ保存する。後から接続した購読側にも配信する。
定義
home_pose_saver.py:20
home_pose_saver.HomePoseSaver._global_frame
_global_frame
定義
home_pose_saver.py:40
home_pose_saver.HomePoseSaver._publisher
_publisher
定義
home_pose_saver.py:59
home_pose_saver.HomePoseSaver._tf_listener
_tf_listener
定義
home_pose_saver.py:61
home_pose_saver.HomePoseSaver.STABLE_POSE_DRIFT_METERS
float STABLE_POSE_DRIFT_METERS
定義
home_pose_saver.py:27
home_pose_saver.HomePoseSaver._stable_since_ns
int|None _stable_since_ns
定義
home_pose_saver.py:65
home_pose_saver.HomePoseSaver._publish_home_pose
None _publish_home_pose(self)
定義
home_pose_saver.py:205
home_pose_saver.HomePoseSaver.STABLE_FOR_SEC
float STABLE_FOR_SEC
定義
home_pose_saver.py:29
home_pose_saver.HomePoseSaver._stable_pose
tuple[float, float, float]|None _stable_pose
定義
home_pose_saver.py:64
home_pose_saver.HomePoseSaver._failed
bool _failed
定義
home_pose_saver.py:66
home_pose_saver.HomePoseSaver._reset_stability
None _reset_stability(self)
定義
home_pose_saver.py:130
home_pose_saver.HomePoseSaver.MAX_SPAWN_DRIFT_METERS
float MAX_SPAWN_DRIFT_METERS
定義
home_pose_saver.py:25
home_pose_saver.HomePoseSaver._tf_buffer
_tf_buffer
定義
home_pose_saver.py:60
home_pose_saver.HomePoseSaver._publish_timer
_publish_timer
定義
home_pose_saver.py:68
home_pose_saver.HomePoseSaver._robot_frame
_robot_frame
定義
home_pose_saver.py:41
home_pose_saver.HomePoseSaver._valid_transform
bool _valid_transform(transform)
定義
home_pose_saver.py:97
home_pose_saver.HomePoseSaver._odom_anchor
tuple[float, float, float]|None _odom_anchor
定義
home_pose_saver.py:63
home_pose_saver.HomePoseSaver._angle_distance
float _angle_distance(float a, float b)
定義
home_pose_saver.py:111
home_pose_saver.HomePoseSaver._pose_distance
tuple[float, float] _pose_distance(cls, tuple[float, float, float] a, tuple[float, float, float] b)
定義
home_pose_saver.py:121
home_pose_saver.HomePoseSaver._home_pose
PoseStamped|None _home_pose
定義
home_pose_saver.py:62
home_pose_saver.HomePoseSaver.FUTURE_TF_TOLERANCE_SEC
float FUTURE_TF_TOLERANCE_SEC
定義
home_pose_saver.py:24
home_pose_saver.HomePoseSaver._odom_frame
_odom_frame
定義
home_pose_saver.py:42
home_pose_saver.HomePoseSaver._lookup_fresh
_lookup_fresh(self, str target, str source, int now_ns)
コールバック内で待機せずに、現在取得できる座標変換を返す。
定義
home_pose_saver.py:74
home_pose_saver.HomePoseSaver._planar_pose
tuple[float, float, float] _planar_pose(transform)
定義
home_pose_saver.py:115
home_pose_saver.HomePoseSaver.STABLE_POSE_DRIFT_RADIANS
float STABLE_POSE_DRIFT_RADIANS
定義
home_pose_saver.py:28
home_pose_saver.HomePoseSaver._check_timer
_check_timer
定義
home_pose_saver.py:67
home_pose_saver.HomePoseSaver._yaw
float _yaw(rotation)
定義
home_pose_saver.py:105
home_pose_saver.HomePoseSaver.MAX_SPAWN_DRIFT_RADIANS
float MAX_SPAWN_DRIFT_RADIANS
定義
home_pose_saver.py:26
home_pose_saver.HomePoseSaver.__init__
None __init__(self)
定義
home_pose_saver.py:31
home_pose_saver.HomePoseSaver._check_pose
None _check_pose(self)
定義
home_pose_saver.py:134
home_pose_saver.HomePoseSaver._fail_closed
None _fail_closed(self, str reason)
定義
home_pose_saver.py:125
home_pose_saver.HomePoseSaver.MAX_TF_AGE_SEC
float MAX_TF_AGE_SEC
定義
home_pose_saver.py:23
home_pose_saver.main
None main(args=None)
定義
home_pose_saver.py:213
scripts
home_pose_saver.py
構築:
1.18.0