Skip to content

Commit 214cd09

Browse files
flynnevaflynneva
authored
Merge pull request #43 from flynneva/develop
bring over updates
2 parents 2f5df04 + cf7202c commit 214cd09

File tree

8 files changed

+151
-50
lines changed

8 files changed

+151
-50
lines changed

‎.github/workflows/greetings.yml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
name: Greetings
22

3-
on: [pull_request, issues]
3+
on: [pull_request_target, issues]
44

55
jobs:
66
greeting:

‎README.md‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,13 @@ See Bosch BNO055 datasheet section "Axis Remap" for valid positions: "P0", "P1"
5656

5757
- **ros_topic_prefix**: ROS topic prefix to be used. Will be prepended to the default topic names (see below). Default="bno055/"
5858

59+
### Calibration
60+
61+
The current calibration values can be requested via the **calibration_request** service (this puts the imu into **CONFIGMODE** for a short time):
62+
63+
```
64+
ros2 service call /bno055/calibration_request example_interfaces/srv/Trigger
65+
```
5966
---
6067
## ROS Topics
6168

@@ -121,6 +128,11 @@ Run with customized parameter file:
121128

122129
ros2 run bno055 bno055 --ros-args --params-file ./src/bno055/bno055/params/bno055_params.yaml
123130

131+
Run launch file:
132+
133+
ros2 launch bno055 bno055.launch.py
134+
135+
124136
### Performing flake8 Linting
125137

126138
To perform code linting with [flake8](https://gitlab.com/pycqa/flake8) just perform:

‎bno055/params/NodeParameters.py‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,10 @@ def __init__(self, node: Node):
8484
node.declare_parameter('offset_mag', value=registers.DEFAULT_OFFSET_MAG)
8585
# +/- 2000 units up to 32000 (dps range dependent) (1 unit = 1/16 dps)
8686
node.declare_parameter('offset_gyr', value=registers.DEFAULT_OFFSET_GYR)
87+
# +/-1000 units
88+
node.declare_parameter('radius_acc', value=registers.DEFAULT_RADIUS_ACC)
89+
# +/-960 units
90+
node.declare_parameter('radius_mag', value=registers.DEFAULT_RADIUS_MAG)
8791
# Sensor standard deviation squared (^2) defaults [x, y, z]
8892
node.declare_parameter('variance_acc', value=registers.DEFAULT_VARIANCE_ACC)
8993
node.declare_parameter('variance_angular_vel', value=registers.DEFAULT_VARIANCE_ANGULAR_VEL)
@@ -142,9 +146,15 @@ def __init__(self, node: Node):
142146
self.offset_acc = node.get_parameter('offset_acc')
143147
node.get_logger().info('\toffset_acc:\t\t"%s"' % self.offset_acc.value)
144148

149+
self.radius_acc = node.get_parameter('radius_acc')
150+
node.get_logger().info('\tradius_acc:\t\t"%s"' % self.radius_acc.value)
151+
145152
self.offset_mag = node.get_parameter('offset_mag')
146153
node.get_logger().info('\toffset_mag:\t\t"%s"' % self.offset_mag.value)
147154

155+
self.radius_mag = node.get_parameter('radius_mag')
156+
node.get_logger().info('\tradius_mag:\t\t"%s"' % self.radius_mag.value)
157+
148158
self.offset_gyr = node.get_parameter('offset_gyr')
149159
node.get_logger().info('\toffset_gyr:\t\t"%s"' % self.offset_gyr.value)
150160

‎bno055/registers.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -246,6 +246,8 @@
246246
#: +/- 2000 units up to 32000 (dps range dependent) (1 unit = 1/16 dps)
247247
DEFAULT_OFFSET_GYR = [0x0002, 0xFFFF, 0xFFFF]
248248

249+
DEFAULT_RADIUS_MAG = 0x0
250+
DEFAULT_RADIUS_ACC = 0x3E8
249251
#: Sensor standard deviation squared (^2) defaults [x, y, z]
250252
#: Used to get covariance matrices (stddev^2 = variance)
251253
#: values taken from this ROS1 driver from octanis:

‎bno055/sensor/SensorService.py‎

Lines changed: 102 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
from rclpy.qos import QoSProfile
4141
from sensor_msgs.msg import Imu, MagneticField, Temperature
4242
from std_msgs.msg import String
43+
from example_interfaces.srv import Trigger
4344

4445

4546
class SensorService:
@@ -59,6 +60,7 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters):
5960
self.pub_mag = node.create_publisher(MagneticField, prefix + 'mag', QoSProf)
6061
self.pub_temp = node.create_publisher(Temperature, prefix + 'temp', QoSProf)
6162
self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', QoSProf)
63+
self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback)
6264

