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