mirs
読み取り中…
検索中…
一致する文字列を見つけられません
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メッセージとして配信する。"""
25from geometry_msgs.msg import PoseStamped
26from nav_msgs.msg import Odometry, Path
27import rclpy
28from rclpy.node import Node
29
30
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
88def 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
100if __name__ == '__main__':
101 main()
/odomの位置と向きを、一定の移動距離ごとに記録する。