6365
def configure(self):
6466
"""Configure the IMU sensor hardware."""
@@ -104,21 +106,26 @@ def configure(self):
104106
'P7': bytes(b'\x24\x05')
105107
}
106108
if not (self.con.transmit(registers.BNO055_AXIS_MAP_CONFIG_ADDR, 2,
107-
mount_positions[self.param.placement_axis_remap.value])):
109+
mount_positions[self.param.placement_axis_remap.value])):
108110
self.node.get_logger().warn('Unable to set sensor placement configuration.')
109111

110112
# Show the current sensor offsets
111-
print('Current sensor offsets:')
112-
self.get_calib_offsets()
113-
if (self.param.set_offsets):
113+
self.node.get_logger().info('Current sensor offsets:')
114+
self.print_calib_data()
115+
if self.param.set_offsets.value:
114116
configured_offsets = \
115117
self.set_calib_offsets(
116118
self.param.offset_acc,
117119
self.param.offset_mag,
118-
self.param.offset_gyr)
119-
if (configured_offsets):
120-
print('Successfully configured sensor offsets to:')
121-
self.get_calib_offsets()
120+
self.param.offset_gyr,
121+
self.param.radius_mag,
122+
self.param.radius_acc)
123+
if configured_offsets:
124+
self.node.get_logger().info('Successfully configured sensor offsets to:')
125+
self.print_calib_data()
126+
else:
127+
self.node.get_logger().warn('setting offsets failed')
128+
122129

123130
# Set Device to NDOF mode
124131
# data fusion for gyroscope, acceleration sensor and magnetometer enabled
@@ -139,15 +146,14 @@ def get_sensor_data(self):
139146
# read from sensor
140147
buf = self.con.receive(registers.BNO055_ACCEL_DATA_X_LSB_ADDR, 45)
141148
# Publish raw data
142-
# TODO: convert rcl Clock time to ros time?
143-
# imu_raw_msg.header.stamp = node.get_clock().now()
149+
imu_raw_msg.header.stamp = self.node.get_clock().now().to_msg()
144150
imu_raw_msg.header.frame_id = self.param.frame_id.value
145151
# TODO: do headers need sequence counters now?
146152
# imu_raw_msg.header.seq = seq
147153

148154
# TODO: make this an option to publish?
149155
imu_raw_msg.orientation_covariance = [
150-
self.param.variance_orientation.value[0], 0.0 , 0.0,
156+
self.param.variance_orientation.value[0], 0.0, 0.0,
151157
0.0, self.param.variance_orientation.value[1], 0.0,
152158
0.0, 0.0, self.param.variance_orientation.value[2]
153159
]
@@ -179,7 +185,7 @@ def get_sensor_data(self):
179185

180186
# TODO: make this an option to publish?
181187
# Publish filtered data
182-
# imu_msg.header.stamp = node.get_clock().now()
188+
imu_msg.header.stamp = self.node.get_clock().now().to_msg()
183189
imu_msg.header.frame_id = self.param.frame_id.value
184190

