Orientation error, add covariance as parameters, parameter for frame_id

CMakeLists.txt

@@ -8,6 +8,9 @@ project(ros_bno08x)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED)
+#message_generation
+#std_msgs
+#)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -50,11 +53,10 @@ find_package(catkin REQUIRED)
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+ #add_service_files(
+ #  FILES
+ #  doCalibration.srv
+ #)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -64,10 +66,10 @@ find_package(catkin REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+ #generate_messages(
+ #  DEPENDENCIES
+ #  std_msgs  # Or other packages containing msgs
+ #)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

launches/bno08x.launch

@@ -1,4 +1,7 @@
 <launch>
-    <node name="ros_bno08x" pkg="ros_bno08x" type="talker.py" respawn="true" respawn_delay="2">
+    <node name="ros_bno08x" pkg="ros_bno08x" type="talker.py" respawn="true" respawn_delay="2" output="screen">
+      <param name="frame_id" type="string" value="imu_frame" />
+      <param name="rotation_vector" type="bool" value="True" />
+      <param name="geomag_vector" type="bool" value="True" />
     </node>
 </launch>

package.xml

@@ -49,7 +49,8 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-
+<!--  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/bno08x_simpletest.py

@@ -13,7 +13,7 @@
 from adafruit_bno08x.i2c import BNO08X_I2C
 
 i2c = busio.I2C(board.SCL, board.SDA, frequency=800000)
-bno = BNO08X_I2C(i2c,address=0x4b) # BNO080 (0x4b) BNO085 (0x4a)
+bno = BNO08X_I2C(i2c,address=0x4a) # BNO080 (0x4b) BNO085 (0x4a)
 
 bno.enable_feature(BNO_REPORT_ACCELEROMETER)
 bno.enable_feature(BNO_REPORT_GYROSCOPE)

src/calibrate.py

@@ -0,0 +1,55 @@
+# SPDX-FileCopyrightText: 2020 Bryan Siepert, written for Adafruit Industries
+#
+# SPDX-License-Identifier: Unlicense
+import time
+import board
+import busio
+from digitalio import DigitalInOut
+import adafruit_bno08x
+from adafruit_bno08x.i2c import BNO08X_I2C
+
+i2c = busio.I2C(board.SCL, board.SDA)
+bno = BNO08X_I2C(i2c,address=0x4a)
+
+bno.begin_calibration()
+# TODO: UPDATE UART/SPI
+bno.enable_feature(adafruit_bno08x.BNO_REPORT_MAGNETOMETER)
+bno.enable_feature(adafruit_bno08x.BNO_REPORT_GAME_ROTATION_VECTOR)
+start_time = time.monotonic()
+calibration_good_at = None
+while True:
+    time.sleep(0.1)
+
+    print("Magnetometer:")
+    mag_x, mag_y, mag_z = bno.magnetic  # pylint:disable=no-member
+    print("X: %0.6f  Y: %0.6f Z: %0.6f uT" % (mag_x, mag_y, mag_z))
+    print("")
+
+    print("Game Rotation Vector Quaternion:")
+    (
+        game_quat_i,
+        game_quat_j,
+        game_quat_k,
+        game_quat_real,
+    ) = bno.game_quaternion  # pylint:disable=no-member
+    print(
+        "I: %0.6f  J: %0.6f K: %0.6f  Real: %0.6f"
+        % (game_quat_i, game_quat_j, game_quat_k, game_quat_real)
+    )
+    calibration_status = bno.calibration_status
+    print(
+        "Magnetometer Calibration quality:",
+        adafruit_bno08x.REPORT_ACCURACY_STATUS[calibration_status],
+        " (%d)" % calibration_status,
+    )
+    if not calibration_good_at and calibration_status >= 2:
+        calibration_good_at = time.monotonic()
+    if calibration_good_at and (time.monotonic() - calibration_good_at > 5.0):
+        input_str = input("\n\nEnter S to save or anything else to continue: ")
+        if input_str.strip().lower() == "s":
+            bno.save_calibration_data()
+            break
+        calibration_good_at = None
+    print("**************************************************************")
+
+print("calibration done")

src/talker.py

@@ -9,82 +9,160 @@
 import sys
 import board
 import busio
+import adafruit_bno08x
 from adafruit_bno08x import (
     BNO_REPORT_ACCELEROMETER,
     BNO_REPORT_GYROSCOPE,
     BNO_REPORT_MAGNETOMETER,
     BNO_REPORT_ROTATION_VECTOR,
+    BNO_REPORT_GYRO_INTEGRATED_ROTATION_VECTOR,
+    BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR
 )
 from adafruit_bno08x.i2c import BNO08X_I2C
 
