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