185191
q = Quaternion()
@@ -190,7 +196,7 @@ def get_sensor_data(self):
190196
q.z = self.unpackBytesToFloat(buf[30], buf[31])
191197
# TODO(flynneva): replace with standard normalize() function
192198
# normalize
193-
norm = sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w)
199+
norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w)
194200
imu_msg.orientation.x = q.x / norm
195201
imu_msg.orientation.y = q.y / norm
196202
imu_msg.orientation.z = q.z / norm
@@ -203,7 +209,7 @@ def get_sensor_data(self):
203209
imu_msg.linear_acceleration.y = \
204210
self.unpackBytesToFloat(buf[34], buf[35]) / self.param.acc_factor.value
205211
imu_msg.linear_acceleration.z = \
206-
self.unpackBytesToFloat( buf[36], buf[37]) / self.param.acc_factor.value
212+
self.unpackBytesToFloat(buf[36], buf[37]) / self.param.acc_factor.value
207213
imu_msg.linear_acceleration_covariance = imu_raw_msg.linear_acceleration_covariance
208214
imu_msg.angular_velocity.x = \
209215
self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value
@@ -215,7 +221,7 @@ def get_sensor_data(self):
215221
self.pub_imu.publish(imu_msg)
216222

217223
# Publish magnetometer data
218-
# mag_msg.header.stamp = node.get_clock().now()
224+
mag_msg.header.stamp = self.node.get_clock().now().to_msg()
219225
mag_msg.header.frame_id = self.param.frame_id.value
220226
# mag_msg.header.seq = seq
221227
mag_msg.magnetic_field.x = \
@@ -232,7 +238,7 @@ def get_sensor_data(self):
232238
self.pub_mag.publish(mag_msg)
233239

234240
# Publish temperature
235-
# temp_msg.header.stamp = node.get_clock().now()
241+
temp_msg.header.stamp = self.node.get_clock().now().to_msg()
236242
temp_msg.header.frame_id = self.param.frame_id.value
237243
# temp_msg.header.seq = seq
238244
temp_msg.temperature = float(buf[44])
@@ -258,8 +264,9 @@ def get_calib_status(self):
258264
# Publish via ROS topic:
259265
self.pub_calib_status.publish(calib_status_str)
260266

261-
def get_calib_offsets(self):
262-
"""Read all calibration offsets and print to screen."""
267+
def get_calib_data(self):
268+
"""Read all calibration data."""
269+
263270
accel_offset_read = self.con.receive(registers.ACCEL_OFFSET_X_LSB_ADDR, 6)
264271
accel_offset_read_x = (accel_offset_read[1] << 8) | accel_offset_read[
265272
0] # Combine MSB and LSB registers into one decimal
@@ -268,6 +275,9 @@ def get_calib_offsets(self):
268275
accel_offset_read_z = (accel_offset_read[5] << 8) | accel_offset_read[
269276
4] # Combine MSB and LSB registers into one decimal
270277

278+
accel_radius_read = self.con.receive(registers.ACCEL_RADIUS_LSB_ADDR, 2)
279+
accel_radius_read_value = (accel_radius_read[1] << 8) | accel_radius_read[0]
280+
271281
mag_offset_read = self.con.receive(registers.MAG_OFFSET_X_LSB_ADDR, 6)
272282
mag_offset_read_x = (mag_offset_read[1] << 8) | mag_offset_read[
273283
0] # Combine MSB and LSB registers into one decimal
@@ -276,6 +286,9 @@ def get_calib_offsets(self):
276286
mag_offset_read_z = (mag_offset_read[5] << 8) | mag_offset_read[
277287
4] # Combine MSB and LSB registers into one decimal
278288

289+
mag_radius_read = self.con.receive(registers.MAG_RADIUS_LSB_ADDR, 2)
290+
mag_radius_read_value = (mag_radius_read[1] << 8) | mag_radius_read[0]
291+
279292
gyro_offset_read = self.con.receive(registers.GYRO_OFFSET_X_LSB_ADDR, 6)
280293
gyro_offset_read_x = (gyro_offset_read[1] << 8) | gyro_offset_read[
281294
0] # Combine MSB and LSB registers into one decimal
@@ -284,31 +297,54 @@ def get_calib_offsets(self):
284297
gyro_offset_read_z = (gyro_offset_read[5] << 8) | gyro_offset_read[
285298
4] # Combine MSB and LSB registers into one decimal
286299

