def planning(self, sx, sy, gx, gy): """ A star path search input: s_x: start x position [m] s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] output: rx: x position list of the final path ry: y position list of the final path """ start_node = self.Node(self.calc_xy_index(sx, self.min_x), self.calc_xy_index(sy, self.min_y), 0.0, -1) goal_node = self.Node(self.calc_xy_index(gx, self.min_x), self.calc_xy_index(gy, self.min_y), 0.0, -1) open_set, closed_set = dict(), dict() open_set[self.calc_grid_index(start_node)] = start_node
评论