Ground Truth Map Module

gt_map.generator

Ground truth map generation from Gazebo SDF files.

This module parses SDF (Simulation Description Format) files and generates 2D occupancy grid maps by projecting 3D geometry onto a horizontal plane at a specified laser height.

generate_map(sdf_path='world/model.sdf', resolution=0.05, laser_z=0.17, padding=1.0, output_name='map_gt', gen_png=True, gen_debug=True)

Generate a 2D occupancy grid map from a Gazebo SDF file.

Parses the SDF file, extracts 3D geometry (boxes, cylinders), and projects them onto a 2D plane at the specified laser height. Outputs PGM and YAML files compatible with ROS 2 map_server.

Parameters:

Name	Type	Description	Default
sdf_path		Path to the SDF file	'world/model.sdf'
resolution		Map resolution in meters per pixel (default: 0.05)	0.05
laser_z		Height of the laser scanner in meters (default: 0.17)	0.17
padding		Extra padding around the map bounds in meters (default: 1.0)	1.0
output_name		Base name for output files (default: 'map_gt')	'map_gt'
gen_png		Whether to generate a PNG preview (default: True)	True
gen_debug		Whether to generate a debug plot (default: True)	True

Returns:

Type	Description
	Tuple of (success: bool, message: str)

Example

success, msg = generate_map('worlds/turtlebot3_world.sdf', output_name='maps/gt/tb3_world') if success: ... print("Map generated successfully")

Source code in gt_map/generator.py

def generate_map(sdf_path='world/model.sdf', resolution=0.05, laser_z=0.17, padding=1.0, output_name='map_gt', gen_png=True, gen_debug=True):
    """Generate a 2D occupancy grid map from a Gazebo SDF file.

    Parses the SDF file, extracts 3D geometry (boxes, cylinders), and projects
    them onto a 2D plane at the specified laser height. Outputs PGM and YAML files
    compatible with ROS 2 map_server.

    Args:
        sdf_path: Path to the SDF file
        resolution: Map resolution in meters per pixel (default: 0.05)
        laser_z: Height of the laser scanner in meters (default: 0.17)
        padding: Extra padding around the map bounds in meters (default: 1.0)
        output_name: Base name for output files (default: 'map_gt')
        gen_png: Whether to generate a PNG preview (default: True)
        gen_debug: Whether to generate a debug plot (default: True)

    Returns:
        Tuple of (success: bool, message: str)

    Example:
        >>> success, msg = generate_map('worlds/turtlebot3_world.sdf', output_name='maps/gt/tb3_world')
        >>> if success:
        ...     print("Map generated successfully")
    """
    print(f"DEBUG: generate_map called for {sdf_path}")
    if not os.path.exists(sdf_path):
        return False, f"Error: {sdf_path} not found."

    try:
        print("DEBUG: Parsing XML...")
        tree = ET.parse(sdf_path)
        root = tree.getroot()
        print("DEBUG: XML Parsed. Finding top model...")

        # Determine if root is 'sdf' or 'model'
        if root.tag == 'sdf':
             top_model = root.find('model')
             if root.find('world'):
                 # It's a world file, look for models in world?
                 # For now, let's assume one main model or panic.
                 print(f"DEBUG: Root is SDF. First model found: {top_model is not None}")
        elif root.tag == 'model':
             top_model = root
        else:
             top_model = None

        if top_model is None:
            return False, f"Could not find <model> in {sdf_path}"

        print(f"DEBUG: Processing top model: {top_model.get('name')}")
        projected_obstacles = process_model_detailed(top_model, np.eye(4), laser_z)

        if not projected_obstacles:
            return False, "No obstacles found at the specified laser height."

        # Calculate bounds
        all_x = []
        all_y = []
        for obs in projected_obstacles:
            if obs['type'] == 'box':
                d = math.sqrt(obs['w']**2 + obs['h']**2) / 2
                all_x.extend([obs['x'] - d, obs['x'] + d])
                all_y.extend([obs['y'] - d, obs['y'] + d])
            elif obs['type'] == 'cylinder':
                all_x.extend([obs['x'] - obs['r'], obs['x'] + obs['r']])
                all_y.extend([obs['y'] - obs['r'], obs['y'] + obs['r']])

        min_x = min(all_x) - padding
        max_x = max(all_x) + padding
        min_y = min(all_y) - padding
        max_y = max(all_y) + padding

        width = int((max_x - min_x) / resolution)
        height = int((max_y - min_y) / resolution)

        grid = np.full((height, width), 255, dtype=np.uint8)

        for obs in projected_obstacles:
            if obs['type'] == 'box':
                hw, hh = obs['w']/2, obs['h']/2
                yaw = obs['yaw']
                cos_y = math.cos(-yaw)
                sin_y = math.sin(-yaw)
                d = math.sqrt(hw**2 + hh**2)
                min_px = int((obs['x'] - d - min_x) / resolution)
                max_px = int((obs['x'] + d - min_x) / resolution)
                min_py = int((obs['y'] - d - min_y) / resolution)
                max_py = int((obs['y'] + d - min_y) / resolution)
                for y_idx in range(max(0, min_py), min(height, max_py + 1)):
                    for x_idx in range(max(0, min_px), min(width, max_px + 1)):
                        lx = (x_idx * resolution + min_x) - obs['x']
                        ly = (y_idx * resolution + min_y) - obs['y']
                        ux = lx * cos_y - ly * sin_y
                        uy = lx * sin_y + ly * cos_y
                        if -hw <= ux <= hw and -hh <= uy <= hh:
                            grid[y_idx, x_idx] = 0
            elif obs['type'] == 'cylinder':
                cx = int((obs['x'] - min_x) / resolution)
                cy = int((obs['y'] - min_y) / resolution)
                r_px = int(obs['r'] / resolution)
                for y_idx in range(max(0, cy - r_px - 1), min(height, cy + r_px + 2)):
                    for x_idx in range(max(0, cx - r_px - 1), min(width, cx + r_px + 2)):
                        dx = (x_idx * resolution + min_x) - obs['x']
                        dy = (y_idx * resolution + min_y) - obs['y']
                        if dx*dx + dy*dy <= obs['r']**2:
                            grid[y_idx, x_idx] = 0

        pgm_path = f"{output_name}.pgm"
        yaml_path = f"{output_name}.yaml"

        # Ensure parent directory exists
        os.makedirs(os.path.dirname(pgm_path) if os.path.dirname(pgm_path) else '.', exist_ok=True)

        with open(pgm_path, 'wb') as f:
            f.write(f"P5\n{width} {height}\n255\n".encode())
            f.write(np.flipud(grid).tobytes())

        yaml_data = {
            'image': os.path.basename(pgm_path),  # Use basename for relative path
            'resolution': float(resolution),
            'origin': [float(min_x), float(min_y), 0.0],
            'negate': 0,
            'occupied_thresh': 0.65,
            'free_thresh': 0.196
        }
        with open(yaml_path, 'w') as f:
            yaml.dump(yaml_data, f, default_flow_style=False)

        messages = [f"Success: {pgm_path} and {yaml_path} generated."]

        if gen_png:
            p_success, p_path = generate_png(pgm_path)
            if p_success:
                messages.append(f"PNG image generated: {p_path}")
            else:
                messages.append(f"Warning: PNG generation failed: {p_path}")

        if gen_debug:
            d_success, d_path = generate_debug_plot(projected_obstacles, laser_z, output_name)
            if d_success:
                messages.append(f"Debug plot generated: {d_path}")
            else:
                messages.append(f"Warning: Debug plot failed: {d_path}")

        return True, "\n".join(messages)
    except Exception as e:
        import traceback
        return False, f"Error: {str(e)}\n{traceback.format_exc()}"

