mirs
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
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
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
34
TIMER_PERIOD = 0.1
# [s] 制御周期
35
TARGET_DISTANCE = 3.0
# [m] 目標移動距離
36
P_GAIN = 0.5
# Pゲイン [1/s]
37
MAX_SPEED = 0.3
# [m/s] 指令上限
38
MIN_SPEED = 0.05
# [m/s] 指令下限(静摩擦に勝つため)
39
40
41
class
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
109
def
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
126
if
__name__ ==
'__main__'
:
127
main
()
linear_test.OdomLinearTest
目標距離までの残りに応じて速度を調整し、直進走行を試す。
定義
linear_test.py:41
linear_test.OdomLinearTest.target_distance
target_distance
定義
linear_test.py:57
linear_test.OdomLinearTest.start_x
start_x
定義
linear_test.py:54
linear_test.OdomLinearTest.publisher_
publisher_
定義
linear_test.py:47
linear_test.OdomLinearTest.current_distance
float current_distance
定義
linear_test.py:56
linear_test.OdomLinearTest.subscription
subscription
定義
linear_test.py:48
linear_test.OdomLinearTest.start_y
start_y
定義
linear_test.py:55
linear_test.OdomLinearTest.timer
timer
定義
linear_test.py:60
linear_test.OdomLinearTest.__init__
__init__(self)
定義
linear_test.py:44
linear_test.OdomLinearTest.is_moving
bool is_moving
定義
linear_test.py:58
linear_test.OdomLinearTest.control_loop
control_loop(self)
定義
linear_test.py:81
linear_test.OdomLinearTest.odom_callback
odom_callback(self, msg)
定義
linear_test.py:66
linear_test.main
main(args=None)
定義
linear_test.py:109
scripts
linear_test.py
構築:
1.18.0