From 3c080570cf7cee37493b197c9d79a5c38d93b614 Mon Sep 17 00:00:00 2001 From: Fred Labrosse Date: Fri, 27 Jun 2025 10:00:09 +0100 Subject: [PATCH 1/2] Added an option to publish raw accelerations in the "normal" topic. --- README.md | 1 + bno055/params/NodeParameters.py | 5 +++++ bno055/params/bno055_params.yaml | 1 + bno055/sensor/SensorService.py | 15 +++++++++------ 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 7d2148e..5e1e05b 100644 --- a/README.md +++ b/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 diff --git a/bno055/params/NodeParameters.py b/bno055/params/NodeParameters.py index 947fdaf..009f546 100644 --- a/bno055/params/NodeParameters.py +++ b/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\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) diff --git a/bno055/params/bno055_params.yaml b/bno055/params/bno055_params.yaml index 3add21f..d2e857c 100644 --- a/bno055/params/bno055_params.yaml +++ b/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 diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index 10f1cf3..1c1ca40 100644 --- a/bno055/sensor/SensorService.py +++ b/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): + 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 From e0de15202ca890e5dbdf5cdb048aca7744331cb4 Mon Sep 17 00:00:00 2001 From: Fred Labrosse Date: Mon, 30 Jun 2025 11:01:01 +0100 Subject: [PATCH 2/2] Fixed access to new parameter. --- README.md | 2 +- bno055/params/NodeParameters.py | 2 +- bno055/params/bno055_params.yaml | 2 +- bno055/sensor/SensorService.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 5e1e05b..2b2d612 100644 --- a/README.md +++ b/README.md @@ -58,7 +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. +- **publish_raw_accel**: set to true to publish raw accelerations in the '/bno055/imu' topic (see below); default=false. ### ROS Topic Prefix diff --git a/bno055/params/NodeParameters.py b/bno055/params/NodeParameters.py index 009f546..22eb9ae 100644 --- a/bno055/params/NodeParameters.py +++ b/bno055/params/NodeParameters.py @@ -151,7 +151,7 @@ def __init__(self, node: Node): % self.placement_axis_remap.value) self.publish_raw_accel = node.get_parameter('publish_raw_accel') - node.get_logger().info('\tpublish_raw_accel:\t\t"%s"' % self.publish_raw_accel.value) + 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) diff --git a/bno055/params/bno055_params.yaml b/bno055/params/bno055_params.yaml index d2e857c..2b125a9 100644 --- a/bno055/params/bno055_params.yaml +++ b/bno055/params/bno055_params.yaml @@ -39,7 +39,7 @@ bno055: frame_id: "bno055" operation_mode: 0x0C placement_axis_remap: "P2" - publish_raw_accel: False + publish_raw_accel: false acc_factor: 100.0 mag_factor: 16000000.0 gyr_factor: 900.0 diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index 1c1ca40..16de2be 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -207,7 +207,7 @@ def get_sensor_data(self): imu_msg.orientation_covariance = imu_raw_msg.orientation_covariance - if (self.param.publish_raw_accel): + if self.param.publish_raw_accel.value: imu_msg.linear_acceleration = imu_raw_msg.linear_acceleration else: imu_msg.linear_acceleration.x = \