text: #include <voxelized_geometry_tools/ros_interface.hpp>
  
  #include <vector>
  
  #include <Eigen/Geometry>
  #include <ros/ros.h>
  #include <std_msgs/ColorRGBA.h>
  #include <visualization_msgs/MarkerArray.h>
  
  #include <common_robotics_utilities/color_builder.hpp>
  #include <common_robotics_utilities/conversions.hpp>
  #include <common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp>
  #include <common_robotics_utilities/utility.hpp>
  #include <common_robotics_utilities/voxel_grid.hpp>
  #include <common_robotics_utilities/zlib_helpers.hpp>
  #include <voxelized_geometry_tools/collision_map.hpp>
  #include <voxelized_geometry_tools/CollisionMapMessage.h>
  #include <voxelized_geometry_tools/tagged_object_collision_map.hpp>
  #include <voxelized_geometry_tools/TaggedObjectCollisionMapMessage.h>
  
  namespace voxelized_geometry_tools
  {
  namespace ros_interface
  {
  visualization_msgs::Marker ExportForDisplay(
      const CollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const auto color_fn
        = [&] (const CollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      if (cell.Occupancy() > 0.5)
      {
        return collision_color;
      }
      else if (cell.Occupancy() < 0.5)
      {
        return free_color;
      }
      else
      {
        return unknown_color;
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<CollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "collision_map";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::MarkerArray ExportForSeparateDisplay(
      const CollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    visualization_msgs::Marker collision_only_marker
        = ExportForDisplay(collision_map, collision_color, no_color, no_color);
    collision_only_marker.ns = "collision_only";
    visualization_msgs::Marker free_only_marker
        = ExportForDisplay(collision_map, no_color, free_color, no_color);
    free_only_marker.ns = "free_only";
    visualization_msgs::Marker unknown_only_marker
        = ExportForDisplay(collision_map, no_color, no_color, unknown_color);
    unknown_only_marker.ns = "unknown_only";
    visualization_msgs::MarkerArray display_messages;
    display_messages.markers = {collision_only_marker,
                                free_only_marker,
                                unknown_only_marker};
    return display_messages;
  }
  
  visualization_msgs::Marker ExportSurfacesForDisplay(
      const CollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    const auto color_fn
        = [&] (const CollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex& index)
    {
      if (collision_map.IsSurfaceIndex(index))
      {
        if (cell.Occupancy() > 0.5)
        {
          return collision_color;
        }
        else if (cell.Occupancy() < 0.5)
        {
          return free_color;
        }
        else
        {
          return unknown_color;
        }
      }
      else
      {
        return no_color;
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<CollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "collision_surfaces";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::MarkerArray ExportSurfacesForSeparateDisplay(
      const CollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    visualization_msgs::Marker collision_only_marker
        = ExportSurfacesForDisplay(
            collision_map, collision_color, no_color, no_color);
    collision_only_marker.ns = "collision_surfaces_only";
    visualization_msgs::Marker free_only_marker
        = ExportSurfacesForDisplay(
            collision_map, no_color, free_color, no_color);
    free_only_marker.ns = "free_surfaces_only";
    visualization_msgs::Marker unknown_only_marker
        = ExportSurfacesForDisplay(
            collision_map, no_color, no_color, unknown_color);
    unknown_only_marker.ns = "unknown_surfaces_only";
    visualization_msgs::MarkerArray display_messages;
    display_messages.markers = {collision_only_marker,
                                free_only_marker,
                                unknown_only_marker};
    return display_messages;
  }
  
  visualization_msgs::Marker ExportConnectedComponentsForDisplay(
      const CollisionMap& collision_map,
      const bool color_unknown_components)
  {
    const std_msgs::ColorRGBA unknown_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.5, 0.5, 0.5, 1.0);
    const auto color_fn
        = [&] (const CollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      if (cell.Occupancy() != 0.5)
      {
        return LookupComponentColor(cell.Component());
      }
      else
      {
        if (color_unknown_components)
        {
          return LookupComponentColor(cell.Component());
        }
        else
        {
          return unknown_color;
        }
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<CollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "connected_components";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::Marker ExportIndexMapForDisplay(
      const CollisionMap& collision_map,
      const std::unordered_map<
          common_robotics_utilities::voxel_grid::GridIndex, uint8_t>& index_map,
      const std_msgs::ColorRGBA& surface_color)
  {
    const auto color_fn
        = [&] (const CollisionCell&,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      return surface_color;
    };
    auto display_rep = ExportVoxelGridIndexMapToRViz<CollisionCell>(
        collision_map, index_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "collision_map_surface";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::Marker ExportIndicesForDisplay(
      const CollisionMap& collision_map,
      const std::vector<common_robotics_utilities::voxel_grid::GridIndex>&
          indices,
      const std_msgs::ColorRGBA& surface_color)
  {
    const auto color_fn
        = [&] (const CollisionCell&,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      return surface_color;
    };
    auto display_rep = ExportVoxelGridIndicesToRViz<CollisionCell>(
        collision_map, indices, collision_map.GetFrame(), color_fn);
    display_rep.ns = "collision_map_surface";
    display_rep.id = 1;
    return display_rep;
  }
  
  CollisionMapMessage GetMessageRepresentation(const CollisionMap& map)
  {
    CollisionMapMessage map_message;
    map_message.header.stamp = ros::Time::now();
    map_message.header.frame_id = map.GetFrame();
    std::vector<uint8_t> buffer;
    CollisionMap::Serialize(map, buffer);
    map_message.serialized_map
        = common_robotics_utilities::zlib_helpers::CompressBytes(buffer);
    map_message.is_compressed = true;
    return map_message;
  }
  
  CollisionMap LoadFromMessageRepresentation(const CollisionMapMessage& message)
  {
    if (message.is_compressed)
    {
      const std::vector<uint8_t> decompressed_map
          = common_robotics_utilities::zlib_helpers::DecompressBytes(
              message.serialized_map);
      return CollisionMap::Deserialize(decompressed_map, 0).Value();
    }
    else
    {
      return CollisionMap::Deserialize(message.serialized_map, 0).Value();
    }
  }
  
  visualization_msgs::MarkerArray ExportForDisplay(
      const DynamicSpatialHashedCollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const auto color_fn
        = [&] (const CollisionCell& cell,
               const Eigen::Vector4d&)
    {
      if (cell.Occupancy() > 0.5)
      {
        return collision_color;
      }
      else if (cell.Occupancy() < 0.5)
      {
        return free_color;
      }
      else
      {
        return unknown_color;
      }
    };
    auto display_rep
        = ExportDynamicSpatialHashedVoxelGridToRViz<CollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn, color_fn);
    display_rep.first.ns = "dsh_collision_map_chunks";
    display_rep.first.id = 1;
    display_rep.second.ns = "dsh_collision_map_cells";
    display_rep.second.id = 1;
    visualization_msgs::MarkerArray display_markers;
    display_markers.markers = {display_rep.first, display_rep.second};
    return display_markers;
  }
  
  visualization_msgs::MarkerArray ExportForSeparateDisplay(
      const DynamicSpatialHashedCollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    visualization_msgs::MarkerArray collision_only_markers
        = ExportForDisplay(collision_map, collision_color, no_color, no_color);
    for (auto& collision_only_marker : collision_only_markers.markers)
    {
     collision_only_marker.ns += "_collision_only";
    }
    visualization_msgs::MarkerArray free_only_markers
        = ExportForDisplay(collision_map, no_color, free_color, no_color);
    for (auto& free_only_marker : free_only_markers.markers)
    {
      free_only_marker.ns += "_free_only";
    }
    visualization_msgs::MarkerArray unknown_only_markers
        = ExportForDisplay(collision_map, no_color, no_color, unknown_color);
    for (auto& unknown_only_marker : unknown_only_markers.markers)
    {
      unknown_only_marker.ns += "_unknown_only";
    }
    visualization_msgs::MarkerArray display_messages;
    display_messages.markers.insert(display_messages.markers.end(),
                                    collision_only_markers.markers.begin(),
                                    collision_only_markers.markers.end());
    display_messages.markers.insert(display_messages.markers.end(),
                                    free_only_markers.markers.begin(),
                                    free_only_markers.markers.end());
    display_messages.markers.insert(display_messages.markers.end(),
                                    unknown_only_markers.markers.begin(),
                                    unknown_only_markers.markers.end());
    return display_messages;
  }
  
  DynamicSpatialHashedCollisionMapMessage GetMessageRepresentation(
      const DynamicSpatialHashedCollisionMap& map)
  {
    DynamicSpatialHashedCollisionMapMessage map_message;
    map_message.header.stamp = ros::Time::now();
    map_message.header.frame_id = map.GetFrame();
    std::vector<uint8_t> buffer;
    DynamicSpatialHashedCollisionMap::Serialize(map, buffer);
    map_message.serialized_map
        = common_robotics_utilities::zlib_helpers::CompressBytes(buffer);
    map_message.is_compressed = true;
    return map_message;
  }
  
  DynamicSpatialHashedCollisionMap LoadFromMessageRepresentation(
      const DynamicSpatialHashedCollisionMapMessage& message)
  {
    if (message.is_compressed)
    {
      const std::vector<uint8_t> decompressed_map
          = common_robotics_utilities::zlib_helpers::DecompressBytes(
              message.serialized_map);
      return DynamicSpatialHashedCollisionMap::Deserialize(
          decompressed_map, 0).Value();
    }
    else
    {
      return DynamicSpatialHashedCollisionMap::Deserialize(
          message.serialized_map, 0).Value();
    }
  }
  
  visualization_msgs::Marker ExportForDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const auto color_fn
        = [&] (const TaggedObjectCollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      if (cell.Occupancy() > 0.5)
      {
        return collision_color;
      }
      else if (cell.Occupancy() < 0.5)
      {
        return free_color;
      }
      else
      {
        return unknown_color;
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "tagged_object_collision_map";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::Marker ExportForDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const std::map<uint32_t, std_msgs::ColorRGBA>& object_color_map)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    const bool specifies_colors = object_color_map.empty();
    const auto color_fn
        = [&] (const TaggedObjectCollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      const uint32_t object_id = cell.ObjectId();
      if (specifies_colors)
      {
        const auto found_itr = object_color_map.find(object_id);
        if (found_itr != object_color_map.end())
        {
          return found_itr->second;
        }
        else
        {
          return no_color;
        }
      }
      else
      {
        return LookupComponentColor(object_id, 1.0);
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "tagged_object_collision_map";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::MarkerArray ExportForSeparateDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    visualization_msgs::Marker collision_only_marker
        = ExportForDisplay(collision_map, collision_color, no_color, no_color);
    collision_only_marker.ns = "tagged_object_collision_only";
    visualization_msgs::Marker free_only_marker
        = ExportForDisplay(collision_map, no_color, free_color, no_color);
    free_only_marker.ns = "tagged_object_free_only";
    visualization_msgs::Marker unknown_only_marker
        = ExportForDisplay(collision_map, no_color, no_color, unknown_color);
    unknown_only_marker.ns = "tagged_object_unknown_only";
    visualization_msgs::MarkerArray display_messages;
    display_messages.markers = {collision_only_marker,
                                free_only_marker,
                                unknown_only_marker};
    return display_messages;
  }
  
  visualization_msgs::Marker ExportSurfacesForDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    const auto color_fn
        = [&] (const TaggedObjectCollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex& index)
    {
      if (collision_map.IsSurfaceIndex(index))
      {
        if (cell.Occupancy() > 0.5)
        {
          return collision_color;
        }
        else if (cell.Occupancy() < 0.5)
        {
          return free_color;
        }
        else
        {
          return unknown_color;
        }
      }
      else
      {
        return no_color;
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "tagged_object_collision_surfaces";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::Marker ExportSurfacesForDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const std::map<uint32_t, std_msgs::ColorRGBA>& object_color_map)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    const bool specifies_colors = object_color_map.empty();
    const auto color_fn
        = [&] (const TaggedObjectCollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex& index)
    {
      if (collision_map.IsSurfaceIndex(index))
      {
        const uint32_t object_id = cell.ObjectId();
        if (specifies_colors)
        {
          const auto found_itr = object_color_map.find(object_id);
          if (found_itr != object_color_map.end())
          {
            return found_itr->second;
          }
          else
          {
            return no_color;
          }
        }
        else
        {
          return LookupComponentColor(object_id, 1.0);
        }
      }
      else
      {
        return no_color;
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "tagged_object_collision_surfaces";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::MarkerArray ExportSurfacesForSeparateDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const std_msgs::ColorRGBA& collision_color,
      const std_msgs::ColorRGBA& free_color,
      const std_msgs::ColorRGBA& unknown_color)
  {
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    visualization_msgs::Marker collision_only_marker
        = ExportSurfacesForDisplay(
            collision_map, collision_color, no_color, no_color);
    collision_only_marker.ns = "tagged_object_collision_surfaces_only";
    visualization_msgs::Marker free_only_marker
        = ExportSurfacesForDisplay(
            collision_map, no_color, free_color, no_color);
    free_only_marker.ns = "tagged_object_free_surfaces_only";
    visualization_msgs::Marker unknown_only_marker
        = ExportSurfacesForDisplay(
            collision_map, no_color, no_color, unknown_color);
    unknown_only_marker.ns = "tagged_object_unknown_surfaces_only";
    visualization_msgs::MarkerArray display_messages;
    display_messages.markers = {collision_only_marker,
                                free_only_marker,
                                unknown_only_marker};
    return display_messages;
  }
  
  visualization_msgs::Marker ExportConnectedComponentsForDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const bool color_unknown_components)
  {
    const std_msgs::ColorRGBA unknown_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.5, 0.5, 0.5, 1.0);
    const auto color_fn
        = [&] (const TaggedObjectCollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      if (cell.Occupancy() != 0.5)
      {
        return LookupComponentColor(cell.Component());
      }
      else
      {
        if (color_unknown_components)
        {
          return LookupComponentColor(cell.Component());
        }
        else
        {
          return unknown_color;
        }
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "tagged_object_connected_components";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::Marker ExportSpatialSegmentForDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const uint32_t object_id, const uint32_t spatial_segment)
  {
    const uint32_t number_of_segments
        = collision_map.GetNumSpatialSegments().Value();
    const std_msgs::ColorRGBA no_color
        = common_robotics_utilities::color_builder
            ::MakeFromFloatColors<std_msgs::ColorRGBA>(0.0, 0.0, 0.0, 0.0);
    const auto color_fn
        = [&] (const TaggedObjectCollisionCell& cell,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      if ((cell.ObjectId() == object_id)
          && (cell.SpatialSegment() == spatial_segment))
      {
        if (number_of_segments < 22)
        {
          return LookupComponentColor(cell.SpatialSegment());
        }
        else
        {
          return common_robotics_utilities::color_builder
              ::InterpolateHotToCold<std_msgs::ColorRGBA>(
                  static_cast<double>(cell.SpatialSegment()),
                  0.0,
                  static_cast<double>(number_of_segments));
        }
      }
      else
      {
        return no_color;
      }
    };
    auto display_rep
        = ExportVoxelGridToRViz<TaggedObjectCollisionCell>(
            collision_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "tagged_object_"
                     + std::to_string(object_id)
                     + "_spatial_segment_"
                     + std::to_string(spatial_segment);
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::Marker ExportIndexMapForDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const std::unordered_map<
          common_robotics_utilities::voxel_grid::GridIndex, uint8_t>& index_map,
      const std_msgs::ColorRGBA& surface_color)
  {
    const auto color_fn
        = [&] (const TaggedObjectCollisionCell&,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      return surface_color;
    };
    auto display_rep = ExportVoxelGridIndexMapToRViz<TaggedObjectCollisionCell>(
        collision_map, index_map, collision_map.GetFrame(), color_fn);
    display_rep.ns = "tagged_object_collision_map_surface";
    display_rep.id = 1;
    return display_rep;
  }
  
  visualization_msgs::Marker ExportIndicesForDisplay(
      const TaggedObjectCollisionMap& collision_map,
      const std::vector<common_robotics_utilities::voxel_grid::GridIndex>&
          indices,
      const std_msgs::ColorRGBA& surface_color)
  {
    const auto color_fn
        = [&] (const TaggedObjectCollisionCell&,
               const common_robotics_utilities::voxel_grid::GridIndex&)
    {
      return surface_color;
    };
    auto display_rep = ExportVoxelGridIndicesToRViz<TaggedObjectCollisionCell>(
        collision_map, indices, collision_map.GetFrame(), color_fn);
    display_rep.ns = "tagged_object_collision_map_surface";
    display_rep.id = 1;
    return display_rep;
  }
  
  TaggedObjectCollisionMapMessage GetMessageRepresentation(
      const TaggedObjectCollisionMap& map)
  {
    TaggedObjectCollisionMapMessage map_message;
    map_message.header.stamp = ros::Time::now();
    map_message.header.frame_id = map.GetFrame();
    std::vector<uint8_t> buffer;
    TaggedObjectCollisionMap::Serialize(map, buffer);
    map_message.serialized_map
        = common_robotics_utilities::zlib_helpers::CompressBytes(buffer);
    map_message.is_compressed = true;
    return map_message;
  }
  
  TaggedObjectCollisionMap LoadFromMessageRepresentation(
      const TaggedObjectCollisionMapMessage& message)
  {
    if (message.is_compressed)
    {
      const std::vector<uint8_t> decompressed_map
          = common_robotics_utilities::zlib_helpers::DecompressBytes(
              message.serialized_map);
      return TaggedObjectCollisionMap::Deserialize(decompressed_map, 0).Value();
    }
    else
    {
      return TaggedObjectCollisionMap::Deserialize(
          message.serialized_map, 0).Value();
    }
  }
  }  // namespace ros_interface
  }  // namespace voxelized_geometry_tools
  