67 lines
1.9 KiB
Python
Executable File
67 lines
1.9 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
"""Publish initial pose for AMCL localization."""
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from geometry_msgs.msg import PoseWithCovarianceStamped
|
|
|
|
|
|
class InitialPosePublisher(Node):
|
|
def __init__(self):
|
|
super().__init__('initial_pose_publisher')
|
|
self.publisher = self.create_publisher(
|
|
PoseWithCovarianceStamped,
|
|
'/initialpose',
|
|
10
|
|
)
|
|
self.timer = self.create_timer(1.0, self.publish_pose)
|
|
self.get_logger().info('Initial pose publisher started')
|
|
self.published = False
|
|
|
|
def publish_pose(self):
|
|
if self.published:
|
|
return
|
|
|
|
msg = PoseWithCovarianceStamped()
|
|
msg.header.stamp.sec = 0
|
|
msg.header.stamp.nanosec = 0
|
|
msg.header.frame_id = 'map'
|
|
msg.pose.pose.position.x = 0.0
|
|
msg.pose.pose.position.y = 0.0
|
|
msg.pose.pose.position.z = 0.0
|
|
msg.pose.pose.orientation.x = 0.0
|
|
msg.pose.pose.orientation.y = 0.0
|
|
msg.pose.pose.orientation.z = 0.0
|
|
msg.pose.pose.orientation.w = 1.0
|
|
msg.pose.covariance = [
|
|
0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
|
|
0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
|
|
]
|
|
|
|
self.publisher.publish(msg)
|
|
self.get_logger().info('Published initial pose (0, 0)')
|
|
self.published = True
|
|
# Shutdown after publishing
|
|
self.get_logger().info('Shutting down initial pose publisher')
|
|
raise rclpy.shutdown()
|
|
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
try:
|
|
node = InitialPosePublisher()
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
if rclpy.ok():
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|