path_following.py 文件源码

def find_retreat_positions(self):

        total_num_position = 1
        retreat_positions = []
        current_segment = -1
        offset_distance = 1

        current_retrace_length = 0

        while offset_distance < total_num_position + 1:
            length = math.hypot(self.computed_path[current_segment][0] - self.computed_path[current_segment - 1][0],
                                self.computed_path[current_segment][1] - self.computed_path[current_segment - 1][1])

            while offset_distance < current_retrace_length + length and offset_distance < total_num_position + 1:
                proportion = (offset_distance - current_retrace_length) / length
                new_position = list(self.computed_path[current_segment]
                                    + (np.array(self.computed_path[current_segment - 1])
                                       - self.computed_path[current_segment]) * proportion)

                retreat_positions.append(new_position)
                offset_distance += 1

            current_retrace_length += length

        return retreat_positions