import rospi
from orientation.msg import Orientation
from Adafruit_BNO055 import BNO055

pub = rospy.Publisher('orientation', Orientation, queue_size=10)
rospy.init_node('orientation_node', anonymous=True)
rospy.loginfo("Starting orientation node")
rate = rospy.Rate(50)
bno = BNO055.BNO055(serial_port='/dev/serial0')#, rst=18)
# Initialize the BNO055 and stop if something went wrong.
if not bno.begin():
    raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')

while not rospy.is_shutdown():
	heading, roll, pitch = bno.read_euler()
	orient=Orientation()
	orient.pitch=pitch
	orient.roll=roll
	orient.yaw=heading
	#info_str = 'Y{ 0:0.2F } R={ 1:0.2F } P={ 2:0.2F }'.format(heading, roll, pitch)
	#rospy.loginfo(info_str)
	pub.publish(orient)
	rate.sleep()
