Python topological_map Examples

Example #1

File: policies.py Project: hawesie/strands_navigation

    def update_map(self,map_name) :
        self.topo_map = topological_map(self.map_name)
        self.map_edges = MarkerArray()
                
        counter=0
        total = len(self.route_nodes.source)

        #print 'updating '+str(total)+' edges'        
        while counter < total :
            #print 'Creating edge '+str(counter) 
            inds = self.topo_map._get_node_index(self.route_nodes.source[counter])
            indt = self.topo_map._get_node_index(self.route_nodes.target[counter])
            point1=Point()
            point2=Point()
            point1= (self.topo_map.nodes[inds]._get_pose()).position
            point2= (self.topo_map.nodes[indt]._get_pose()).position
            #val = self.route_nodes.prob[counter]
            self.create_edge(point1, point2)
            counter+=1

        idn = 0
        for m in self.map_edges.markers:
            m.id = idn
            idn += 1
        self.updating=False
        
        
        print "All Done"

Example #2

File: node_controller.py Project: Vesperal-Hunter/workspace_for_master_project

 def update_map(self, msg) :
     print "updating node controllers..."
     self.topo_map = topological_map(msg.name, msg=msg)
     self._marker_server.clear()
     self._marker_server.applyChanges()
   
     for i in self.topo_map.nodes :
         self._create_marker(i.name, i._get_pose(), i.name)

Example #3

    def update_map(self, msg):
        print "updating node controllers..."
        self.topo_map = topological_map(msg.name, msg=msg)
        self._marker_server.clear()
        self._marker_server.applyChanges()

        for i in self.topo_map.nodes:
            self._create_marker(i.name, i._get_pose(), i.name)

Example #4

    def __init__(self, pointset, waypoint):

        self.topo_map = topological_map(pointset)
        self.topo_map.remove_node(waypoint)

        map_update = rospy.Publisher('/update_map', std_msgs.msg.Time)
        map_update.publish(rospy.Time.now())

        rospy.loginfo("All Done ...")

Example #5

File: node_rm.py Project: Vesperal-Hunter/workspace_for_master_project

    def __init__(self, pointset, waypoint) :

        self.topo_map = topological_map(pointset)
        self.topo_map.remove_node(waypoint)    
        
        
        map_update = rospy.Publisher('/update_map', std_msgs.msg.Time)        
        map_update.publish(rospy.Time.now())

        rospy.loginfo("All Done ...")

Example #6

File: visualise_map.py Project: hawesie/strands_navigation

 def _update_everything(self) :
     print "updating ..."
     print "loading file from map %s" %self._point_set
     self.topo_map = topological_map(self._point_set)
     print "Done"
     self.wayp_marker = waypoints_markers(self.topo_map)
     self.map_edges = edges_marker(self.topo_map)
     self.node_zone = vertices_marker(self.topo_map)
     self.edge_std = edges_std_marker(self._point_set)
     self.policies = policies_marker(self._point_set)
     self.edge_std.update_map(self._point_set)
     self.policies.update_map(self._point_set) 

Example #7

    def __init__(self, pointset, name, dist):

        try:
            pos = rospy.wait_for_message('/robot_pose', Pose, timeout=10.0)
        except rospy.ROSException:
            rospy.logwarn("Failed to get robot pose")
            return

        self.topo_map = topological_map(pointset)
        self.topo_map.add_node(name, dist, pos, 'move_base')

        map_update = rospy.Publisher('/update_map', std_msgs.msg.Time)
        map_update.publish(rospy.Time.now())
        rospy.loginfo("All Done ...")

Example #8

File: add_node.py Project: Vesperal-Hunter/workspace_for_master_project

    def __init__(self, pointset, name, dist) :

        try:
            pos = rospy.wait_for_message('/robot_pose', Pose, timeout=10.0)
        except rospy.ROSException :
            rospy.logwarn("Failed to get robot pose")
            return

        self.topo_map = topological_map(pointset)     
        self.topo_map.add_node(name,dist, pos, 'move_base')

        map_update = rospy.Publisher('/update_map', std_msgs.msg.Time)        
        map_update.publish(rospy.Time.now())
        rospy.loginfo("All Done ...")

