def get_list_of_edges(self):
self.eids = []
rospy.loginfo("Querying for list of edges")
for i in self.lnodes.nodes:
for j in i.edges:
if j.edge_id not in self.edgid:
self.edgid.append(j.edge_id)
val = {}
val["edge_id"] = j.edge_id
#val["model_id"]=self.lnodes.name+'__'+j.edge_id
ori = {}
ori['pos'] = dc_util.msg_to_document(i.pose.position)
ori['name'] = i.name
rad = get_radius(i)
ori['radius'] = rad
val["ori"] = ori
ddn = get_node(self.lnodes, j.node)
dest = {}
dest['name'] = j.node
dest['pos'] = dc_util.msg_to_document(ddn.pose.position)
rad = get_radius(ddn)
dest['radius'] = rad
val["dest"] = dest
val["dist"] = get_distance_to_node(i, ddn)
self.eids.append(val)
fdbmsg = 'Done. %d edges found' % len(self.edgid)
rospy.loginfo(fdbmsg)
e.node = lnodes[0][0].name
a[0].edges.append(e)
lnodes.append(a)
print "Connecting %s to %s"%(lnodes[0][0].name, a[0].name)
e = get_empty_edge(mapname, 'docking')
e.edge_id = "%s_%s"%(lnodes[0][0].name, a[0].name)
e.node = a[0].name
lnodes[0][0].edges.append(e)
#print lnodes[0][0].edges
onodes = []
for i in lnodes:
r = []
q = dc_util.msg_to_document(i[0])
r.append(q)
r.append(i[1])
onodes.append(r)
yml = yaml.safe_dump(onodes, default_flow_style=False)
print yml
#print s_output
fh = open(outfile, "w")
#s_output = str(bson.json_util.dumps(nodeinf, indent=1))
s_output = str(yml)
print s_output
fh.write(s_output)
fh.close
e.action = k['action']
eid = get_edge_id(i.name, e.node, eids)
eids.append(eid)
e.edge_id = eid
e.top_vel = 0.55
e.map_2d = map_name
# e.use_default_recovery = True
# e.use_default_nav_recovery = True
# e.use_default_helpers = True
n.edges.append(e)
nnodes.append(n)
onodes = []
for i in nnodes:
r = []
q = dc_util.msg_to_document(i)
m = {}
m["map"] = map_name
m["pointset"] = dataset_name
m["node"] = n.name
r.append(q)
r.append(m)
onodes.append(r)
onodes.sort(key=lambda x: x[0]['name'])
yml = yaml.safe_dump(onodes, default_flow_style=False)
#print yml
#print s_output
fh = open(outfile, "w")
#s_output = str(bson.json_util.dumps(nodeinf, indent=1))