Merge pull request #43 from flynneva/develop

bring over updates
flynneva · Mar 3, 2022 · 214cd09 · 214cd09
2 parents 2f5df04 + cf7202c
commit 214cd09
Show file tree

Hide file tree

Showing 8 changed files with 151 additions and 50 deletions.
diff --git a/.github/workflows/greetings.yml b/.github/workflows/greetings.yml
@@ -1,6 +1,6 @@
 name: Greetings
 
-on: [pull_request, issues]
+on: [pull_request_target, issues]
 
 jobs:
   greeting:

diff --git a/README.md b/README.md
@@ -56,6 +56,13 @@ See Bosch BNO055 datasheet section "Axis Remap" for valid positions: "P0", "P1"
 
 - **ros_topic_prefix**: ROS topic prefix to be used. Will be prepended to the default topic names (see below). Default="bno055/"
 
+### Calibration
+
+The current calibration values can be requested via the **calibration_request** service (this puts the imu into **CONFIGMODE** for a short time):
+
+```
+ros2 service call /bno055/calibration_request example_interfaces/srv/Trigger
+```
 ---
 ## ROS Topics
 
@@ -121,6 +128,11 @@ Run with customized parameter file:
 
     ros2 run bno055 bno055 --ros-args --params-file ./src/bno055/bno055/params/bno055_params.yaml
 
+Run launch file:
+
+    ros2 launch bno055 bno055.launch.py
+
+
 ### Performing flake8 Linting
 
 To perform code linting with [flake8](https://gitlab.com/pycqa/flake8) just perform:

diff --git a/bno055/params/NodeParameters.py b/bno055/params/NodeParameters.py
@@ -84,6 +84,10 @@ def __init__(self, node: Node):
         node.declare_parameter('offset_mag', value=registers.DEFAULT_OFFSET_MAG)
         # +/- 2000 units up to 32000 (dps range dependent)               (1 unit = 1/16 dps)
         node.declare_parameter('offset_gyr', value=registers.DEFAULT_OFFSET_GYR)
+        # +/-1000 units
+        node.declare_parameter('radius_acc', value=registers.DEFAULT_RADIUS_ACC)
+        #  +/-960 units
+        node.declare_parameter('radius_mag', value=registers.DEFAULT_RADIUS_MAG)
         # Sensor standard deviation squared (^2) defaults [x, y, z]
         node.declare_parameter('variance_acc', value=registers.DEFAULT_VARIANCE_ACC)
         node.declare_parameter('variance_angular_vel', value=registers.DEFAULT_VARIANCE_ANGULAR_VEL)
@@ -142,9 +146,15 @@ def __init__(self, node: Node):
             self.offset_acc = node.get_parameter('offset_acc')
             node.get_logger().info('\toffset_acc:\t\t"%s"' % self.offset_acc.value)
 
+            self.radius_acc = node.get_parameter('radius_acc')
+            node.get_logger().info('\tradius_acc:\t\t"%s"' % self.radius_acc.value)
+
             self.offset_mag = node.get_parameter('offset_mag')
             node.get_logger().info('\toffset_mag:\t\t"%s"' % self.offset_mag.value)
 
+            self.radius_mag = node.get_parameter('radius_mag')
+            node.get_logger().info('\tradius_mag:\t\t"%s"' % self.radius_mag.value)
+
             self.offset_gyr = node.get_parameter('offset_gyr')
             node.get_logger().info('\toffset_gyr:\t\t"%s"' % self.offset_gyr.value)
 

diff --git a/bno055/registers.py b/bno055/registers.py
@@ -246,6 +246,8 @@
 #: +/- 2000 units up to 32000 (dps range dependent)               (1 unit = 1/16 dps)
 DEFAULT_OFFSET_GYR = [0x0002, 0xFFFF, 0xFFFF]
 
+DEFAULT_RADIUS_MAG = 0x0
+DEFAULT_RADIUS_ACC = 0x3E8
 #: Sensor standard deviation squared (^2) defaults [x, y, z]
 #: Used to get covariance matrices (stddev^2 = variance)
 #: values taken from this ROS1 driver from octanis:

diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py
@@ -40,6 +40,7 @@
 from rclpy.qos import QoSProfile
 from sensor_msgs.msg import Imu, MagneticField, Temperature
 from std_msgs.msg import String
+from example_interfaces.srv import Trigger
 
 
 class SensorService:
@@ -59,6 +60,7 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters):
         self.pub_mag = node.create_publisher(MagneticField, prefix + 'mag', QoSProf)
         self.pub_temp = node.create_publisher(Temperature, prefix + 'temp', QoSProf)
         self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', QoSProf)
