map_task1.py
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