msg_utils.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
00004 
00005 
00006 import rospy
00007 from pgm_learner.msg import DiscreteNode, LinearGaussianNode, ConditionalProbability, GraphStructure, GraphEdge
00008 
00009 from libpgm.graphskeleton import GraphSkeleton
00010 from libpgm.nodedata import NodeData
00011 
00012 def graph_skeleton_from_node_data(nd):
00013     skel = GraphSkeleton()
00014     skel.V = []
00015     skel.E = []
00016     for name, v in nd.Vdata.items():
00017         skel.V += [name]
00018         skel.E += [[name, c] for c in v["children"]]
00019     return skel
00020 
00021 def graph_skeleton_from_ros(graph_structure):
00022     skel = GraphSkeleton()
00023     skel.V = graph_structure.nodes
00024     skel.E = [[e.node_from, e.node_to] for e in graph_structure.edges]
00025     return skel
00026 
00027 def graph_skeleton_to_ros(skel):
00028     graph = GraphStructure()
00029     if skel.V and len(skel.V) > 0:
00030         graph.nodes = map(str, skel.V)
00031     if skel.E and len(skel.E) > 0:
00032         graph.edges = [GraphEdge(str(e[0]),str(e[1])) for e in skel.E]
00033     return graph
00034 
00035 def graph_state_dict_from_ros(graph_state):
00036     data = {}
00037     for s in graph_state.node_states:
00038         data[s.node] = s.state
00039     return data
00040 
00041 def graph_states_dict_from_ros(graph_states):
00042     return [graph_state_dict_from_ros(gs) for gs in graph_states]
00043 
00044 def discrete_node_from_dict(name, d):
00045     n = DiscreteNode()
00046     n.name = str(name)
00047     n.outcomes = map(str, d["vals"])
00048     if d["parents"]:
00049         n.parents = map(str, d["parents"])
00050     if d["children"]:
00051         n.children = map(str, d["children"])
00052     cprob = d["cprob"]
00053     if isinstance(cprob, dict):
00054         n.CPT = [ConditionalProbability(values=eval(k), probabilities=v) for k,v in (d["cprob"]).items()]
00055     else:
00056         n.CPT = [ConditionalProbability(values=map(str, d["vals"]), probabilities=cprob)]
00057     return n
00058 
00059 def discrete_nodes_to_ros(d):
00060     return [discrete_node_from_dict(k, v) for k,v in d.items()]
00061 
00062 def dict_from_ros_discrete_node(msg):
00063     d = {}
00064     d["vals"] = msg.outcomes
00065     d["numoutcomes"] = len(msg.outcomes)
00066     d["parents"] = msg.parents
00067     d["children"] = msg.children
00068     if len(msg.CPT) == 1:
00069         d["cprob"] = msg.CPT[0].probabilities
00070     else:
00071         d["cprob"] = {str(p.values): p.probabilities for p in msg.CPT}
00072     return d
00073 
00074 
00075 def discrete_nodedata_from_ros(nodes):
00076     nd = NodeData()
00077     nd.Vdata = {n.name: dict_from_ros_discrete_node(n) for n in nodes}
00078     return nd
00079 
00080 def linear_gaussian_node_from_dict(name, d):
00081     n = LinearGaussianNode()
00082     n.name = str(name)
00083     if d["parents"]:
00084         n.parents = map(str, d["parents"])
00085     if d["children"]:
00086         n.children = map(str, d["children"])
00087     n.mean = d["mean_base"]
00088     n.variance = d["variance"]
00089     n.mean_scalar = d["mean_scal"]
00090     return n
00091 
00092 def linear_gaussian_nodes_to_ros(d):
00093     return [linear_gaussian_node_from_dict(k,v) for k,v in d.items()]


pgm_learner
Author(s): Yuki Furuta
autogenerated on Wed Jul 10 2019 03:24:11