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()