Example #9

    def update_map(self, msg):
        print "updating edge controllers..."

        self.topo_map = topological_map(msg.name, msg=msg)
        self._edge_server.clear()
        self._edge_server.applyChanges()

        for node in self.topo_map.nodes:
            for i in node.edges:
                ind = self.topo_map._get_node_index(i['node'])
                V1 = Point()
                V2 = Point()
                V1 = (node._get_pose()).position
                V2 = (self.topo_map.nodes[ind]._get_pose()).position
                edge_name = node.name + "_" + self.topo_map.nodes[ind].name
                self._edge_marker(edge_name, V1, V2, edge_name)

Example #10

File: edge_controller.py Project: Vesperal-Hunter/workspace_for_master_project

 def update_map(self, msg) :
     print "updating edge controllers..."
     
     self.topo_map = topological_map(msg.name, msg=msg)       
     self._edge_server.clear()
     self._edge_server.applyChanges()
     
     for node in self.topo_map.nodes :
         for i in node.edges :
             ind = self.topo_map._get_node_index(i['node'])
             V1=Point()
             V2=Point()
             V1= (node._get_pose()).position
             V2= (self.topo_map.nodes[ind]._get_pose()).position
             edge_name=node.name+"_"+self.topo_map.nodes[ind].name
             self._edge_marker(edge_name, V1, V2, edge_name)

Example #11

File: visualise_map.py Project: hawesie/strands_navigation

    def map_callback(self, msg) :
        if not self._edit_mode :
            self.goto_cont.update_map(msg)
        self.node_cont.update_map(msg)
        self.vert_cont.update_map(msg)
        self.edge_cont.update_map(msg)
        self.add_rm_node.update_map(msg)
        self.topo_map = topological_map(msg.name, msg=msg)

        self.wayp_marker = waypoints_markers(self.topo_map)
        self.map_edges = edges_marker(self.topo_map)
        self.node_zone = vertices_marker(self.topo_map)
        self.edge_std = edges_std_marker(self._point_set)
        self.policies = policies_marker(self._point_set)
        self.edge_std.update_map(self._point_set)
        self.policies.update_map(self._point_set)        

Example #12

File: vertex_controller.py Project: Vesperal-Hunter/workspace_for_master_project

 def update_map(self, msg) :
     print "updating vertex controllers..."
     self.topo_map = topological_map(msg.name, msg=msg)
     self._vertex_server.clear()
     self._vertex_server.applyChanges()
     
     for node in self.topo_map.nodes :
         node._get_coords()
         count=0
         for i in node.vertices :
             Vert = Point()
             Vert.z = 0.05
             Vert.x = node.px + i[0]
             Vert.y = node.py + i[1]
             vertname = "%s-%d" %(node.name, count)
             Pos = Pose()
             Pos.position = Vert
             self._vertex_marker(vertname, Pos, vertname)
             count+=1

Example #13

    def update_map(self, msg):
        print("updating vertex controllers...")
        self.topo_map = topological_map(msg.name, msg=msg)
        self._vertex_server.clear()
        self._vertex_server.applyChanges()

        for node in self.topo_map.nodes:
            node._get_coords()
            count = 0
            for i in node.vertices:
                Vert = Point()
                Vert.z = 0.05
                Vert.x = node.px + i[0]
                Vert.y = node.py + i[1]
                vertname = "%s-%d" % (node.name, count)
                Pos = Pose()
                Pos.position = Vert
                self._vertex_marker(vertname, Pos, vertname)
                count += 1

Example #14

File: visualise_map.py Project: Vesperal-Hunter/workspace_for_master_project

    def _update_everything(self) :

        print "loading file from map %s" %self._point_set
        self.topo_map = topological_map(self._point_set)
        print "Done"
        
        self._marker_server = InteractiveMarkerServer(self._point_set+"_markers")      
        self._vertex_server = InteractiveMarkerServer(self._point_set+"_zones")
        self._edge_server = InteractiveMarkerServer(self._point_set+"_edges")

        self.map_edges = MarkerArray()
        self.map_nodes = MarkerArray()
        self.node_zone = MarkerArray()

        self._create_interactive_markers()
        self._create_marker_array()
        self._create_edges_array()
        self._create_vertices_array()
        
        self.update_needed=False