-def bno08x_node():
+class bno08x:
+
+    def __init__(self):
+        self.i2c = busio.I2C(board.SCL, board.SDA, frequency=800000)
+        self.bno = BNO08X_I2C(self.i2c,address=0x4a) # BNO080 (0x4b) BNO085 (0x4a)
+        self.bno08x_node_init()
+        self.bno08x_node()
+
+    def bno08x_node_init(self):
+        # Initialize ROS node
+        rospy.init_node('bno08x')
+        # load covariance from parameter
+        self.cov_linear = rospy.get_param('~cov_linear', -1)
+        self.cov_angular = rospy.get_param('~cov_angular', -1)
+        self.cov_orientation = rospy.get_param('~cov_orientation', -1)
+        self.cov_magnetic = rospy.get_param('~cov_magnetic', -1)
+
+        # load additional parameter
+        self.frame_id = rospy.get_param('~frame_id', 'imu')
+        self.rotation_vector = rospy.get_param('~rotation_vector', True)
+        self.geomag_vector = rospy.get_param('~geomag_vector', True)
+
+        # define publisher
+        if self.rotation_vector == True:
+            self.raw_pub = rospy.Publisher('bno08x/raw', Imu, queue_size=10)
+        if self.geomag_vector == True:
+            self.geo_pub = rospy.Publisher('bno08x/geo_raw', Imu, queue_size=10)
+
+        self.mag_pub = rospy.Publisher('bno08x/mag', MagneticField, queue_size=10)
+        self.status_pub = rospy.Publisher('bno08x/status', DiagnosticStatus, queue_size=10)
+
+
+        self.rate = rospy.Rate(20) # frequency in Hz
+        rospy.loginfo("bno08x node launched.")
+
+        self.calib_status = 0
+
+        self.bno.enable_feature(BNO_REPORT_ACCELEROMETER)
+        self.bno.enable_feature(BNO_REPORT_GYROSCOPE)
+        self.bno.enable_feature(BNO_REPORT_MAGNETOMETER)
+        if self.rotation_vector == True:
+            self.bno.enable_feature(BNO_REPORT_ROTATION_VECTOR)
+        if self.geomag_vector == True:    
+            self.bno.enable_feature(BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR)
+
+        time.sleep(0.5) # ensure IMU is initialized
+
+    def bno08x_node(self):
+        while not rospy.is_shutdown():
+
+            if self.rotation_vector == True:
+                self.publishIMU(BNO_REPORT_ROTATION_VECTOR)
+
+            if self.geomag_vector == True:
+                self.publishIMU(BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR)
+
+            mag_msg = MagneticField()
+            mag_x, mag_y, mag_z = self.bno.magnetic
+            mag_msg.header.stamp = rospy.Time.now()
+            mag_msg.magnetic_field.x = mag_x
+            mag_msg.magnetic_field.y = mag_y
+            mag_msg.magnetic_field.z = mag_z
 
-    # Initialize ROS node
-    raw_pub = rospy.Publisher('bno08x/raw', Imu, queue_size=10)
-    mag_pub = rospy.Publisher('bno08x/mag', MagneticField, queue_size=10)
-    status_pub = rospy.Publisher('bno08x/status', DiagnosticStatus, queue_size=10)
-    rospy.init_node('bno08x')
-    rate = rospy.Rate(100) # frequency in Hz
-    rospy.loginfo(rospy.get_caller_id() + "  bno08x node launched.")
+            mag_msg.magnetic_field_covariance[0] = -1
+            if self.cov_magnetic != -1:
+                mag_msg.magnetic_field_covariance[0] = self.cov_magnetic
+                mag_msg.magnetic_field_covariance[4] = self.cov_magnetic
+                mag_msg.magnetic_field_covariance[8] = self.cov_magnetic
 
-    i2c = busio.I2C(board.SCL, board.SDA, frequency=800000)
-    bno = BNO08X_I2C(i2c,address=0x4b) # BNO080 (0x4b) BNO085 (0x4a)
+            self.mag_pub.publish(mag_msg)
+
+            self.calib_status = self.bno.calibration_status
+            status_msg = DiagnosticStatus()
+            status_msg.name = "bno08x IMU"
+
+            status_msg.message ="Magnetometer Calibration quality:" + adafruit_bno08x.REPORT_ACCURACY_STATUS[self.calib_status]
+            if self.calib_status == 0:
+                status_msg.level = 1
+            else:
+                status_msg.level = 0
 