+        self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback)
 
     def configure(self):
         """Configure the IMU sensor hardware."""
@@ -104,21 +106,26 @@ def configure(self):
             'P7': bytes(b'\x24\x05')
         }
         if not (self.con.transmit(registers.BNO055_AXIS_MAP_CONFIG_ADDR, 2,
-                mount_positions[self.param.placement_axis_remap.value])):
+                                  mount_positions[self.param.placement_axis_remap.value])):
             self.node.get_logger().warn('Unable to set sensor placement configuration.')
 
         # Show the current sensor offsets
-        print('Current sensor offsets:')
-        self.get_calib_offsets()
-        if (self.param.set_offsets):
+        self.node.get_logger().info('Current sensor offsets:')
+        self.print_calib_data()
+        if self.param.set_offsets.value:
             configured_offsets = \
                 self.set_calib_offsets(
                     self.param.offset_acc,
                     self.param.offset_mag,
-                    self.param.offset_gyr)
-            if (configured_offsets):
-                print('Successfully configured sensor offsets to:')
-                self.get_calib_offsets()
+                    self.param.offset_gyr,
+                    self.param.radius_mag,
+                    self.param.radius_acc)
+            if configured_offsets:
+                self.node.get_logger().info('Successfully configured sensor offsets to:')
+                self.print_calib_data()
+            else:
+                self.node.get_logger().warn('setting offsets failed')
+
 
         # Set Device to NDOF mode
         # data fusion for gyroscope, acceleration sensor and magnetometer enabled
@@ -139,15 +146,14 @@ def get_sensor_data(self):
         # read from sensor
         buf = self.con.receive(registers.BNO055_ACCEL_DATA_X_LSB_ADDR, 45)
         # Publish raw data
-        # TODO: convert rcl Clock time to ros time?
-        # imu_raw_msg.header.stamp = node.get_clock().now()
+        imu_raw_msg.header.stamp = self.node.get_clock().now().to_msg()
         imu_raw_msg.header.frame_id = self.param.frame_id.value
         # TODO: do headers need sequence counters now?
         # imu_raw_msg.header.seq = seq
 
         # TODO: make this an option to publish?
         imu_raw_msg.orientation_covariance = [
-            self.param.variance_orientation.value[0], 0.0 , 0.0,
+            self.param.variance_orientation.value[0], 0.0, 0.0,
             0.0, self.param.variance_orientation.value[1], 0.0,
             0.0, 0.0, self.param.variance_orientation.value[2]
         ]
@@ -179,7 +185,7 @@ def get_sensor_data(self):
 
         # TODO: make this an option to publish?
         # Publish filtered data
-        # imu_msg.header.stamp = node.get_clock().now()
+        imu_msg.header.stamp = self.node.get_clock().now().to_msg()
         imu_msg.header.frame_id = self.param.frame_id.value
 
         q = Quaternion()
@@ -190,7 +196,7 @@ def get_sensor_data(self):
         q.z = self.unpackBytesToFloat(buf[30], buf[31])
         # TODO(flynneva): replace with standard normalize() function
         # normalize
-        norm = sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w)
+        norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w)
         imu_msg.orientation.x = q.x / norm
         imu_msg.orientation.y = q.y / norm
         imu_msg.orientation.z = q.z / norm
@@ -203,7 +209,7 @@ def get_sensor_data(self):
         imu_msg.linear_acceleration.y = \
             self.unpackBytesToFloat(buf[34], buf[35]) / self.param.acc_factor.value
         imu_msg.linear_acceleration.z = \
-            self.unpackBytesToFloat( buf[36], buf[37]) / self.param.acc_factor.value
+            self.unpackBytesToFloat(buf[36], buf[37]) / self.param.acc_factor.value
         imu_msg.linear_acceleration_covariance = imu_raw_msg.linear_acceleration_covariance
         imu_msg.angular_velocity.x = \
             self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value
@@ -215,7 +221,7 @@ def get_sensor_data(self):
         self.pub_imu.publish(imu_msg)
 
         # Publish magnetometer data
-        # mag_msg.header.stamp = node.get_clock().now()
+        mag_msg.header.stamp = self.node.get_clock().now().to_msg()
         mag_msg.header.frame_id = self.param.frame_id.value
         # mag_msg.header.seq = seq
         mag_msg.magnetic_field.x = \
@@ -232,7 +238,7 @@ def get_sensor_data(self):
         self.pub_mag.publish(mag_msg)
 
         # Publish temperature
