mirs
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
trajectory_publisher.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
"""走行した軌跡をnav_msgs/Pathメッセージとして配信する。"""
25
from
geometry_msgs.msg
import
PoseStamped
26
from
nav_msgs.msg
import
Odometry, Path
27
import
rclpy
28
from
rclpy.node
import
Node
29
30
31
class
TrajectoryPublisher
(Node):
32
"""/odomの位置と向きを、一定の移動距離ごとに記録する。"""
33
34
def
__init__
(self):
35
super().
__init__
(
'trajectory_publisher'
)
36
37
self.declare_parameter(
'frame_id'
,
'odom'
)
38
self.declare_parameter(
'min_distance'
, 0.05)
# [m] この距離以上動いたら記録
39
self.declare_parameter(
'max_jump'
, 0.5)
# [m] 一気に飛んだらノイズとみなして無視
40
self.declare_parameter(
'max_poses'
, 10000)
# 軌跡の保持上限
41
self.
frame_id
= self.get_parameter(
'frame_id'
).value
42
43
self.
path_pub
= self.create_publisher(Path,
'/traveled_path'
, 10)
44
self.
odom_sub
= self.create_subscription(Odometry,
'/odom'
, self.
odom_callback
, 10)
45
46
self.
path_msg
= Path()
47
self.
path_msg
.header.frame_id = self.
frame_id
48
49
self.
last_pose
=
None
50
self.
min_distance
= self.get_parameter(
'min_distance'
).value
51
self.
max_jump
= self.get_parameter(
'max_jump'
).value
52
self.
max_poses
= self.get_parameter(
'max_poses'
).value
53
54
def
odom_callback
(self, msg):
55
current_pose = msg.pose.pose
56
57
if
self.
last_pose
is
None
:
58
self.
last_pose
= current_pose
59
return
60
61
dist = self.
calculate_distance
(self.
last_pose
, current_pose)
62
63
# 異常値フィルタ: ノイズとみなして無視
64
if
dist > self.
max_jump
:
65
return
66
67
# 一定距離動いたら記録
68
if
dist > self.
min_distance
:
69
pose_stamped = PoseStamped()
70
pose_stamped.header = msg.header
71
pose_stamped.header.frame_id = self.
frame_id
72
pose_stamped.pose = current_pose
73
74
self.
path_msg
.poses.append(pose_stamped)
75
if
len(self.
path_msg
.poses) > self.
max_poses
:
76
del self.
path_msg
.poses[0:len(self.
path_msg
.poses) - self.
max_poses
]
77
self.
path_msg
.header.stamp = msg.header.stamp
78
79
self.
path_pub
.publish(self.
path_msg
)
80
self.
last_pose
= current_pose
81
82
def
calculate_distance
(self, pose1, pose2):
83
dx = pose1.position.x - pose2.position.x
84
dy = pose1.position.y - pose2.position.y
85
return
(dx**2 + dy**2)**0.5
86
87
88
def
main
(args=None):
89
rclpy.init(args=args)
90
node =
TrajectoryPublisher
()
91
try
:
92
rclpy.spin(node)
93
except
KeyboardInterrupt:
94
pass
95
finally
:
96
node.destroy_node()
97
rclpy.shutdown()
98
99
100
if
__name__ ==
'__main__'
:
101
main
()
trajectory_publisher.TrajectoryPublisher
/odomの位置と向きを、一定の移動距離ごとに記録する。
定義
trajectory_publisher.py:31
trajectory_publisher.TrajectoryPublisher.max_jump
max_jump
定義
trajectory_publisher.py:51
trajectory_publisher.TrajectoryPublisher.calculate_distance
calculate_distance(self, pose1, pose2)
定義
trajectory_publisher.py:82
trajectory_publisher.TrajectoryPublisher.path_msg
path_msg
定義
trajectory_publisher.py:46
trajectory_publisher.TrajectoryPublisher.last_pose
last_pose
定義
trajectory_publisher.py:49
trajectory_publisher.TrajectoryPublisher.path_pub
path_pub
定義
trajectory_publisher.py:43
trajectory_publisher.TrajectoryPublisher.__init__
__init__(self)
定義
trajectory_publisher.py:34
trajectory_publisher.TrajectoryPublisher.frame_id
frame_id
定義
trajectory_publisher.py:41
trajectory_publisher.TrajectoryPublisher.min_distance
min_distance
定義
trajectory_publisher.py:50
trajectory_publisher.TrajectoryPublisher.odom_callback
odom_callback(self, msg)
定義
trajectory_publisher.py:54
trajectory_publisher.TrajectoryPublisher.max_poses
max_poses
定義
trajectory_publisher.py:52
trajectory_publisher.TrajectoryPublisher.odom_sub
odom_sub
定義
trajectory_publisher.py:44
trajectory_publisher.main
main(args=None)
定義
trajectory_publisher.py:88
scripts
trajectory_publisher.py
構築:
1.18.0