def __init__(self):
        print("Graph Search Service Started")

        # Input: csv file
        self.map_name = rospy.get_param("/map_name")

        # Loading map
        self.script_dir = os.path.dirname(__file__)
        self.map_path = self.script_dir + "/maps/" + self.map_name
        self.map_img = self.script_dir + "/maps/map.png"

        gc = graph_creator()
        self.duckietown_graph = gc.build_graph_from_csv(csv_filename=self.map_name)
        self.duckietown_problem = GraphSearchProblem(self.duckietown_graph, None, None)

        print("Map loaded successfully.\n")

        self.image_pub = rospy.Publisher("~map_graph", Image, queue_size=1, latch=True)
        self.bridge = CvBridge()

        # Send graph through publisher
        self.duckietown_graph.draw(self.script_dir, highlight_edges=None, map_name=self.map_name)
        cv_image = cv2.imread(self.map_path + ".png", cv2.IMREAD_COLOR)
        overlay = self.prepImage(cv_image)
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(overlay, "bgr8"))
    def __init__(self):
        print 'Graph Search Service Started'

        # Input: csv file
        self.map_name = rospy.get_param('/map_name')

        # Loading map
        self.script_dir = os.path.dirname(__file__)
        self.map_path = self.script_dir + '/maps/' + self.map_name
        self.map_img = self.script_dir + '/maps/map.png'

        gc = graph_creator()
        self.duckietown_graph = gc.build_graph_from_csv(csv_filename=self.map_name)
        self.duckietown_problem = GraphSearchProblem(self.duckietown_graph, None, None)
    
        print "Map loaded successfully!\n"

        self.image_pub = rospy.Publisher("~map_graph",Image, queue_size = 1, latch=True)
        self.bridge = CvBridge()

        # Send graph through publisher
        self.duckietown_graph.draw(self.script_dir, highlight_edges=None, map_name = self.map_name)
        cv_image = cv2.imread(self.map_path + '.png', cv2.CV_LOAD_IMAGE_COLOR)
        overlay = self.prepImage(cv_image)
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(overlay, "bgr8"))
    def loadComboBoxItems(self):
        # Loading map
        self.map_name = rospy.get_param('/map_name', 'tiles_226')
        self.script_dir = os.path.dirname(__file__)
        self.map_path = self.script_dir + '/../../src/maps/' + self.map_name
        gc = graph_creator()
        gc.build_graph_from_csv(csv_filename=self.map_name)

        node_locations = gc.node_locations
        #comboBoxList = sorted([int(key) for key in node_locations if key[0:4]!='turn'])
        comboBoxList = []
        for key in node_locations:
            if key[0:4] == 'turn':
                continue
            elif int(key) % 2 == 0: # allows only selection of odd numbered nodes
                continue
            comboBoxList += [int(key)]
        comboBoxList = sorted(comboBoxList)
        comboBoxList = [str(key) for key in comboBoxList]
        self._widget.comboBoxDestination.addItems(comboBoxList)
        self._widget.comboBoxStart.addItems(comboBoxList)
    def loadComboBoxItems(self):
        # Loading map
        self.map_name = rospy.get_param('/map_name', 'tiles_226')
        self.script_dir = os.path.dirname(__file__)
        self.map_path = self.script_dir + '/../../src/maps/' + self.map_name
        gc = graph_creator()
        gc.build_graph_from_csv(csv_filename=self.map_name)

        node_locations = gc.node_locations
        #comboBoxList = sorted([int(key) for key in node_locations if key[0:4]!='turn'])
        comboBoxList = []
        for key in node_locations:
            if key[0:4] == 'turn':
                continue
            elif int(key
                     ) % 2 == 0:  # allows only selection of odd numbered nodes
                continue
            comboBoxList += [int(key)]
        comboBoxList = sorted(comboBoxList)
        comboBoxList = [str(key) for key in comboBoxList]
        self._widget.comboBoxDestination.addItems(comboBoxList)
        self._widget.comboBoxStart.addItems(comboBoxList)
Example #5
0
    def __init__(self):
        rospy.loginfo('Graph Search Service Started')

        # Input: csv file
        self.map_name = rospy.get_param('/map_name')

        # Loading map
        self.script_dir = os.path.dirname(__file__)
        self.map_path = self.script_dir + '/maps/' + self.map_name
        self.map_img = self.script_dir + '/maps/map.png'

        gc = graph_creator()
        self.duckietown_graph = gc.build_graph_from_csv(csv_filename=self.map_name)
        self.duckietown_problem = GraphSearchProblem(self.duckietown_graph, None, None)             
        print "Map loaded successfully!\n"

	self.current_node = 0
	
        #Subscribers
        
        #road block detected
        self.sub_rb = rospy.Subscriber('~road_blocked', BoolStamped, self.handle_road_blocked)
        #duckie is turning; propagate now
        self.sub_turn = rospy.Subscriber("~turn_type", Int16, self.propagate_location)        
        
	#Publishers
	#publish to here to request an a* search from ActionsDispatcherNode
	self.pub_plan_request = rospy.Publisher("~plan_request", SourceTargetNodes, queue_size = 1)	
	
        self.image_pub = rospy.Publisher("~map_graph",Image, queue_size = 1, latch=True)
        self.bridge = CvBridge()

        # Send graph through publisher
        self.duckietown_graph.draw(self.script_dir, highlight_edges=None, map_name = self.map_name)
        cv_image = cv2.imread(self.map_path + '.png', cv2.CV_LOAD_IMAGE_COLOR)
        overlay = self.prepImage(cv_image)
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(overlay, "bgr8"))