mirs
読み取り中…
検索中…
一致する文字列を見つけられません
rotate_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"""一回転して停止し、オドメトリの角度精度を確認する。"""
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
33from tf_transformations import euler_from_quaternion
34
35TIMER_PERIOD = 0.1 # [s] 制御周期
36TARGET_ANGLE = 2.0 * math.pi # [rad] 目標回転角(一回転)
37P_GAIN = 0.5 # Pゲイン [1/s]
38MAX_SPEED = 0.5 # [rad/s] 指令上限
39MIN_SPEED = 0.1 # [rad/s] 指令下限
40
41
42class OdomRotateTest(Node):
43 """目標角度までの残りに応じて角速度を調整し、旋回走行を試す。"""
44
45 def __init__(self):
46 super().__init__('odom_rotate_test')
47
48 self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
49 self.subscription = self.create_subscription(
50 Odometry,
51 '/odom',
52 self.odom_callback,
53 10)
54
55 self.last_yaw = None
56 self.accumulated_angle = 0.0 # [rad] 積算回転角
57 self.target_angle = TARGET_ANGLE # [rad]
58 self.is_moving = False
59
60 self.timer = self.create_timer(TIMER_PERIOD, self.control_loop)
61
62 self.get_logger().info('Odom Rotate Test Node Started')
63 self.get_logger().info(f'Target Angle: {math.degrees(self.target_angle):.1f} degrees')
64 self.get_logger().info('Waiting for /odom data...')
65
66 def odom_callback(self, msg):
67 orientation_q = msg.pose.pose.orientation
68 orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
69 (_roll, _pitch, current_yaw) = euler_from_quaternion(orientation_list)
70
71 if self.last_yaw is None:
72 self.last_yaw = current_yaw
73 self.get_logger().info(f'Start Yaw: {math.degrees(current_yaw):.3f} deg')
74 self.is_moving = True
75 return
76
77 # ±PI境界の跳びを補正して積算する
78 delta = current_yaw - self.last_yaw
79 if delta < -math.pi:
80 delta += 2 * math.pi
81 elif delta > math.pi:
82 delta -= 2 * math.pi
83
84 self.accumulated_angle += delta
85 self.last_yaw = current_yaw
86
87 def control_loop(self):
88 if not self.is_moving:
89 return
90
91 twist = Twist()
92
93 remaining_angle = self.target_angle - abs(self.accumulated_angle)
94
95 if remaining_angle <= 0:
96 twist.angular.z = 0.0
97 self.publisher_.publish(twist)
98 final_deg = math.degrees(self.accumulated_angle)
99 self.get_logger().info(f'Target Reached! Final Angle: {final_deg:.3f} deg')
100 self.is_moving = False
101 time.sleep(1.0)
102 raise SystemExit
103 else:
104 speed = P_GAIN * remaining_angle
105 speed = max(MIN_SPEED, min(MAX_SPEED, speed))
106
107 twist.angular.z = speed # 左回転(反時計回り)
108 self.publisher_.publish(twist)
109
110 current_deg = math.degrees(self.accumulated_angle)
111 self.get_logger().info(
112 f'Angle: {current_deg:.1f} / 360.0 deg (Speed: {speed:.3f} rad/s)')
113
114
115def main(args=None):
116 rclpy.init(args=args)
117 node = OdomRotateTest()
118
119 try:
120 rclpy.spin(node)
121 except SystemExit:
122 pass
123 except Exception as e:
124 print(e)
125 finally:
126 node.publisher_.publish(Twist())
127 node.destroy_node()
128 rclpy.shutdown()
129
130
131if __name__ == '__main__':
132 main()
目標角度までの残りに応じて角速度を調整し、旋回走行を試す。