def add_node(self, name, dist, pos, std_action) :
rospy.loginfo('Creating Node: '+name)
msg_store = MessageStoreProxy(collection='topological_maps')
found = False
for i in self.nodes :
if i.name == name :
found = True
if found :
rospy.logerr("Node already exists, try another name")
else :
rospy.loginfo('Adding node: '+name)
meta = {}
meta["map"] = self.map
meta["pointset"] = self.name
meta["node"] = name
node = TopologicalNode()
node.name = name
node.map = self.map
node.pointset = self.name
node.pose = pos
vertices=[(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69), (-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69), (0.287, -0.69), (0.69, -0.287)]
for j in vertices :
v = Vertex()
v.x = float(j[0])
v.y = float(j[1])
node.verts.append(v)
cx = node.pose.position.x
cy = node.pose.position.y
close_nodes = []
for i in self.nodes :
ndist = i._get_distance(cx, cy)
if ndist < dist :
if i.name != 'ChargingPoint' :
close_nodes.append(i.name)
for i in close_nodes :
edge = Edge()
edge.node = i
edge.action = std_action
node.edges.append(edge)
msg_store.insert(node,meta)
for i in close_nodes :
self.add_edge(i, name, std_action)
def update_node(node, pointset):
to_pop=['edges','verts','pose']
n = TopologicalNode()
n.yaw_goal_tolerance = 0.1
n.xy_goal_tolerance = 0.3
lsl = n.__slots__
for j in lsl:
if node.has_key(j) and not j in to_pop:
setattr(n, j, node[j])
ppos = node['pose']
p = dc_util.dictionary_to_message(ppos, Pose)
#print p
#n.pose.append(p)
n.pose = p
n.pointset = pointset
return n
def create_node(name, mapname, pointset, line, vertices):
o=[]
n = TopologicalNode()
n.name = name
n.map = mapname
n.pointset = pointset
n.pose = pose_from_waypoint(line)
n.yaw_goal_tolerance = 0.1
n.xy_goal_tolerance = 0.3
n.verts = get_vertex_list(vertices)
m = {}
m["map"] = mapname
m["pointset"] = pointset
m["node"] = n.name
o.append(n)
o.append(m)
return o
def add_node(self, name, dist, pos, std_action):
rospy.loginfo('Creating Node: ' + name)
msg_store = MessageStoreProxy(collection='topological_maps')
found = False
for i in self.nodes:
if i.name == name:
found = True
if found:
rospy.logerr("Node already exists, try another name")
else:
rospy.loginfo('Adding node: ' + name)
meta = {}
meta["map"] = self.map
meta["pointset"] = self.name
meta["node"] = name
node = TopologicalNode()
node.name = name
node.map = self.map
node.pointset = self.name
node.pose = pos
vertices = [(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69),
(-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69),
(0.287, -0.69), (0.69, -0.287)]
for j in vertices:
v = Vertex()
v.x = float(j[0])
v.y = float(j[1])
node.verts.append(v)
cx = node.pose.position.x
cy = node.pose.position.y
close_nodes = []
for i in self.nodes:
ndist = i._get_distance(cx, cy)
if ndist < dist:
if i.name != 'ChargingPoint':
close_nodes.append(i.name)
for i in close_nodes:
edge = Edge()
edge.node = i
edge.action = std_action
node.edges.append(edge)
msg_store.insert(node, meta)
for i in close_nodes:
self.add_edge(i, name, std_action)
# need to reload the map when a node is added, for consistency
self.loadMap(self.name)
def update_node(node, pointset):
to_pop = ['edges', 'verts', 'pose']
n = TopologicalNode()
n.yaw_goal_tolerance = 0.1
n.xy_goal_tolerance = 0.3
lsl = n.__slots__
for j in lsl:
if node.has_key(j) and not j in to_pop:
setattr(n, j, node[j])
ppos = node['pose']
p = dc_util.dictionary_to_message(ppos, Pose)
#print p
#n.pose.append(p)
n.pose = p
n.pointset = pointset
return n
from geometry_msgs.msg import Pose
if __name__ == '__main__':
rospy.init_node('insert_empty_map')
if len(sys.argv) != 2:
rospy.logwarn("usage: insert_empty_map.py pointset_name")
sys.exit(1)
pointset = sys.argv[1]
map_loader = YamlMapLoader()
if pointset in map_loader.get_maps():
rospy.logwarn(
"Map with name {0} already exists. Try another one.".format(
sys.argv[1]))
sys.exit(1)
msg_store = MessageStoreProxy(collection='topological_maps')
empty_node = TopologicalNode()
empty_node.name = "temp_node"
empty_node.map = "unused"
empty_node.pointset = pointset
meta = {}
meta['pointset'] = pointset
meta['map'] = "unused"
meta['node'] = "temp_node"
msg_store.insert(empty_node, meta)
filename = str(sys.argv[1])
outfile = str(sys.argv[2])
dataset_name = str(sys.argv[3])
map_name = str(sys.argv[4])
eids = [] #list of known edge id's
lnodes = loadMap(filename, dataset_name, map_name)
nnodes = []
meta = {}
meta["map"] = map_name
meta["pointset"] = dataset_name
for i in lnodes:
n = TopologicalNode()
n.name = i.name
meta["node"] = i.name
n.map = i.map
n.pointset = i.pointset
p = Pose()
p.position.x = float(i.waypoint[0])
p.position.y = float(i.waypoint[1])
p.position.z = float(i.waypoint[2])
p.orientation.x = float(i.waypoint[3])
p.orientation.y = float(i.waypoint[4])
p.orientation.z = float(i.waypoint[5])
p.orientation.w = float(i.waypoint[6])
n.pose = p
for j in i.vertices:
v = Vertex()
i[0].localise_by_topic = json_str
#print i[0]
print "Updating %s--%s" % (i[0].pointset, i[0].name)
msg_store.update_id(msgid, i[0], i[1], upsert=False)
if __name__ == '__main__':
rospy.init_node('strands_navigation_migration')
host = rospy.get_param("mongodb_host")
port = rospy.get_param("mongodb_port")
client = pymongo.MongoClient(host, port)
to_pop = ['_id', '_meta']
b = TopologicalNode().__slots__
d = Edge().__slots__
print '========= Current Topological NODE definition Slots ==========='
print b
print '========= Current Topological EDGE definition Slots ==========='
print d
to_update = check_for_update(b, d, client)
print '========= The following maps need to be updated ==========='
print to_update
update_maps(to_update, client)
check_sanity(client)
filename=str(sys.argv[1])
outfile=str(sys.argv[2])
dataset_name=str(sys.argv[3])
map_name=str(sys.argv[4])
eids=[] #list of known edge id's
lnodes=loadMap(filename, dataset_name, map_name)
nnodes=[]
meta = {}
meta["map"] = map_name
meta["pointset"] = dataset_name
for i in lnodes:
n = TopologicalNode()
n.name = i.name
meta["node"] = i.name
n.map = i.map
n.pointset = i.pointset
p = Pose()
p.position.x=float(i.waypoint[0])
p.position.y=float(i.waypoint[1])
p.position.z=float(i.waypoint[2])
p.orientation.x=float(i.waypoint[3])
p.orientation.y=float(i.waypoint[4])
p.orientation.z=float(i.waypoint[5])
p.orientation.w=float(i.waypoint[6])
n.pose = p
for j in i.vertices :
v = Vertex()