KeyPointsManager.py 文件源码

def add(self, marker):
        for i in range(len(self.keyPointList)):
            if (self.keyPointList[i].id==marker.id):
                return
        position = self.transformMarkerToWorld(marker)
        k = KeyPoint(marker.id, Point(position[0], position[1], position[2]), Quaternion(0.,  0.,  0.,  1.))
        self.keyPointList.append(k)
        self.addWaypointMarker(k)
        rospy.loginfo('Marker is added to following position')
        rospy.loginfo(k.pose)
        pass