300+
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,
301+
'mag_offset': {'x': mag_offset_read_x, 'y': mag_offset_read_y, 'z': mag_offset_read_z}, 'mag_radius': mag_radius_read_value,
302+
'gyro_offset': {'x': gyro_offset_read_x, 'y': gyro_offset_read_y, 'z': gyro_offset_read_z}}
303+
304+
return calib_data
305+
306+
def print_calib_data(self):
307+
"""Read all calibration data and print to screen."""
308+
calib_data = self.get_calib_data()
287309
self.node.get_logger().info(
288310
'\tAccel offsets (x y z): %d %d %d' % (
289-
accel_offset_read_x,
290-
accel_offset_read_y,
291-
accel_offset_read_z))
311+
calib_data['accel_offset']['x'],
312+
calib_data['accel_offset']['y'],
313+
calib_data['accel_offset']['z']))
314+
315+
self.node.get_logger().info(
316+
'\tAccel radius: %d' % (
317+
calib_data['accel_radius'],
318+
)
319+
)
292320

293321
self.node.get_logger().info(
294322
'\tMag offsets (x y z): %d %d %d' % (
295-
mag_offset_read_x,
296-
mag_offset_read_y,
297-
mag_offset_read_z))
323+
calib_data['mag_offset']['x'],
324+
calib_data['mag_offset']['y'],
325+
calib_data['mag_offset']['z']))
326+
327+
self.node.get_logger().info(
328+
'\tMag radius: %d' % (
329+
calib_data['mag_radius'],
330+
)
331+
)
298332

299333
self.node.get_logger().info(
300334
'\tGyro offsets (x y z): %d %d %d' % (
301-
gyro_offset_read_x,
302-
gyro_offset_read_y,
303-
gyro_offset_read_z))
335+
calib_data['gyro_offset']['x'],
336+
calib_data['gyro_offset']['y'],
337+
calib_data['gyro_offset']['z']))
304338

305-
def set_calib_offsets(self, acc_offset, mag_offset, gyr_offset):
339+
def set_calib_offsets(self, acc_offset, mag_offset, gyr_offset, mag_radius, acc_radius):
306340
"""
307341
Write calibration data (define as 16 bit signed hex).
308342
309343
:param acc_offset:
310344
:param mag_offset:
311345
:param gyr_offset:
346+
:param mag_radius:
347+
:param acc_radius:
312348
"""
313349
# Must switch to config mode to write out
314350
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):
317353