-        # temp_msg.header.stamp = node.get_clock().now()
+        temp_msg.header.stamp = self.node.get_clock().now().to_msg()
         temp_msg.header.frame_id = self.param.frame_id.value
         # temp_msg.header.seq = seq
         temp_msg.temperature = float(buf[44])
@@ -258,8 +264,9 @@ def get_calib_status(self):
         # Publish via ROS topic:
         self.pub_calib_status.publish(calib_status_str)
 
-    def get_calib_offsets(self):
-        """Read all calibration offsets and print to screen."""
+    def get_calib_data(self):
+        """Read all calibration data."""
+
         accel_offset_read = self.con.receive(registers.ACCEL_OFFSET_X_LSB_ADDR, 6)
         accel_offset_read_x = (accel_offset_read[1] << 8) | accel_offset_read[
             0]  # Combine MSB and LSB registers into one decimal
@@ -268,6 +275,9 @@ def get_calib_offsets(self):
         accel_offset_read_z = (accel_offset_read[5] << 8) | accel_offset_read[
             4]  # Combine MSB and LSB registers into one decimal
 
+        accel_radius_read = self.con.receive(registers.ACCEL_RADIUS_LSB_ADDR, 2)
+        accel_radius_read_value = (accel_radius_read[1] << 8) | accel_radius_read[0]
+
         mag_offset_read = self.con.receive(registers.MAG_OFFSET_X_LSB_ADDR, 6)
         mag_offset_read_x = (mag_offset_read[1] << 8) | mag_offset_read[
             0]  # Combine MSB and LSB registers into one decimal
@@ -276,6 +286,9 @@ def get_calib_offsets(self):
         mag_offset_read_z = (mag_offset_read[5] << 8) | mag_offset_read[
             4]  # Combine MSB and LSB registers into one decimal
 
+        mag_radius_read = self.con.receive(registers.MAG_RADIUS_LSB_ADDR, 2)
+        mag_radius_read_value = (mag_radius_read[1] << 8) | mag_radius_read[0]
+
         gyro_offset_read = self.con.receive(registers.GYRO_OFFSET_X_LSB_ADDR, 6)
         gyro_offset_read_x = (gyro_offset_read[1] << 8) | gyro_offset_read[
             0]  # Combine MSB and LSB registers into one decimal
@@ -284,31 +297,54 @@ def get_calib_offsets(self):
         gyro_offset_read_z = (gyro_offset_read[5] << 8) | gyro_offset_read[
             4]  # Combine MSB and LSB registers into one decimal
 
+        calib_data = {'accel_offset': {'x': accel_offset_read_x, 'y': accel_offset_read_y, 'z': accel_offset_read_z}, 'accel_radius': accel_radius_read_value,
+                      'mag_offset': {'x': mag_offset_read_x, 'y': mag_offset_read_y, 'z': mag_offset_read_z}, 'mag_radius': mag_radius_read_value,
+                      'gyro_offset': {'x': gyro_offset_read_x, 'y': gyro_offset_read_y, 'z': gyro_offset_read_z}}
+
+        return calib_data
+
+    def print_calib_data(self):
+        """Read all calibration data and print to screen."""
+        calib_data = self.get_calib_data()
         self.node.get_logger().info(
             '\tAccel offsets (x y z): %d %d %d' % (
-                accel_offset_read_x,
-                accel_offset_read_y,
-                accel_offset_read_z))
+                calib_data['accel_offset']['x'],
+                calib_data['accel_offset']['y'],
+                calib_data['accel_offset']['z']))
+
+        self.node.get_logger().info(
+            '\tAccel radius: %d' % (
+                calib_data['accel_radius'],
+            )
+        )
 
         self.node.get_logger().info(
             '\tMag offsets (x y z): %d %d %d' % (
-                mag_offset_read_x,
-                mag_offset_read_y,
-                mag_offset_read_z))
+                calib_data['mag_offset']['x'],
+                calib_data['mag_offset']['y'],
+                calib_data['mag_offset']['z']))
+
+        self.node.get_logger().info(
+            '\tMag radius: %d' % (
+                calib_data['mag_radius'],
+            )
+        )
 
         self.node.get_logger().info(
             '\tGyro offsets (x y z): %d %d %d' % (
-                gyro_offset_read_x,
-                gyro_offset_read_y,
-                gyro_offset_read_z))
+                calib_data['gyro_offset']['x'],
+                calib_data['gyro_offset']['y'],
+                calib_data['gyro_offset']['z']))
 