-    bno.enable_feature(BNO_REPORT_ACCELEROMETER)
-    bno.enable_feature(BNO_REPORT_GYROSCOPE)
-    bno.enable_feature(BNO_REPORT_MAGNETOMETER)
-    bno.enable_feature(BNO_REPORT_ROTATION_VECTOR)
+            self.status_pub.publish(status_msg)
+
+            self.rate.sleep()   
 
-    time.sleep(0.5) # ensure IMU is initialized
+        rospy.loginfo(rospy.get_caller_id() + "  bno08x node finished")
 
-    while True:
+    def publishIMU(self, vector_type):
         raw_msg = Imu()
         raw_msg.header.stamp = rospy.Time.now()
+        raw_msg.header.frame_id = self.frame_id
 
-        accel_x, accel_y, accel_z = bno.acceleration
+        accel_x, accel_y, accel_z = self.bno.acceleration
         raw_msg.linear_acceleration.x = accel_x
         raw_msg.linear_acceleration.y = accel_y
         raw_msg.linear_acceleration.z = accel_z
 
-        gyro_x, gyro_y, gyro_z = bno.gyro
+        gyro_x, gyro_y, gyro_z = self.bno.gyro
         raw_msg.angular_velocity.x = gyro_x
         raw_msg.angular_velocity.y = gyro_y
         raw_msg.angular_velocity.z = gyro_z
-
-        quat_i, quat_j, quat_k, quat_real = bno.quaternion
-        raw_msg.orientation.w = quat_i
-        raw_msg.orientation.x = quat_j
-        raw_msg.orientation.y = quat_k
-        raw_msg.orientation.z = quat_real
 
-
         raw_msg.orientation_covariance[0] = -1
         raw_msg.linear_acceleration_covariance[0] = -1
         raw_msg.angular_velocity_covariance[0] = -1
 
-        raw_pub.publish(raw_msg)
+        if self.cov_orientation != -1:
+            raw_msg.orientation_covariance[0] = self.cov_orientation
+            raw_msg.orientation_covariance[4] = self.cov_orientation
+            raw_msg.orientation_covariance[8] = self.cov_orientation
+
+
+        if self.cov_linear != -1:
+            raw_msg.linear_acceleration_covariance[0] = self.cov_linear
+            raw_msg.linear_acceleration_covariance[4] = self.cov_linear
+            raw_msg.linear_acceleration_covariance[8] = self.cov_linear
 
-        mag_msg = MagneticField()
-        mag_x, mag_y, mag_z = bno.magnetic
-        mag_msg.header.stamp = rospy.Time.now()
-        mag_msg.magnetic_field.x = mag_x
-        mag_msg.magnetic_field.y = mag_y
-        mag_msg.magnetic_field.z = mag_z
-        mag_msg.magnetic_field_covariance[0] = -1
-        mag_pub.publish(mag_msg)
+        if self.cov_angular != -1:
+            raw_msg.angular_velocity_covariance[0] = self.cov_angular
+            raw_msg.angular_velocity_covariance[4] = self.cov_angular
+            raw_msg.angular_velocity_covariance[8] = self.cov_angular
+
+        if vector_type == BNO_REPORT_ROTATION_VECTOR:
+            quat_i, quat_j, quat_k, quat_real = self.bno.quaternion
+
+        if vector_type == BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR:
+            quat_i, quat_j, quat_k, quat_real = self.bno.geomagnetic_quaternion
 
-        status_msg = DiagnosticStatus()
-        status_msg.level = 0
-        status_msg.name = "bno08x IMU"
-        status_msg.message = ""
-        status_pub.publish(status_msg)
+        raw_msg.orientation.w = quat_real
+        raw_msg.orientation.x = quat_i
+        raw_msg.orientation.y = quat_j
+        raw_msg.orientation.z = quat_k
+
+        if vector_type == BNO_REPORT_ROTATION_VECTOR:
+            self.raw_pub.publish(raw_msg)
+
+        if vector_type == BNO_REPORT_GEOMAGNETIC_ROTATION_VECTOR:
+            self.geo_pub.publish(raw_msg)
+
 
-        rate.sleep()   
-
-    rospy.loginfo(rospy.get_caller_id() + "  bno08x node finished")
 
 if __name__ == '__main__':
     try:
-        bno08x_node()
+        bno08x()
     except rospy.ROSInterruptException:
         rospy.loginfo(rospy.get_caller_id() + "  bno08x node exited with exception.")