piksi_driver.py 文件源码

def callback_sbp_base_pos(self, msg, **metadata):

        if self.debug:
            rospy.loginfo("Received SBP_MSG_BASE_POS (Sender: %d): %s" % (msg.sender, repr(msg)))

        if self.send_observations:
            for s in self.obs_senders:
                s.send(msg)

        # publish tf for rtk frame
        if self.publish_utm_rtk_tf:
            if not self.proj:
                self.init_proj((msg.lat, msg.lon))

            E,N = self.proj(msg.lon,msg.lat, inverse=False)

            self.transform.header.stamp = rospy.Time.now()
            self.transform.transform.translation.x = E
            self.transform.transform.translation.y = N
            self.transform.transform.translation.z = -msg.height
            self.tf_br.sendTransform(self.transform)