318354
# Seems to only work when writing 1 register at a time
319355
try:
320-
self.con.transmit(registers.ACCEL_OFFSET_X_LSB_ADDR, 1, bytes([acc_offset[0] & 0xFF]))
321-
self.con.transmit(registers.ACCEL_OFFSET_X_MSB_ADDR, 1, bytes([(acc_offset[0] >> 8) & 0xFF]))
322-
self.con.transmit(registers.ACCEL_OFFSET_Y_LSB_ADDR, 1, bytes([acc_offset[1] & 0xFF]))
323-
self.con.transmit(registers.ACCEL_OFFSET_Y_MSB_ADDR, 1, bytes([(acc_offset[1] >> 8) & 0xFF]))
324-
self.con.transmit(registers.ACCEL_OFFSET_Z_LSB_ADDR, 1, bytes([acc_offset[2] & 0xFF]))
325-
self.con.transmit(registers.ACCEL_OFFSET_Z_MSB_ADDR, 1, bytes([(acc_offset[2] >> 8) & 0xFF]))
326-
327-
self.con.transmit(registers.MAG_OFFSET_X_LSB_ADDR, 1, bytes([mag_offset[0] & 0xFF]))
328-
self.con.transmit(registers.MAG_OFFSET_X_MSB_ADDR, 1, bytes([(mag_offset[0] >> 8) & 0xFF]))
329-
self.con.transmit(registers.MAG_OFFSET_Y_LSB_ADDR, 1, bytes([mag_offset[1] & 0xFF]))
330-
self.con.transmit(registers.MAG_OFFSET_Y_MSB_ADDR, 1, bytes([(mag_offset[1] >> 8) & 0xFF]))
331-
self.con.transmit(registers.MAG_OFFSET_Z_LSB_ADDR, 1, bytes([mag_offset[2] & 0xFF]))
332-
self.con.transmit(registers.MAG_OFFSET_Z_MSB_ADDR, 1, bytes([(mag_offset[2] >> 8) & 0xFF]))
333-
334-
self.con.transmit(registers.GYRO_OFFSET_X_LSB_ADDR, 1, bytes([gyr_offset[0] & 0xFF]))
335-
self.con.transmit(registers.GYRO_OFFSET_X_MSB_ADDR, 1, bytes([(gyr_offset[0] >> 8) & 0xFF]))
336-
self.con.transmit(registers.GYRO_OFFSET_Y_LSB_ADDR, 1, bytes([gyr_offset[1] & 0xFF]))
337-
self.con.transmit(registers.GYRO_OFFSET_Y_MSB_ADDR, 1, bytes([(gyr_offset[1] >> 8) & 0xFF]))
338-
self.con.transmit(registers.GYRO_OFFSET_Z_LSB_ADDR, 1, bytes([gyr_offset[2] & 0xFF]))
339-
self.con.transmit(registers.GYRO_OFFSET_Z_MSB_ADDR, 1, bytes([(gyr_offset[2] >> 8) & 0xFF]))
356+
self.con.transmit(registers.ACCEL_OFFSET_X_LSB_ADDR, 1, bytes([acc_offset.value[0] & 0xFF]))
357+
self.con.transmit(registers.ACCEL_OFFSET_X_MSB_ADDR, 1, bytes([(acc_offset.value[0] >> 8) & 0xFF]))
358+
self.con.transmit(registers.ACCEL_OFFSET_Y_LSB_ADDR, 1, bytes([acc_offset.value[1] & 0xFF]))
359+
self.con.transmit(registers.ACCEL_OFFSET_Y_MSB_ADDR, 1, bytes([(acc_offset.value[1] >> 8) & 0xFF]))
360+
self.con.transmit(registers.ACCEL_OFFSET_Z_LSB_ADDR, 1, bytes([acc_offset.value[2] & 0xFF]))
361+
self.con.transmit(registers.ACCEL_OFFSET_Z_MSB_ADDR, 1, bytes([(acc_offset.value[2] >> 8) & 0xFF]))
362+
363+
self.con.transmit(registers.ACCEL_RADIUS_LSB_ADDR, 1, bytes([acc_radius.value & 0xFF]))
364+
self.con.transmit(registers.ACCEL_RADIUS_MSB_ADDR, 1, bytes([(acc_radius.value >> 8) & 0xFF]))
365+
366+
self.con.transmit(registers.MAG_OFFSET_X_LSB_ADDR, 1, bytes([mag_offset.value[0] & 0xFF]))
367+
self.con.transmit(registers.MAG_OFFSET_X_MSB_ADDR, 1, bytes([(mag_offset.value[0] >> 8) & 0xFF]))
368+
self.con.transmit(registers.MAG_OFFSET_Y_LSB_ADDR, 1, bytes([mag_offset.value[1] & 0xFF]))
369+
self.con.transmit(registers.MAG_OFFSET_Y_MSB_ADDR, 1, bytes([(mag_offset.value[1] >> 8) & 0xFF]))
370+
self.con.transmit(registers.MAG_OFFSET_Z_LSB_ADDR, 1, bytes([mag_offset.value[2] & 0xFF]))
371+
self.con.transmit(registers.MAG_OFFSET_Z_MSB_ADDR, 1, bytes([(mag_offset.value[2] >> 8) & 0xFF]))
372+
373+
self.con.transmit(registers.MAG_RADIUS_LSB_ADDR, 1, bytes([mag_radius.value & 0xFF]))
374+
self.con.transmit(registers.MAG_RADIUS_MSB_ADDR, 1, bytes([(mag_radius.value >> 8) & 0xFF]))
375+
376+
self.con.transmit(registers.GYRO_OFFSET_X_LSB_ADDR, 1, bytes([gyr_offset.value[0] & 0xFF]))
377+
self.con.transmit(registers.GYRO_OFFSET_X_MSB_ADDR, 1, bytes([(gyr_offset.value[0] >> 8) & 0xFF]))
378+
self.con.transmit(registers.GYRO_OFFSET_Y_LSB_ADDR, 1, bytes([gyr_offset.value[1] & 0xFF]))
379+
self.con.transmit(registers.GYRO_OFFSET_Y_MSB_ADDR, 1, bytes([(gyr_offset.value[1] >> 8) & 0xFF]))
380+
self.con.transmit(registers.GYRO_OFFSET_Z_LSB_ADDR, 1, bytes([gyr_offset.value[2] & 0xFF]))
381+
self.con.transmit(registers.GYRO_OFFSET_Z_MSB_ADDR, 1, bytes([(gyr_offset.value[2] >> 8) & 0xFF]))
382+
340383
return True
341384
except Exception: # noqa: B902
342385
return False
343386

