piksi_driver.py 文件源码

def publish_odom(self):
        if self.last_baseline is None or self.last_vel is None:
            return

        if self.last_baseline.tow == self.last_vel.tow:
            self.odom_msg.header.stamp = rospy.Time.now()

            self.odom_msg.pose.pose.position.x = self.last_baseline.e/1000.0
            self.odom_msg.pose.pose.position.y = self.last_baseline.n/1000.0
            self.odom_msg.pose.pose.position.z = -self.last_baseline.d/1000.0

            self.odom_msg.twist.twist.linear.x = self.last_vel.e/1000.0
            self.odom_msg.twist.twist.linear.y = self.last_vel.n/1000.0
            self.odom_msg.twist.twist.linear.z = -self.last_vel.d/1000.0

            self.pub_odom.publish(self.odom_msg)