map_task1.py

 avatar
unknown
plain_text
a year ago
2.7 kB
3
Indexable
import rclpy
from rclpy.node import Node
from rclpy.qos import (
    QoSProfile,
    QoSReliabilityPolicy,
    QoSHistoryPolicy,
    QoSDurabilityPolicy
)
from nav_msgs.msg import OccupancyGrid, MapMetaData
from random import randint
import tf2_ros
from geometry_msgs.msg import TransformStamped

class MapPublisher(Node):
    def __init__(self):
        super().__init__('map_publisher')
        qos_profile = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10  # Keeps last 10 messages
        )
        self.publisher = self.create_publisher(OccupancyGrid, '/map', qos_profile)
        timer_period = 2.0  # seconds (0.5 Hz frequency)
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.tf_broadcaster = tf2_ros.StaticTransformBroadcaster(self)
        self.send_transform()

    def send_transform(self):
        transform = TransformStamped()
        transform.header.stamp = self.get_clock().now().to_msg()
        transform.header.frame_id = 'odom'
        transform.child_frame_id = 'map'
        transform.transform.translation.x = 0.0
        transform.transform.translation.y = 0.0
        transform.transform.translation.z = 0.0
        transform.transform.rotation.x = 0.0
        transform.transform.rotation.y = 0.0
        transform.transform.rotation.z = 0.0
        transform.transform.rotation.w = 1.0
        self.tf_broadcaster.sendTransform(transform)

    def timer_callback(self):
        width, height = 20, 20
        grid = [0] * (width * height)
        for _ in range(int((width*height)*0.15)):  # Populate random cells with obstacles 15%
            index = randint(0, width * height - 1)
            grid[index] = 100

        # Setting up the map metadata
        meta = MapMetaData()
        meta.map_load_time = self.get_clock().now().to_msg()
        meta.resolution = 0.5  # 5m per cell
        meta.width = width
        meta.height = height
        meta.origin.position.x = -0.5 * meta.width * meta.resolution #Center
        meta.origin.position.y = -0.5 * meta.height * meta.resolution

        msg = OccupancyGrid()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = "map"
        msg.info = meta
        msg.data = grid
        self.publisher.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    map_publisher = MapPublisher()
    rclpy.spin(map_publisher)
    map_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
Editor is loading...
Leave a Comment