def Simple_JPS_Planner(_map, start, goal): #_map : [] grid map #start: position #goal : pose #Open_list = [current jump point1: {'LeftDown': False, 'LeftUp': False, # 'RightDown': False, 'RightUp': False, # 'Right': False, 'Left': False, # 'Up': False, 'Down': False}, # current jump point2: {'LeftDown': False, 'LeftUp': False, # 'RightDown': False, 'RightUp': False, # 'Right': False, 'Left': False, # 'Up': False, 'Down': False}] import JPS width = _map.info.width height = _map.info.height start_num = maplib.position_num(_map, start) end_num = maplib.position_num(_map, goal.position) DENSITY = 5 raw_field = JPS_GenerateMap(_map.data, width, height) field = JPS.generate_field( raw_field, (lambda cell: True if cell > DENSITY else False), True) (start_row, start_colomn, end_row, end_colomn) = Generate_Cor(start_num, end_num) path = JPS.jps(field, start_row, start_colomn, end_row, end_colomn) path = JPS.get_full_path(path) print len(path) return path
def Simple_JPS_Planner(_map, start, goal): #_map : [] grid map #start: position #goal : pose #Open_list = [current jump point1: {'LeftDown': False, 'LeftUp': False, # 'RightDown': False, 'RightUp': False, # 'Right': False, 'Left': False, # 'Up': False, 'Down': False}, # current jump point2: {'LeftDown': False, 'LeftUp': False, # 'RightDown': False, 'RightUp': False, # 'Right': False, 'Left': False, # 'Up': False, 'Down': False}] import JPS width = _map.info.width height = _map.info.height start_num = maplib.position_num(_map, start) end_num = maplib.position_num(_map, goal.position) DENSITY = 5 raw_field = JPS_GenerateMap(_map.data, width, height) field = JPS.generate_field(raw_field, (lambda cell: True if cell > DENSITY else False), True) (start_row, start_colomn, end_row, end_colomn) = Generate_Cor(start_num, end_num) path = JPS.jps(field, start_row, start_colomn, end_row, end_colomn) path = JPS.get_full_path(path) print len(path) return path