diff --git a/README.md b/README.md index 7d2148e..2b2d612 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..22eb9ae 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"%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..2b125a9 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..16de2be 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.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