mirs
読み取り中…
検索中…
一致する文字列を見つけられません
trajectory_logic_test.py
[詳解]
1# Copyright 2026 mirs260x
2#
3# Permission is hereby granted, free of charge, to any person obtaining a copy
4# of this software and associated documentation files (the "Software"), to deal
5# in the Software without restriction, including without limitation the rights
6# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7# copies of the Software, and to permit persons to whom the Software is
8# furnished to do so, subject to the following conditions:
9#
10# The above copyright notice and this permission notice shall be included in
11# all copies or substantial portions of the Software.
12#
13# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19# THE SOFTWARE.
20
21
22"""
23ROSを起動せずに、走行軌跡を記録する処理を検証する。
24
25必要なROSモジュールを代替する最小限の定義を用意し、実際のスクリプトを読み込む。
26距離計算、5 cmごとの記録条件、位置が急変した場合の除外処理を確認する。
27"""
28import importlib.util
29from pathlib import Path
30import sys
31import types
32from unittest.mock import MagicMock
33
34
36 if 'rclpy' in sys.modules and hasattr(sys.modules['rclpy'], '_mirs_stub'):
37 return
38 rclpy = types.ModuleType('rclpy')
39 rclpy._mirs_stub = True
40 rclpy.init = lambda *a, **k: None
41 rclpy.spin = lambda *a, **k: None
42 rclpy.shutdown = lambda *a, **k: None
43
44 node_mod = types.ModuleType('rclpy.node')
45
46 class Node:
47 def __init__(self, name):
48 self._name = name
49 self._params = {'frame_id': 'odom'}
50
51 def declare_parameter(self, name, value):
52 self._params[name] = value
53
54 def get_parameter(self, name):
55 m = MagicMock()
56 m.value = self._params[name]
57 return m
58
59 def create_publisher(self, *a, **k):
60 m = MagicMock()
61 m.published = []
62 orig = m.publish
63 m.publish = lambda msg: (m.published.append(msg), orig(msg))
64 return m
65
66 def create_subscription(self, *a, **k):
67 return MagicMock()
68
69 def get_logger(self):
70 return MagicMock()
71
72 def destroy_node(self):
73 pass
74
75 node_mod.Node = Node
76 rclpy.node = node_mod
77
78 nav_msgs = types.ModuleType('nav_msgs')
79 nav_msgs_msg = types.ModuleType('nav_msgs.msg')
80
81 class _Pos:
82 def __init__(self, x=0.0, y=0.0):
83 self.x, self.y = x, y
84
85 class _Pose:
86 def __init__(self, x=0.0, y=0.0):
87 self.position = _Pos(x, y)
88
89 class Odometry:
90 pass
91
92 class Path:
93 def __init__(self):
94 self.header = MagicMock()
95 self.poses = []
96
97 class PoseStamped:
98 def __init__(self):
99 self.header = MagicMock()
100 self.pose = None
101
102 nav_msgs_msg.Odometry = Odometry
103 nav_msgs_msg.Path = Path
104 nav_msgs_msg.PoseStamped = PoseStamped
105
106 geom = types.ModuleType('geometry_msgs')
107 geom_msg = types.ModuleType('geometry_msgs.msg')
108 geom_msg.PoseStamped = PoseStamped
109
110 sys.modules['rclpy'] = rclpy
111 sys.modules['rclpy.node'] = node_mod
112 sys.modules['nav_msgs'] = nav_msgs
113 sys.modules['nav_msgs.msg'] = nav_msgs_msg
114 sys.modules['geometry_msgs'] = geom
115 sys.modules['geometry_msgs.msg'] = geom_msg
116
117
119
120
121SPEC = importlib.util.spec_from_file_location(
122 'trajectory_publisher',
123 Path(__file__).resolve().parents[1] / 'scripts' / 'trajectory_publisher.py')
124_mod = importlib.util.module_from_spec(SPEC)
125SPEC.loader.exec_module(_mod)
126
127
128def _odom(x, y):
129 msg = MagicMock()
130 msg.header.stamp = 't'
131 msg.pose.pose.position.x = x
132 msg.pose.pose.position.y = y
133 msg.pose.pose = msg.pose.pose
134 # 実コードは msg.pose.pose をそのまま保持するので参照を渡す
135 return msg
136
137
139 node = _mod.TrajectoryPublisher.__new__(_mod.TrajectoryPublisher)
140 a = MagicMock()
141 a.position.x, a.position.y = 0.0, 0.0
142 b = MagicMock()
143 b.position.x, b.position.y = 3.0, 4.0
144 assert node.calculate_distance(a, b) == 5.0
145
146
148 node = _mod.TrajectoryPublisher()
149 node.odom_callback(_odom(0, 0)) # 初回はlast_pose設定のみ
150 assert len(node.path_msg.poses) == 0
151 node.odom_callback(_odom(0.01, 0)) # 1cm: 記録しない
152 assert len(node.path_msg.poses) == 0
153 node.odom_callback(_odom(0.06, 0)) # 6cm: 記録
154 assert len(node.path_msg.poses) == 1
155
156
158 node = _mod.TrajectoryPublisher()
159 node.odom_callback(_odom(0, 0))
160 node.odom_callback(_odom(10.0, 0)) # ジャンプは無視
161 assert len(node.path_msg.poses) == 0
162 # last_poseが更新されていないこと (=次に近傍点が来たら記録される)
163 node.odom_callback(_odom(0.06, 0))
164 assert len(node.path_msg.poses) == 1