def 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}]
width = _map.info.width
height = _map.info.height
start_num = maplib.position_num(_map, start)
end_num = maplib.position_num(_map, goal.position)
Map_ = JPS_GenerateMap(_map.data, width, height)
Open_list = {
start_num: {
'LeftDown': False,
'LeftUp': False,
'RightDown': False,
'RightUp': False,
'Right': False,
'Left': False,
'Up': False,
'Down': False
}
}
visited_list = []
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
def MessPoses(self, poses):
with self.locker:
if len(poses.poses) > 0:
self.Map.data = copy.deepcopy(self.init_map.data)
#print 'MessPoses'
for pose in poses.poses:
num = maplib.position_num(self.Map, pose.position)
self.Map.data[num] = 100
self.map_pub.publish(self.Map)
def 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}]
width = _map.info.width
height = _map.info.height
start_num = maplib.position_num(_map, start)
end_num = maplib.position_num(_map, goal.position)
Map_ = JPS_GenerateMap(_map.data, width, height)
Open_list = {start_num:{'LeftDown' : False, 'LeftUp' : False, 'RightDown' : False, 'RightUp' : False, 'Right' : False, 'Left' : False, 'Up' : False, 'Down' : False}}
visited_list = []
def GoalAchieveable(self, _map, goal):
print type(_map),_map.info.resolution
num = maplib.position_num(_map, goal)
if _map.data[num] != 100:
for i in range(self.radius):
for j in range(self.radius):
if _map.data[_num +i * _map.info.width + j] != 100 and _map.data[_num + i * _map.info.width - j] != 100 and _map.data[_num - i * _map.info.width + j] != 100 and _map.data[_num - i * _map.info.width - j] != 100:
pass
else:
rospy.loginfo('Goal not achieveable')
return False
return True
def GoalAchieveable(self, _map, goal):
print type(_map), _map.info.resolution
num = maplib.position_num(_map, goal)
if _map.data[num] != 100:
for i in range(self.radius):
for j in range(self.radius):
if _map.data[_num + i * _map.info.width +
j] != 100 and _map.data[
_num + i * _map.info.width -
j] != 100 and _map.data[
_num - i * _map.info.width +
j] != 100 and _map.data[
_num - i * _map.info.width -
j] != 100:
pass
else:
rospy.loginfo('Goal not achieveable')
return False
return True
def Crash(self, _map, data):
for i in data:
num = maplib.position_num(_map, i)
if _map.data[num] == 100:
return True
def MessPosition(self, point):
with self.locker:
num = maplib.position_num(self.Map, point.point)
self.Map.data[num] = 100
self.test_map.publish(self.Map)
def MessPosition(self, point): with self.locker: num = maplib.position_num(self.Map, point.point) self.Map.data[num]=100 self.test_map.publish(self.Map)
def Crash(self, _map, data):
for i in data:
num = maplib.position_num(_map, i)
if _map.data[num] == 100:
return True