from collections import OrderedDict
import os
import json

import genmsg
import rosmsg
import rospkg


def load_specs(context, search_path, msg):
    spec = genmsg.load_msg_by_type(context, msg, search_path)
    genmsg.load_depends(context, spec, search_path)


def _get_info(context, msg):
    base_type = genmsg.msgs.bare_msg_type(msg)
    if base_type in genmsg.msgs.BUILTIN_TYPES:
        return base_type
    else:
        spec = context.get_registered(base_type)
        return OrderedDict((k, _get_info(context, t)) for k, t in
                           zip(spec.names, spec.types))


def get_info(context, search_path, msg):
    load_specs(context, search_path, msg)
    return _get_info(context, msg)


rospack = rospkg.RosPack()

search_path = {}
for p in rospack.list():
    package_paths = rosmsg._get_package_paths(p, rospack)
    search_path[p] = [os.path.join(d, 'msg') for d in package_paths]

context = genmsg.MsgContext.create_default()

info = get_info(
    context, search_path, 'geometry_msgs/PoseWithCovarianceStamped')
print(json.dumps(info, indent=4))