Example #15

File: visualise_map.py Project: gpdas/topological_navigation

    def _update_everything(self):

        print "loading file from map %s" % self._point_set
        self.topo_map = topological_map(self._point_set)
        print "Done"

        self._marker_server = InteractiveMarkerServer(self._point_set +
                                                      "_markers")
        self._vertex_server = InteractiveMarkerServer(self._point_set +
                                                      "_zones")
        self._edge_server = InteractiveMarkerServer(self._point_set + "_edges")

        self.map_edges = MarkerArray()
        self.map_nodes = MarkerArray()
        self.node_zone = MarkerArray()

        self._create_interactive_markers()
        self._create_marker_array()
        self._create_edges_array()
        self._create_vertices_array()

        self.update_needed = False

Example #16

    def update_map(self, map_name):

        self.topo_map = topological_map(self.map_name)

        self.map_edges = MarkerArray()

        total = len(self.route_nodes.source)

        if total > 0:
            self.maxval = numpy.nanmax(self.route_nodes.prob) + 0.000001
            #self.minval= numpy.min(self.route_nodes.prob)-0.000001
        else:
            self.maxval = 0
            self.minval = 0
        counter = 0
        total = len(self.route_nodes.source)

        while counter < total:
            inds = self.topo_map._get_node_index(
                self.route_nodes.source[counter])
            indt = self.topo_map._get_node_index(
                self.route_nodes.target[counter])
            point1 = Point()
            point2 = Point()
            point1 = (self.topo_map.nodes[inds]._get_pose()).position
            point2 = (self.topo_map.nodes[indt]._get_pose()).position
            val = self.route_nodes.prob[counter]
            #val = val/maxval
            if not math.isnan(val):
                self.create_edge(point1, point2, val)
            counter += 1

        idn = 0
        for m in self.map_edges.markers:
            m.id = idn
            idn += 1
        self.updating = False

Example #17

File: rm_map_from_db.py Project: Vesperal-Hunter/workspace_for_master_project

    def __init__(self, pointset) :

        self.topo_map = topological_map(pointset)
        self.topo_map.delete_map()           
        rospy.loginfo("All Done ...")

Example #18

File: joy_add_node.py Project: Vesperal-Hunter/workspace_for_master_project

 def MapCallback(self, msg) :
     self.topo_map = topological_map(msg.name, msg=msg)
     self.map_received =True

Example #19

File: joy_add_node.py Project: BlenderCN-Org/workspace_for_master_project

 def MapCallback(self, msg):
     self.topo_map = topological_map(msg.name, msg=msg)
     self.map_received = True

Example #20

File: rm_map_from_db.py Project: shuishida/aims_ori_wheel

    def __init__(self, pointset):

        self.topo_map = topological_map(pointset)
        self.topo_map.delete_map()
        rospy.loginfo("All Done ...")

Example #21

    def __init__(self, pointset, or_waypoint, de_waypoint, action):

        self.topo_map = topological_map(pointset)
        self.topo_map.add_edge(or_waypoint, de_waypoint, action)
        rospy.loginfo("All Done ...")

Example #22

File: add_edge.py Project: Vesperal-Hunter/workspace_for_master_project

    def __init__(self, pointset, or_waypoint, de_waypoint, action) :

        self.topo_map = topological_map(pointset)
        self.topo_map.add_edge(or_waypoint, de_waypoint, action)           
        rospy.loginfo("All Done ...")

Example #23

File: node_manager.py Project: shuishida/aims_ori_wheel

 def update_map(self, msg) :
     print "updating node controllers..."
     self.topo_map = topological_map(msg.name, msg=msg)
     self._node_server.clear()
     self._node_server.applyChanges()
     self.create_marker()

Example #24

File: node_manager.py Project: hawesie/strands_navigation

 def update_map(self, msg) :
     print "updating node controllers..."
     self.topo_map = topological_map(msg.name, msg=msg)
     self._node_server.clear()
     self._node_server.applyChanges()
     self.create_marker()