nav_asr_6.py 文件源码

def GetWayPoints(self,Data):
        filename = Data.data
        #clear all existing marks in rviz
        for marker in self.makerArray.markers:
            marker.action = Marker.DELETE
        self.makerArray_pub.publish(self.makerArray)

        # clear former lists
        self.waypoint_list.clear()
        self.marker_list[:] = []
        self.makerArray.markers[:] = []

        f = open(filename,'r')
        for line in f.readlines():
            j = line.split(",")
            current_wp_name     = str(j[0])        
            current_point       = Point(float(j[1]),float(j[2]),float(j[3]))
            current_quaternion  = Quaternion(float(j[4]),float(j[5]),float(j[6]),float(j[7]))

            self.waypoint_list[current_wp_name] = Pose(current_point,current_quaternion)

        f.close()

        self.create_markers()
        self.makerArray_pub.publish(self.makerArray)