ros_ekf.py 文件源码

def publishPosition(self):
        msg = PointStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = '/target_' + str(self.tag)

        msg.point = Point(self.estimated_position[0], self.estimated_position[1], 0)
        self.position_publisher.publish(msg)
        self.publishCovarianceMatrix()