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)