-    def set_calib_offsets(self, acc_offset, mag_offset, gyr_offset):
+    def set_calib_offsets(self, acc_offset, mag_offset, gyr_offset, mag_radius, acc_radius):
         """
         Write calibration data (define as 16 bit signed hex).
 
         :param acc_offset:
         :param mag_offset:
         :param gyr_offset:
+        :param mag_radius:
+        :param acc_radius:
         """
         # Must switch to config mode to write out
         if not (self.con.transmit(registers.BNO055_OPR_MODE_ADDR, 1, bytes([registers.OPERATION_MODE_CONFIG]))):
@@ -317,29 +353,47 @@ def set_calib_offsets(self, acc_offset, mag_offset, gyr_offset):
 
         # Seems to only work when writing 1 register at a time
         try:
-            self.con.transmit(registers.ACCEL_OFFSET_X_LSB_ADDR, 1, bytes([acc_offset[0] & 0xFF]))
-            self.con.transmit(registers.ACCEL_OFFSET_X_MSB_ADDR, 1, bytes([(acc_offset[0] >> 8) & 0xFF]))
-            self.con.transmit(registers.ACCEL_OFFSET_Y_LSB_ADDR, 1, bytes([acc_offset[1] & 0xFF]))
-            self.con.transmit(registers.ACCEL_OFFSET_Y_MSB_ADDR, 1, bytes([(acc_offset[1] >> 8) & 0xFF]))
-            self.con.transmit(registers.ACCEL_OFFSET_Z_LSB_ADDR, 1, bytes([acc_offset[2] & 0xFF]))
-            self.con.transmit(registers.ACCEL_OFFSET_Z_MSB_ADDR, 1, bytes([(acc_offset[2] >> 8) & 0xFF]))
-
-            self.con.transmit(registers.MAG_OFFSET_X_LSB_ADDR, 1, bytes([mag_offset[0] & 0xFF]))
-            self.con.transmit(registers.MAG_OFFSET_X_MSB_ADDR, 1, bytes([(mag_offset[0] >> 8) & 0xFF]))
-            self.con.transmit(registers.MAG_OFFSET_Y_LSB_ADDR, 1, bytes([mag_offset[1] & 0xFF]))
-            self.con.transmit(registers.MAG_OFFSET_Y_MSB_ADDR, 1, bytes([(mag_offset[1] >> 8) & 0xFF]))
-            self.con.transmit(registers.MAG_OFFSET_Z_LSB_ADDR, 1, bytes([mag_offset[2] & 0xFF]))
-            self.con.transmit(registers.MAG_OFFSET_Z_MSB_ADDR, 1, bytes([(mag_offset[2] >> 8) & 0xFF]))
-
-            self.con.transmit(registers.GYRO_OFFSET_X_LSB_ADDR, 1, bytes([gyr_offset[0] & 0xFF]))
-            self.con.transmit(registers.GYRO_OFFSET_X_MSB_ADDR, 1, bytes([(gyr_offset[0] >> 8) & 0xFF]))
-            self.con.transmit(registers.GYRO_OFFSET_Y_LSB_ADDR, 1, bytes([gyr_offset[1] & 0xFF]))
-            self.con.transmit(registers.GYRO_OFFSET_Y_MSB_ADDR, 1, bytes([(gyr_offset[1] >> 8) & 0xFF]))
-            self.con.transmit(registers.GYRO_OFFSET_Z_LSB_ADDR, 1, bytes([gyr_offset[2] & 0xFF]))
-            self.con.transmit(registers.GYRO_OFFSET_Z_MSB_ADDR, 1, bytes([(gyr_offset[2] >> 8) & 0xFF]))
+            self.con.transmit(registers.ACCEL_OFFSET_X_LSB_ADDR, 1, bytes([acc_offset.value[0] & 0xFF]))
+            self.con.transmit(registers.ACCEL_OFFSET_X_MSB_ADDR, 1, bytes([(acc_offset.value[0] >> 8) & 0xFF]))
+            self.con.transmit(registers.ACCEL_OFFSET_Y_LSB_ADDR, 1, bytes([acc_offset.value[1] & 0xFF]))
+            self.con.transmit(registers.ACCEL_OFFSET_Y_MSB_ADDR, 1, bytes([(acc_offset.value[1] >> 8) & 0xFF]))
+            self.con.transmit(registers.ACCEL_OFFSET_Z_LSB_ADDR, 1, bytes([acc_offset.value[2] & 0xFF]))
+            self.con.transmit(registers.ACCEL_OFFSET_Z_MSB_ADDR, 1, bytes([(acc_offset.value[2] >> 8) & 0xFF]))
+
+            self.con.transmit(registers.ACCEL_RADIUS_LSB_ADDR, 1, bytes([acc_radius.value & 0xFF]))
+            self.con.transmit(registers.ACCEL_RADIUS_MSB_ADDR, 1, bytes([(acc_radius.value >> 8) & 0xFF]))
+
+            self.con.transmit(registers.MAG_OFFSET_X_LSB_ADDR, 1, bytes([mag_offset.value[0] & 0xFF]))
+            self.con.transmit(registers.MAG_OFFSET_X_MSB_ADDR, 1, bytes([(mag_offset.value[0] >> 8) & 0xFF]))
+            self.con.transmit(registers.MAG_OFFSET_Y_LSB_ADDR, 1, bytes([mag_offset.value[1] & 0xFF]))
+            self.con.transmit(registers.MAG_OFFSET_Y_MSB_ADDR, 1, bytes([(mag_offset.value[1] >> 8) & 0xFF]))
+            self.con.transmit(registers.MAG_OFFSET_Z_LSB_ADDR, 1, bytes([mag_offset.value[2] & 0xFF]))
+            self.con.transmit(registers.MAG_OFFSET_Z_MSB_ADDR, 1, bytes([(mag_offset.value[2] >> 8) & 0xFF]))
+
+            self.con.transmit(registers.MAG_RADIUS_LSB_ADDR, 1, bytes([mag_radius.value & 0xFF]))
+            self.con.transmit(registers.MAG_RADIUS_MSB_ADDR, 1, bytes([(mag_radius.value >> 8) & 0xFF]))
+
+            self.con.transmit(registers.GYRO_OFFSET_X_LSB_ADDR, 1, bytes([gyr_offset.value[0] & 0xFF]))
+            self.con.transmit(registers.GYRO_OFFSET_X_MSB_ADDR, 1, bytes([(gyr_offset.value[0] >> 8) & 0xFF]))
+            self.con.transmit(registers.GYRO_OFFSET_Y_LSB_ADDR, 1, bytes([gyr_offset.value[1] & 0xFF]))
+            self.con.transmit(registers.GYRO_OFFSET_Y_MSB_ADDR, 1, bytes([(gyr_offset.value[1] >> 8) & 0xFF]))
+            self.con.transmit(registers.GYRO_OFFSET_Z_LSB_ADDR, 1, bytes([gyr_offset.value[2] & 0xFF]))
+            self.con.transmit(registers.GYRO_OFFSET_Z_MSB_ADDR, 1, bytes([(gyr_offset.value[2] >> 8) & 0xFF]))
+
             return True
         except Exception:  # noqa: B902
             return False
 
