pub.py

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
import random

class GridPublisher(Node):
    def __init__(self):
        super().__init__('grid_publisher')
        # OccupancyGridメッセージ用のPublisherを作成
        self.publisher_ = self.create_publisher(OccupancyGrid, 'occupancy_grid', 10)
        # 1秒ごとにpublish_gridを呼び出すタイマー
        self.timer = self.create_timer(1.0, self.publish_grid)
        # グリッドサイズと解像度の設定
        self.width = 10
        self.height = 10
        self.resolution = 1.0

    def publish_grid(self):
        msg = OccupancyGrid()
        # ヘッダー情報
        msg.header.frame_id = 'map'
        msg.header.stamp = self.get_clock().now().to_msg()
        # マップ情報
        msg.info.resolution = self.resolution
        msg.info.width = self.width
        msg.info.height = self.height
        # 原点情報（ここでは(0,0)）
        msg.info.origin.position.x = 0.0
        msg.info.origin.position.y = 0.0
        msg.info.origin.position.z = 0.0
        msg.info.origin.orientation.w = 1.0
        # データ生成（0:空, 100:占有, -1:未知）
        msg.data = [random.choice([0, 100, -1]) for _ in range(self.width * self.height)]
        # メッセージをpublish
        self.publisher_.publish(msg)
        self.get_logger().info('OccupancyGridをpublishしました')


def main(args=None):
    rclpy.init(args=args)
    node = GridPublisher()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()