nav_asr_3.py 文件源码

def GetWayPoints(self,Data):
        # dir = os.path.dirname(__file__)
        # filename = dir+'/locationPoint.txt'
        filename = Data.data
        rospy.loginfo(str(filename))
        rospy.loginfo(self.locations)
        self.locations.clear()
        rospy.loginfo(self.locations)

        f = open(filename,'r')

        for i in f.readlines():
            j = i.split(",")
            current_wp_name     = str(j[0])
            rospy.loginfo(current_wp_name)          

            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.locations[current_wp_name] = Pose(current_point,current_quaternion)

        f.close()
        rospy.loginfo(self.locations)

        #Rviz Marker
        self.init_markers()

        self.IsWPListOK = True