mirs
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
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
26
import
math
27
import
time
28
29
from
geometry_msgs.msg
import
Twist
30
from
nav_msgs.msg
import
Odometry
31
import
rclpy
32
from
rclpy.node
import
Node
33
from
tf_transformations
import
euler_from_quaternion
34
35
TIMER_PERIOD = 0.1
# [s] 制御周期
36
TARGET_ANGLE = 2.0 * math.pi
# [rad] 目標回転角(一回転)
37
P_GAIN = 0.5
# Pゲイン [1/s]
38
MAX_SPEED = 0.5
# [rad/s] 指令上限
39
MIN_SPEED = 0.1
# [rad/s] 指令下限
40
41
42
class
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
115
def
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
131
if
__name__ ==
'__main__'
:
132
main
()
rotate_test.OdomRotateTest
目標角度までの残りに応じて角速度を調整し、旋回走行を試す。
定義
rotate_test.py:42
rotate_test.OdomRotateTest.target_angle
target_angle
定義
rotate_test.py:57
rotate_test.OdomRotateTest.__init__
__init__(self)
定義
rotate_test.py:45
rotate_test.OdomRotateTest.subscription
subscription
定義
rotate_test.py:49
rotate_test.OdomRotateTest.last_yaw
last_yaw
定義
rotate_test.py:55
rotate_test.OdomRotateTest.odom_callback
odom_callback(self, msg)
定義
rotate_test.py:66
rotate_test.OdomRotateTest.is_moving
bool is_moving
定義
rotate_test.py:58
rotate_test.OdomRotateTest.timer
timer
定義
rotate_test.py:60
rotate_test.OdomRotateTest.accumulated_angle
float accumulated_angle
定義
rotate_test.py:56
rotate_test.OdomRotateTest.control_loop
control_loop(self)
定義
rotate_test.py:87
rotate_test.OdomRotateTest.publisher_
publisher_
定義
rotate_test.py:48
rotate_test.main
main(args=None)
定義
rotate_test.py:115
scripts
rotate_test.py
構築:
1.18.0