+    def calibration_request_callback(self, request, response):
+        if not (self.con.transmit(registers.BNO055_OPR_MODE_ADDR, 1, bytes([registers.OPERATION_MODE_CONFIG]))):
+            self.node.get_logger().warn('Unable to set IMU into config mode.')
+        sleep(0.025)
+        calib_data = self.get_calib_data()
+        if not (self.con.transmit(registers.BNO055_OPR_MODE_ADDR, 1, bytes([registers.OPERATION_MODE_NDOF]))):
+            self.node.get_logger().warn('Unable to set IMU operation mode into operation mode.')
+        response.success = True
+        response.message = str(calib_data)
+        return response
+
     def unpackBytesToFloat(self, start, end):
         return float(struct.unpack('h', struct.pack('BB', start, end))[0])
diff --git a/launch/bno055.launch.py b/launch/bno055.launch.py
@@ -0,0 +1,19 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+def generate_launch_description():
+    ld = LaunchDescription()
+    config = os.path.join(
+        get_package_share_directory('bno055'),
+        'config',
+        'bno055_params.yaml'
+        )
+
+    node=Node(
+        package = 'bno055',
+        executable = 'bno055',
+        parameters = [config]
+    )
+    ld.add_action(node)
+    return ld
diff --git a/package.xml b/package.xml
@@ -15,7 +15,7 @@
   <exec_depend>rclpy</exec_depend>
   <!-- Publishing standard message types: -->
   <exec_depend>std_msgs</exec_depend>
-
+  <exec_depend>example_interfaces</exec_depend>
   <!-- These test dependencies are optional
   Their purpose is to make sure that the code passes the linters -->
   <test_depend>ament_copyright</test_depend>

diff --git a/setup.py b/setup.py
@@ -1,3 +1,5 @@
+import os
+from glob import glob
 from setuptools import find_packages
 from setuptools import setup
 
@@ -16,6 +18,8 @@
         ('share/ament_index/resource_index/packages',
          ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'), glob('bno055/params/*.yaml'))
     ],
     install_requires=['setuptools'],
     zip_safe=True,