63 lines
2.1 KiB
Python
63 lines
2.1 KiB
Python
#!/usr/bin/env python3
|
|
"""One-shot /map -> pgm+yaml exporter. Run INSIDE the robot's VSLAM/nav
|
|
container (read-only w.r.t. the robot; subscribes once, writes /data, exits).
|
|
|
|
source /opt/ros/foxy/setup.bash && timeout 30 python3 map_export_once.py
|
|
"""
|
|
import math
|
|
import sys
|
|
|
|
import rclpy
|
|
from nav_msgs.msg import OccupancyGrid
|
|
from rclpy.node import Node
|
|
from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy
|
|
|
|
OUT = "/data/map_export"
|
|
|
|
|
|
class Grab(Node):
|
|
def __init__(self):
|
|
super().__init__("map_export_once")
|
|
qos = QoSProfile(depth=1,
|
|
reliability=QoSReliabilityPolicy.RELIABLE,
|
|
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
|
|
self.create_subscription(OccupancyGrid, "/map", self.cb, qos)
|
|
self.done = False
|
|
|
|
def cb(self, msg):
|
|
if self.done:
|
|
return
|
|
self.done = True
|
|
w, h, res = msg.info.width, msg.info.height, msg.info.resolution
|
|
ox, oy = msg.info.origin.position.x, msg.info.origin.position.y
|
|
q = msg.info.origin.orientation
|
|
yaw = math.atan2(2 * (q.w * q.z + q.x * q.y),
|
|
1 - 2 * (q.y * q.y + q.z * q.z))
|
|
d = msg.data
|
|
# map_saver convention: unknown(-1)->205, free->254, occupied->0
|
|
px = bytearray(w * h)
|
|
for i in range(w * h):
|
|
v = d[i]
|
|
px[i] = 205 if v < 0 else (0 if v >= 65 else 254)
|
|
with open(OUT + ".pgm", "wb") as f:
|
|
f.write(b"P5\n%d %d\n255\n" % (w, h))
|
|
for y in range(h - 1, -1, -1): # grid row 0 = bottom -> flip
|
|
f.write(bytes(px[y * w:(y + 1) * w]))
|
|
with open(OUT + ".yaml", "w") as f:
|
|
f.write("image: map_export.pgm\nresolution: %.6f\n"
|
|
"origin: [%.6f, %.6f, %.6f]\nnegate: 0\n"
|
|
"occupied_thresh: 0.65\nfree_thresh: 0.196\n"
|
|
% (res, ox, oy, yaw))
|
|
print("EXPORTED %dx%d @ %.3fm origin=(%.2f, %.2f, %.2f)"
|
|
% (w, h, res, ox, oy, yaw), flush=True)
|
|
rclpy.shutdown()
|
|
|
|
|
|
def main():
|
|
rclpy.init()
|
|
rclpy.spin(Grab())
|
|
|
|
|
|
if __name__ == "__main__":
|
|
sys.exit(main())
|