x,y,qz,qw,tx,tz = romi.read_pose()
#rospy.loginfo("Raw Pose <= pos %0.3f,%0.3f,%0.3f, quat %0.3f,%0.3f,%0.3f,%0.3f twist %0.4f,%0.4f", x,y,0, 0,0,qz,qw, tx,tz)
romi_odom = Odometry()
romi_odom.header.frame_id = "odom"
romi_odom.header.stamp = rospy.Time.now()
romi_odom.header.seq = sequence
sequence += 1
romi_odom.child_frame_id = "base_link"
romi_odom.pose.pose.position = Point(x=x, y=y, z=0.0)
romi_odom.pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=qz, w=qw)
romi_odom.twist.twist.linear.x = tx
romi_odom.twist.twist.linear.y = 0.0
romi_odom.twist.twist.linear.z = 0.0
romi_odom.twist.twist.angular.x = 0.0
romi_odom.twist.twist.angular.y = 0.0
romi_odom.twist.twist.angular.z = tz
odom_pub.publish(romi_odom)
rospy.loginfo("Odometry %s", romi_odom)
rate.sleep()
if __name__ == '__main__':
try:
romi = AStar()
romipi_i2c()
except rospy.ROSInterruptException:
pass
romi.close()
