|
| 1 | +# Copyright 2026 Open Source Robotics Foundation |
| 2 | +# |
| 3 | +# Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +# you may not use this file except in compliance with the License. |
| 5 | +# You may obtain a copy of the License at |
| 6 | +# |
| 7 | +# http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +# |
| 9 | +# Unless required by applicable law or agreed to in writing, software |
| 10 | +# distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +# See the License for the specific language governing permissions and |
| 13 | +# limitations under the License. |
| 14 | + |
| 15 | +from math import isfinite |
| 16 | + |
| 17 | +from geometry_msgs.msg import Point |
| 18 | +import rclpy |
| 19 | +from rclpy.duration import Duration |
| 20 | +from rclpy.node import Node |
| 21 | +from rclpy.qos import QoSProfile, ReliabilityPolicy |
| 22 | +from rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate |
| 23 | +from rmf_prototype_msgs.msg import Region |
| 24 | +from visualization_msgs.msg import Marker, MarkerArray |
| 25 | + |
| 26 | + |
| 27 | +SOURCE_COLORS = ( |
| 28 | + (0.96, 0.26, 0.21), |
| 29 | + (0.18, 0.80, 0.44), |
| 30 | + (0.20, 0.60, 0.98), |
| 31 | + (1.00, 0.65, 0.15), |
| 32 | + (0.61, 0.35, 0.71), |
| 33 | + (0.10, 0.74, 0.80), |
| 34 | +) |
| 35 | + |
| 36 | + |
| 37 | +def _point(x, y): |
| 38 | + point = Point() |
| 39 | + point.x = float(x) |
| 40 | + point.y = float(y) |
| 41 | + point.z = 0.05 |
| 42 | + return point |
| 43 | + |
| 44 | + |
| 45 | +def _marker_lifetime(patch, update, default_ttl_sec): |
| 46 | + for ttl_sec in ( |
| 47 | + patch.ttl_sec, |
| 48 | + update.source.default_ttl_sec, |
| 49 | + default_ttl_sec, |
| 50 | + ): |
| 51 | + if isfinite(ttl_sec) and ttl_sec > 0.0: |
| 52 | + return Duration(seconds=ttl_sec).to_msg() |
| 53 | + return Duration().to_msg() |
| 54 | + |
| 55 | + |
| 56 | +def _new_marker(update, patch, marker_id, marker_type, color, default_ttl_sec): |
| 57 | + marker = Marker() |
| 58 | + marker.header.frame_id = update.source.header.frame_id |
| 59 | + marker.ns = update.source.source_id |
| 60 | + marker.id = marker_id |
| 61 | + marker.type = marker_type |
| 62 | + marker.action = Marker.ADD |
| 63 | + marker.pose = update.source.robot_pose |
| 64 | + marker.frame_locked = False |
| 65 | + marker.lifetime = _marker_lifetime(patch, update, default_ttl_sec) |
| 66 | + marker.color.r = color[0] |
| 67 | + marker.color.g = color[1] |
| 68 | + marker.color.b = color[2] |
| 69 | + marker.color.a = ( |
| 70 | + 0.85 if patch.update_type == MapRegionPatch.UPDATE_OBSTACLE else 0.35 |
| 71 | + ) |
| 72 | + return marker |
| 73 | + |
| 74 | + |
| 75 | +def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): |
| 76 | + point_regions = [] |
| 77 | + rectangle_lines = [] |
| 78 | + |
| 79 | + for region in patch.regions: |
| 80 | + if region.hint == Region.HINT_POINT and len(region.points) == 2: |
| 81 | + point_regions.append(_point(region.points[0], region.points[1])) |
| 82 | + elif ( |
| 83 | + region.hint == Region.HINT_AXIS_ALIGNED_RECTANGLE |
| 84 | + and len(region.points) >= 4 |
| 85 | + and len(region.points) % 2 == 0 |
| 86 | + ): |
| 87 | + xs = region.points[0::2] |
| 88 | + ys = region.points[1::2] |
| 89 | + min_x, max_x = min(xs), max(xs) |
| 90 | + min_y, max_y = min(ys), max(ys) |
| 91 | + corners = ( |
| 92 | + _point(min_x, min_y), |
| 93 | + _point(max_x, min_y), |
| 94 | + _point(max_x, max_y), |
| 95 | + _point(min_x, max_y), |
| 96 | + ) |
| 97 | + rectangle_lines.extend(( |
| 98 | + corners[0], corners[1], |
| 99 | + corners[1], corners[2], |
| 100 | + corners[2], corners[3], |
| 101 | + corners[3], corners[0], |
| 102 | + )) |
| 103 | + |
| 104 | + markers = [] |
| 105 | + marker_id = first_id |
| 106 | + if point_regions: |
| 107 | + marker = _new_marker( |
| 108 | + update, |
| 109 | + patch, |
| 110 | + marker_id, |
| 111 | + Marker.POINTS, |
| 112 | + color, |
| 113 | + default_ttl_sec, |
| 114 | + ) |
| 115 | + marker.scale.x = 0.12 |
| 116 | + marker.scale.y = 0.12 |
| 117 | + marker.points = point_regions |
| 118 | + markers.append(marker) |
| 119 | + marker_id += 1 |
| 120 | + |
| 121 | + if rectangle_lines: |
| 122 | + marker = _new_marker( |
| 123 | + update, |
| 124 | + patch, |
| 125 | + marker_id, |
| 126 | + Marker.LINE_LIST, |
| 127 | + color, |
| 128 | + default_ttl_sec, |
| 129 | + ) |
| 130 | + marker.scale.x = 0.08 |
| 131 | + marker.points = rectangle_lines |
| 132 | + markers.append(marker) |
| 133 | + |
| 134 | + return markers |
| 135 | + |
| 136 | + |
| 137 | +class RegionMarkerState: |
| 138 | + """Convert region updates into persistent per-source RViz marker actions.""" |
| 139 | + |
| 140 | + def __init__(self, default_ttl_sec=30.0): |
| 141 | + self.default_ttl_sec = default_ttl_sec |
| 142 | + self.markers_by_source = {} |
| 143 | + self.next_ids = {} |
| 144 | + self.latest_stamps = {} |
| 145 | + self.source_colors = {} |
| 146 | + |
| 147 | + def apply_update(self, update): |
| 148 | + """Return marker actions that apply one region update to the RViz state.""" |
| 149 | + stamp_nsec = ( |
| 150 | + update.source.header.stamp.sec * 1_000_000_000 |
| 151 | + + update.source.header.stamp.nanosec |
| 152 | + ) |
| 153 | + if stamp_nsec == 0 or not update.source.header.frame_id: |
| 154 | + return MarkerArray() |
| 155 | + |
| 156 | + key = (update.source.source_id, update.source.map_name) |
| 157 | + latest_stamp = self.latest_stamps.get(key) |
| 158 | + if latest_stamp is not None and stamp_nsec < latest_stamp: |
| 159 | + return MarkerArray() |
| 160 | + |
| 161 | + color = self.source_colors.setdefault( |
| 162 | + key, |
| 163 | + SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)], |
| 164 | + ) |
| 165 | + marker_array = MarkerArray() |
| 166 | + self.markers_by_source[key] = [ |
| 167 | + marker |
| 168 | + for marker in self.markers_by_source.get(key, ()) |
| 169 | + if marker[3] is None or marker[3] > stamp_nsec |
| 170 | + ] |
| 171 | + |
| 172 | + if update.reset_source: |
| 173 | + for namespace, marker_id, frame_id, _ in self.markers_by_source[key]: |
| 174 | + marker = Marker() |
| 175 | + marker.header.frame_id = frame_id |
| 176 | + marker.ns = namespace |
| 177 | + marker.id = marker_id |
| 178 | + marker.action = Marker.DELETE |
| 179 | + marker_array.markers.append(marker) |
| 180 | + self.markers_by_source[key] = [] |
| 181 | + self.next_ids[key] = 0 |
| 182 | + |
| 183 | + next_id = self.next_ids.get(key, 0) |
| 184 | + new_markers = [] |
| 185 | + for patch in update.patches: |
| 186 | + if patch.update_type not in ( |
| 187 | + MapRegionPatch.UPDATE_CLEAR, |
| 188 | + MapRegionPatch.UPDATE_OBSTACLE, |
| 189 | + ): |
| 190 | + continue |
| 191 | + patch_markers = _markers_from_patch( |
| 192 | + update, |
| 193 | + patch, |
| 194 | + next_id, |
| 195 | + color, |
| 196 | + self.default_ttl_sec, |
| 197 | + ) |
| 198 | + new_markers.extend(patch_markers) |
| 199 | + next_id += len(patch_markers) |
| 200 | + |
| 201 | + marker_array.markers.extend(new_markers) |
| 202 | + self.next_ids[key] = next_id |
| 203 | + self.markers_by_source[key].extend( |
| 204 | + ( |
| 205 | + marker.ns, |
| 206 | + marker.id, |
| 207 | + marker.header.frame_id, |
| 208 | + stamp_nsec + marker.lifetime.sec * 1_000_000_000 |
| 209 | + + marker.lifetime.nanosec |
| 210 | + if marker.lifetime.sec > 0 or marker.lifetime.nanosec > 0 |
| 211 | + else None, |
| 212 | + ) |
| 213 | + for marker in new_markers |
| 214 | + ) |
| 215 | + |
| 216 | + if new_markers or update.reset_source: |
| 217 | + self.latest_stamps[key] = stamp_nsec |
| 218 | + |
| 219 | + return marker_array |
| 220 | + |
| 221 | + |
| 222 | +class RegionUpdateVisualizer(Node): |
| 223 | + """Visualize active map-region contributions as colored RViz markers.""" |
| 224 | + |
| 225 | + def __init__(self): |
| 226 | + super().__init__('region_update_visualizer') |
| 227 | + input_topic = self.declare_parameter( |
| 228 | + 'input_topic', '/map/region_updates' |
| 229 | + ).value |
| 230 | + output_topic = self.declare_parameter( |
| 231 | + 'output_topic', '/map/region_markers' |
| 232 | + ).value |
| 233 | + default_ttl_sec = self.declare_parameter( |
| 234 | + 'default_ttl_sec', 30.0 |
| 235 | + ).value |
| 236 | + |
| 237 | + reliable_qos = QoSProfile( |
| 238 | + depth=10, |
| 239 | + reliability=ReliabilityPolicy.RELIABLE, |
| 240 | + ) |
| 241 | + self.state = RegionMarkerState(default_ttl_sec) |
| 242 | + self.publisher = self.create_publisher( |
| 243 | + MarkerArray, |
| 244 | + output_topic, |
| 245 | + reliable_qos, |
| 246 | + ) |
| 247 | + self.subscription = self.create_subscription( |
| 248 | + MapRegionUpdate, |
| 249 | + input_topic, |
| 250 | + self.visualize_update, |
| 251 | + reliable_qos, |
| 252 | + ) |
| 253 | + self.get_logger().info( |
| 254 | + f'Visualizing {input_topic} as {output_topic}' |
| 255 | + ) |
| 256 | + |
| 257 | + def visualize_update(self, update): |
| 258 | + """Publish the marker actions for an incoming region update.""" |
| 259 | + marker_array = self.state.apply_update(update) |
| 260 | + if marker_array.markers: |
| 261 | + self.publisher.publish(marker_array) |
| 262 | + |
| 263 | + |
| 264 | +def main(): |
| 265 | + rclpy.init() |
| 266 | + node = RegionUpdateVisualizer() |
| 267 | + try: |
| 268 | + rclpy.spin(node) |
| 269 | + except KeyboardInterrupt: |
| 270 | + pass |
| 271 | + finally: |
| 272 | + node.destroy_node() |
| 273 | + rclpy.try_shutdown() |
0 commit comments