83 lines
2.8 KiB
Python
83 lines
2.8 KiB
Python
#!/usr/bin/env python3
|
|
"""local_map_pub — publish a converted map on /map so RViz can preview it LOCALLY.
|
|
|
|
Used by the Pudu Map GUI's "RViz: preview converted map" button: no robot, no
|
|
rosbridge, no map_server needed — just the workstation's ROS (Jazzy) + the
|
|
converted map.yaml. Republishes every 2 s (works with RViz's default volatile
|
|
Map subscription regardless of durability settings).
|
|
|
|
python3 local_map_pub.py /path/to/map.yaml
|
|
"""
|
|
import os
|
|
import sys
|
|
|
|
import numpy as np
|
|
import rclpy
|
|
import yaml
|
|
from nav_msgs.msg import OccupancyGrid
|
|
from PIL import Image
|
|
from rclpy.node import Node
|
|
from rclpy.qos import DurabilityPolicy, QoSProfile
|
|
|
|
|
|
def load_grid(yaml_path):
|
|
with open(yaml_path) as f:
|
|
meta = yaml.safe_load(f)
|
|
img_path = meta["image"]
|
|
if not os.path.isabs(img_path):
|
|
img_path = os.path.join(os.path.dirname(os.path.abspath(yaml_path)), img_path)
|
|
gray = np.array(Image.open(img_path).convert("L")).astype(np.float32)
|
|
free_t = float(meta.get("free_thresh", 0.196))
|
|
occ_t = float(meta.get("occupied_thresh", 0.65))
|
|
if int(meta.get("negate", 0)):
|
|
p = gray / 255.0
|
|
else:
|
|
p = (255.0 - gray) / 255.0
|
|
occ = np.full(gray.shape, -1, dtype=np.int8)
|
|
occ[p <= free_t] = 0
|
|
occ[p >= occ_t] = 100
|
|
# pgm row 0 = top; OccupancyGrid row 0 = bottom -> flip vertically
|
|
occ = occ[::-1, :]
|
|
return meta, occ
|
|
|
|
|
|
class LocalMapPub(Node):
|
|
def __init__(self, yaml_path):
|
|
super().__init__("pudu_gui_local_map_pub")
|
|
meta, occ = load_grid(yaml_path)
|
|
self._msg = OccupancyGrid()
|
|
self._msg.header.frame_id = "map"
|
|
self._msg.info.resolution = float(meta["resolution"])
|
|
self._msg.info.height, self._msg.info.width = occ.shape
|
|
ox, oy = float(meta["origin"][0]), float(meta["origin"][1])
|
|
self._msg.info.origin.position.x = ox
|
|
self._msg.info.origin.position.y = oy
|
|
self._msg.info.origin.orientation.w = 1.0
|
|
self._msg.data = occ.reshape(-1).tolist()
|
|
qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
|
|
self._pub = self.create_publisher(OccupancyGrid, "/map", qos)
|
|
self.create_timer(2.0, self._tick)
|
|
self._tick()
|
|
self.get_logger().info(
|
|
f"publishing {self._msg.info.width}x{self._msg.info.height} @ "
|
|
f"{self._msg.info.resolution} m/px, origin ({ox}, {oy}) from {yaml_path}")
|
|
|
|
def _tick(self):
|
|
self._msg.header.stamp = self.get_clock().now().to_msg()
|
|
self._pub.publish(self._msg)
|
|
|
|
|
|
def main():
|
|
if len(sys.argv) < 2:
|
|
sys.exit("usage: local_map_pub.py /path/to/map.yaml")
|
|
rclpy.init()
|
|
node = LocalMapPub(sys.argv[1])
|
|
try:
|
|
rclpy.spin(node)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|