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