def publisher():
dataPub = rospy.Publisher('rov/bno055', Imu, queue_size=3)
infoPub = rospy.Publisher('rov/bno055_info', bno055_info, queue_size=3)
rospy.init_node('bno055')
rate = rospy.Rate(30) #30Hz data read
# Setup BNO055
# Create and configure the BNO sensor connection.
# Raspberry Pi configuration with I2C and RST connected to GPIO 27:
sensor = BNO055.BNO055(rst=27)
attempts = 0
# Initialize the BNO055 and stop if something went wrong.
while(attempts < 10):
try:
sensor.begin()
break
except Exception as e:
attempts += 1
rospy.sleep(0.25)
if(attempts == 10):
rospy.logerr('Failed to initialize BNO055! Program end')
exit(1)
# Print system status and self test result.
try:
status, self_test, error = sensor.get_system_status()
except Exception as e:
rospy.logerr('Failed to read BNO055 system status! %s', e)
rospy.logdebug('System status: %s', status)
rospy.logdebug('Self test result (0x0F is normal): %s', hex(self_test))
# Print out an error if system status is in error mode.
if(status == 0x01):
rospy.logerr('System error: %s', error)
rospy.logerr('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
try:
sw, bl, accel, mag, gyro = sensor.get_revision()
except Exception as e:
rospy.logerr('Failed to read BNO055 meta-inforamtion! %s', e)
rospy.loginfo('Software version: %d', sw)
rospy.loginfo('Bootloader version: %d', bl)
rospy.loginfo('Accelerometer ID: %s', hex(accel))
rospy.loginfo('Magnetometer ID: %s', hex(mag))
rospy.loginfo('Gyroscope ID: %s', hex(gyro))
rospy.loginfo('Reading BNO055 data...')
while not rospy.is_shutdown():
# Define messages
msgHeader = Header()
infoHeader = Header()
msg = Imu()
info = bno055_info()
orientation = Quaternion()
angular_vel = Vector3()
linear_accel = Vector3()
#no covarience vlaues known
# Update data
attempts = 0
while(attempts < 4):
try:
# Read the calibration status, 0=uncalibrated and 3=fully calibrated.
sys, gyro, accel, mag = sensor.get_calibration_status()
temp_c = sensor.read_temp()
break
except Exception as e:
rospy.logdebug('Failed to read BNO055 calibration stat and temp! %s', e)
attempts += 1
rospy.sleep(0.01)
if(attempts != 4):
info.sysCalibration = sys
info.accelCalibration = accel
info.gyroCalibration = gyro
info.magnoCalibration = mag
info.tempC = temp_c
attempts = 0
while(attempts < 4):
try:
# Orientation as a quaternion:
orientation.x, orientation.y, orientation.z, orientation.w = sensor.read_quaternion()
# Gyroscope data (in degrees per second converted to radians per second):
gry_x, gry_y, gry_z = sensor.read_gyroscope()
angular_vel.x = math.radians(gry_x)
angular_vel.y = math.radians(gry_y)
angular_vel.z = math.radians(gry_z)
# Linear acceleration data (i.e. acceleration from movement, not gravity--
# returned in meters per second squared):
linear_accel.x, linear_accel.y, linear_accel.z = sensor.read_linear_acceleration()
break
except Exception as e:
rospy.logdebug('Failed to read BNO055 data! %s', e)
attempts += 1
rospy.sleep(0.01)
if(attempts != 4):
msg.orientation = orientation
msg.angular_velocity = angular_vel
msg.linear_acceleration = linear_accel
#update message headers
msgHeader.stamp = rospy.Time.now()
msgHeader.frame_id = 'imu_data'
msg.header = msgHeader
infoHeader.stamp = rospy.Time.now()
infoHeader.frame_id = 'imu_info'
info.header = infoHeader
dataPub.publish(msg)
infoPub.publish(info)
rate.sleep()
def publisher():
rospy.init_node('imu')
dataPub = rospy.Publisher('/imu/data', Imu, queue_size=3)
infoPub = rospy.Publisher('/imu/info', bno055_info, queue_size=3)
headingPub = rospy.Publisher('/imu/heading', Float64, queue_size=3)
load_calibration = rospy.get_param("~load_calibration", False)
rate = rospy.Rate(30) #30Hz data read
# Setup BNO055
# Create and configure the BNO sensor connection.
# Raspberry Pi configuration with I2C and RST connected to GPIO 27:
global sensor
sensor = BNO055.BNO055()
try:
sensor.begin()
except Exception as e:
rospy.logerr('Failed to initialize BNO055! %s', e)
sys.exit(1)
#sensor.set_axis_remap(BNO055.AXIS_REMAP_Y, BNO055.AXIS_REMAP_X, BNO055.AXIS_REMAP_Z) #swap x,y axis
# Print system status and self test result.
try:
status, self_test, error = sensor.get_system_status()
except Exception as e:
rospy.logerr('Failed to read BNO055 system status! %s', e)
sys.exit(2)
rospy.logdebug('System status: %s', status)
rospy.logdebug('Self test result (0x0F is normal): %s', hex(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
rospy.logerr('System error: %s', error)
rospy.logerr('See datasheet section 4.3.59 for the meaning.')
sys.exit(3)
# Print BNO055 software revision and other diagnostic data.
try:
sw, bl, accel, mag, gyro = sensor.get_revision()
except Exception as e:
rospy.logerr('Failed to read BNO055 meta-inforamtion! %s', e)
sys.exit(4)
rospy.loginfo('Software version: %d', sw)
rospy.loginfo('Bootloader version: %d', bl)
rospy.loginfo('Accelerometer ID: %s', hex(accel))
rospy.loginfo('Magnetometer ID: %s', hex(mag))
rospy.loginfo('Gyroscope ID: %s', hex(gyro))
rospy.loginfo('Reading BNO055 data...')
if load_calibration:
try:
sensor.set_calibration(
np.load(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
'calibration.npy')).tolist())
except Exception as e:
rospy.logerr("Error loading calibration data: " + str(e))
save_calibration_srv = rospy.Service('/imu/save_calibration', Trigger,
save_calibration)
while not rospy.is_shutdown():
# Define messages
msg = Imu()
info = bno055_info()
heading = Float64()
orientation = Quaternion()
angular_vel = Vector3()
linear_accel = Vector3()
# Update meta data
attempts = 0
sys_cal = 0
temp_c = 0
while attempts < 4:
try:
# Read the calibration status, 0=uncalibrated and 3=fully calibrated.
sys_cal, gyro, accel, mag = sensor.get_calibration_status()
temp_c = sensor.read_temp()
break
except Exception as e:
rospy.logerr(
'Failed to read BNO055 calibration stat and temp! %s', e)
attempts += 1
rospy.sleep(0.01)
if attempts != 4:
info.sysCalibration = sys_cal
info.accelCalibration = accel
info.gyroCalibration = gyro
info.magnoCalibration = mag
info.tempC = temp_c
# Update real data
attempts = 0
while attempts < 4:
try:
# Orientation as a quaternion:
orientation.x, orientation.y, orientation.z, orientation.w = sensor.read_quaternion(
)
# Gyroscope data (in degrees per second converted to radians per second):
gry_x, gry_y, gry_z = sensor.read_gyroscope()
angular_vel.x = math.radians(gry_x)
angular_vel.y = math.radians(gry_y)
angular_vel.z = math.radians(gry_z)
# Linear acceleration data (i.e. acceleration from movement, not gravity--
# returned in meters per second squared):
linear_accel.x, linear_accel.y, linear_accel.z = sensor.read_linear_acceleration(
)
heading.data = sensor.read_euler()[0]
break
except Exception as e:
rospy.logerr('Failed to read BNO055 data! %s', e)
attempts += 1
rospy.sleep(0.01)
if attempts != 4:
msg.orientation = orientation
msg.orientation_covariance[0] = 0.01 # covariance in x,y,z (3x3)
msg.orientation_covariance[4] = 0.01
msg.orientation_covariance[8] = 0.01
msg.angular_velocity = angular_vel
msg.angular_velocity_covariance[
0] = 0.01 # covariance in x,y,z (3x3)
msg.angular_velocity_covariance[4] = 0.01
msg.angular_velocity_covariance[8] = 0.01
msg.linear_acceleration = linear_accel
msg.linear_acceleration_covariance[
0] = 0.01 # covariance in x,y,z (3x3)
msg.linear_acceleration_covariance[4] = 0.01
msg.linear_acceleration_covariance[8] = 0.01
#update message headers
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'imu_link'
dataPub.publish(msg)
info.header.stamp = rospy.Time.now()
info.header.frame_id = 'imu_link'
infoPub.publish(info)
headingPub.publish(heading)
rate.sleep()