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"
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)
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)
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 ...")
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 ...")
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)
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 ...")
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 ...")
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)
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)
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)
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
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
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
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
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
def __init__(self, pointset) : self.topo_map = topological_map(pointset) self.topo_map.delete_map() rospy.loginfo("All Done ...")
def MapCallback(self, msg) : self.topo_map = topological_map(msg.name, msg=msg) self.map_received =True
def MapCallback(self, msg): self.topo_map = topological_map(msg.name, msg=msg) self.map_received = True
def __init__(self, pointset): self.topo_map = topological_map(pointset) self.topo_map.delete_map() rospy.loginfo("All Done ...")
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 ...")
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 ...")
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()