mirs
読み取り中…
検索中…
一致する文字列を見つけられません
linear_test.py
[詳解]
1#!/usr/bin/env python3
2
3# Copyright 2026 mirs260x
4#
5# Permission is hereby granted, free of charge, to any person obtaining a copy
6# of this software and associated documentation files (the "Software"), to deal
7# in the Software without restriction, including without limitation the rights
8# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9# copies of the Software, and to permit persons to whom the Software is
10# furnished to do so, subject to the following conditions:
11#
12# The above copyright notice and this permission notice shall be included in
13# all copies or substantial portions of the Software.
14#
15# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
18# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21# THE SOFTWARE.
22
23
24"""3 m直進して停止し、オドメトリの距離精度を確認する。"""
25
26import math
27import time
28
29from geometry_msgs.msg import Twist
30from nav_msgs.msg import Odometry
31import rclpy
32from rclpy.node import Node
33
34TIMER_PERIOD = 0.1 # [s] 制御周期
35TARGET_DISTANCE = 3.0 # [m] 目標移動距離
36P_GAIN = 0.5 # Pゲイン [1/s]
37MAX_SPEED = 0.3 # [m/s] 指令上限
38MIN_SPEED = 0.05 # [m/s] 指令下限(静摩擦に勝つため)
39
40
41class OdomLinearTest(Node):
42 """目標距離までの残りに応じて速度を調整し、直進走行を試す。"""
43
44 def __init__(self):
45 super().__init__('odom_linear_test')
46
47 self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
48 self.subscription = self.create_subscription(
49 Odometry,
50 '/odom',
51 self.odom_callback,
52 10)
53
54 self.start_x = None
55 self.start_y = None
56 self.current_distance = 0.0 # [m] 起点からの移動距離
57 self.target_distance = TARGET_DISTANCE # [m]
58 self.is_moving = False
59
60 self.timer = self.create_timer(TIMER_PERIOD, self.control_loop)
61
62 self.get_logger().info('Odom Linear Test Node Started')
63 self.get_logger().info(f'Target Distance: {self.target_distance} meters')
64 self.get_logger().info('Waiting for /odom data...')
65
66 def odom_callback(self, msg):
67 x = msg.pose.pose.position.x
68 y = msg.pose.pose.position.y
69
70 if self.start_x is None:
71 self.start_x = x
72 self.start_y = y
73 self.get_logger().info(f'Start Position: x={x:.3f}, y={y:.3f}')
74 self.is_moving = True
75 return
76
77 dx = x - self.start_x
78 dy = y - self.start_y
79 self.current_distance = math.sqrt(dx*dx + dy*dy)
80
81 def control_loop(self):
82 if not self.is_moving:
83 return
84
85 twist = Twist()
86
87 remaining_distance = self.target_distance - self.current_distance
88
89 if remaining_distance <= 0:
90 twist.linear.x = 0.0
91 self.publisher_.publish(twist)
92 self.get_logger().info(
93 f'Target Reached! Final Distance: {self.current_distance:.3f} m')
94 self.is_moving = False
95 time.sleep(1.0)
96 raise SystemExit
97 else:
98 speed = P_GAIN * remaining_distance
99 speed = max(MIN_SPEED, min(MAX_SPEED, speed))
100
101 twist.linear.x = speed
102 self.publisher_.publish(twist)
103
104 self.get_logger().info(
105 f'Distance: {self.current_distance:.3f} m / '
106 f'{self.target_distance:.1f} m (Speed: {speed:.3f} m/s)')
107
108
109def main(args=None):
110 rclpy.init(args=args)
111 node = OdomLinearTest()
112
113 try:
114 rclpy.spin(node)
115 except SystemExit:
116 pass
117 except Exception as e:
118 print(e)
119 finally:
120 # 停止コマンドを送って終了する
121 node.publisher_.publish(Twist())
122 node.destroy_node()
123 rclpy.shutdown()
124
125
126if __name__ == '__main__':
127 main()
目標距離までの残りに応じて速度を調整し、直進走行を試す。