Publishing gravity in acceleration fields of "normal" topic

README.md

@@ -58,6 +58,7 @@ ros2 run bno055 bno055 --ros-args --params-file ./src/bno055/bno055/params/bno05
 - **calib_status_frequency**: frequency (HZ) to read and publish calibration status data from sensor; default=0.1 Hz
 - **placement_axis_remap**: The sensor placement configuration (Axis remapping) defines the position and orientation of the sensor mount.
 See Bosch BNO055 datasheet section "Axis Remap" for valid positions: "P0", "P1" (default), "P2", "P3", "P4", "P5", "P6", "P7".   
+- **publish_raw_accel**: set to true to publish raw accelerations in the '/bno055/imu' topic (see below); default=false.
 
 ### ROS Topic Prefix
 

bno055/params/NodeParameters.py

@@ -75,6 +75,8 @@ def __init__(self, node: Node):
         node.declare_parameter('operation_mode', value=0x0C)
         # placement_axis_remap defines the position and orientation of the sensor mount
         node.declare_parameter('placement_axis_remap', value='P1')
+        # Publish raw accelerations?
+        node.declare_parameter('publish_raw_accel', value=False)
         # scaling factor for acceleration
         node.declare_parameter('acc_factor', value=100.0)
         # scaling factor for magnetometer
@@ -148,6 +150,9 @@ def __init__(self, node: Node):
             node.get_logger().info('\tplacement_axis_remap:\t"%s"'
                                    % self.placement_axis_remap.value)
 
+            self.publish_raw_accel = node.get_parameter('publish_raw_accel')
+            node.get_logger().info('\tpublish_raw_accel:\t"%s"' % self.publish_raw_accel.value)
+
             self.acc_factor = node.get_parameter('acc_factor')
             node.get_logger().info('\tacc_factor:\t\t"%s"' % self.acc_factor.value)
 

bno055/params/bno055_params.yaml

@@ -39,6 +39,7 @@ bno055:
     frame_id: "bno055"
     operation_mode: 0x0C
     placement_axis_remap: "P2"
+    publish_raw_accel: false
     acc_factor: 100.0
     mag_factor: 16000000.0
     gyr_factor: 900.0

bno055/sensor/SensorService.py

@@ -207,12 +207,15 @@ def get_sensor_data(self):
 
         imu_msg.orientation_covariance = imu_raw_msg.orientation_covariance
 
-        imu_msg.linear_acceleration.x = \
-            self.unpackBytesToFloat(buf[32], buf[33]) / self.param.acc_factor.value
-        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
+        if self.param.publish_raw_accel.value:
+            imu_msg.linear_acceleration = imu_raw_msg.linear_acceleration
+        else:
+            imu_msg.linear_acceleration.x = \
+                self.unpackBytesToFloat(buf[32], buf[33]) / self.param.acc_factor.value
+            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
         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