parse_pose(pose_str)

Parse SDF pose string into numpy array.

Parameters:

Name	Type	Description	Default
pose_str		Space-separated string "x y z roll pitch yaw"	required

Returns:

Type	Description
	numpy array of 6 elements [x, y, z, roll, pitch, yaw]

Source code in gt_map/generator.py

def parse_pose(pose_str):
    """Parse SDF pose string into numpy array.

    Args:
        pose_str: Space-separated string "x y z roll pitch yaw"

    Returns:
        numpy array of 6 elements [x, y, z, roll, pitch, yaw]
    """
    if not pose_str:
        return np.zeros(6)
    return np.fromstring(pose_str.strip(), sep=' ')

gt_map.viewer

Interactive map viewer for visualizing ground truth maps.

Displays occupancy grid maps with proper scaling and origin.

show_map(yaml_path)

Display a map from a YAML configuration file.

Parameters:

Name	Type	Description	Default
yaml_path		Path to the map YAML file (ROS map_server format)	required

The function loads the map image and metadata, then displays it with proper world coordinates using matplotlib.

Source code in gt_map/viewer.py

def show_map(yaml_path):
    """Display a map from a YAML configuration file.

    Args:
        yaml_path: Path to the map YAML file (ROS map_server format)

    The function loads the map image and metadata, then displays it
    with proper world coordinates using matplotlib.
    """
    if not os.path.exists(yaml_path):
        print(f"Error: {yaml_path} not found.")
        return

    with open(yaml_path, 'r') as f:
        data = yaml.safe_load(f)

    image_path = data['image']
    # If image path is relative to yaml
    if not os.path.isabs(image_path):
        image_path = os.path.join(os.path.dirname(yaml_path), image_path)

    resolution = data['resolution']
    origin = data['origin'] # [x, y, yaw]

    # Load image
    img = mpimg.imread(image_path)

    # Calculate extent for matplotlib [left, right, bottom, top]
    # In ROS, image (0,0) is bottom-left of map if we use origin correctly.
    # However, PGM images are usually top-down. 
    # My generator uses np.flipud(grid) so the first row of PGM is the highest Y.

    height, width = img.shape[:2]

    left = origin[0]
    right = origin[0] + width * resolution
    bottom = origin[1]
    top = origin[1] + height * resolution

    extent = [left, right, bottom, top]

    plt.figure(figsize=(10, 8))
    plt.imshow(img, cmap='gray', extent=extent, origin='lower')
    plt.colorbar(label='Grayscale Value')
    plt.title(f"Map: {image_path}\nResolution: {resolution}m/px, Origin: {origin[:2]}")
    plt.xlabel("X (meters)")
    plt.ylabel("Y (meters)")
    plt.grid(True, linestyle='--', alpha=0.6)

    print("Opening map viewer... (Close the window to continue)")
    plt.show()