387+
def calibration_request_callback(self, request, response):
388+
if not (self.con.transmit(registers.BNO055_OPR_MODE_ADDR, 1, bytes([registers.OPERATION_MODE_CONFIG]))):
389+
self.node.get_logger().warn('Unable to set IMU into config mode.')
390+
sleep(0.025)
391+
calib_data = self.get_calib_data()
392+
if not (self.con.transmit(registers.BNO055_OPR_MODE_ADDR, 1, bytes([registers.OPERATION_MODE_NDOF]))):
393+
self.node.get_logger().warn('Unable to set IMU operation mode into operation mode.')
394+
response.success = True
395+
response.message = str(calib_data)
396+
return response
397+
344398
def unpackBytesToFloat(self, start, end):
345399
return float(struct.unpack('h', struct.pack('BB', start, end))[0])

‎launch/bno055.launch.py‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
import os
2+
from ament_index_python.packages import get_package_share_directory
3+
from launch import LaunchDescription
4+
from launch_ros.actions import Node
5+
def generate_launch_description():
6+
ld = LaunchDescription()
7+
config = os.path.join(
8+
get_package_share_directory('bno055'),
9+
'config',
10+
'bno055_params.yaml'
11+
)
12+
13+
node=Node(
14+
package = 'bno055',
15+
executable = 'bno055',
16+
parameters = [config]
17+
)
18+
ld.add_action(node)
19+
return ld

‎package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
<exec_depend>rclpy</exec_depend>
1616
<!-- Publishing standard message types: -->
1717
<exec_depend>std_msgs</exec_depend>
18-
18+
<exec_depend>example_interfaces</exec_depend>
1919
<!-- These test dependencies are optional
2020
Their purpose is to make sure that the code passes the linters -->
2121
<test_depend>ament_copyright</test_depend>

‎setup.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,5 @@
1+
import os
2+
from glob import glob
13
from setuptools import find_packages
24
from setuptools import setup
35

@@ -16,6 +18,8 @@
1618
('share/ament_index/resource_index/packages',
1719
['resource/' + package_name]),
1820
('share/' + package_name, ['package.xml']),
21+
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
22+
(os.path.join('share', package_name, 'config'), glob('bno055/params/*.yaml'))
1923
],
2024
install_requires=['setuptools'],
2125
zip_safe=True,

0 commit comments

Comments
 (0)