diff --git a/.gitignore b/.gitignore index e69de29..d6b8cd0 100644 --- a/.gitignore +++ b/.gitignore @@ -0,0 +1,7 @@ +__pycache__/ +*.py[cod] +*.egg-info/ +.pytest_cache/ +build/ +install/ +log/ diff --git a/CLAUDE.md b/CLAUDE.md index 016f729..101a2ac 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -32,8 +32,9 @@ colcon test --packages-select idmind_imu && colcon test-result --verbose ## idmind_imu Driver for the TinkerForge IMU Brick 2.0 (BNO-055), reached over BrickDaemon TCP at -`localhost:4223`. Requires `brickd` running plus the `tinkerforge` Python bindings; neither is -declared in `package.xml`, so `rosdep` will not install them. +`localhost:4223`. Requires `brickd` running plus the `tinkerforge` Python bindings. `brickd` +is external; `tinkerforge` is not in rosdep, so it is declared in `requirements.txt` +(`pip install -r requirements.txt`) rather than `package.xml`. Two nodes ship as console scripts. **`imu_brick_node_v2` is the live one** — it is what the launch file starts. `imu_brick_node` is the legacy version, unused by any launch file, kept @@ -45,9 +46,8 @@ The launch file names the node `idmind_imu` and namespaces it under the `robot_n argument (default empty). Topic names are built from `self.get_name() + "/"`, so the real topics are `/idmind_imu/*`, or `//idmind_imu/*` when `robot_name` is set. -**The README documents them as `imu_brick_node/*` and is wrong** — along with describing -`euler` as degrees when it publishes radians, and omitting the `gravity` and `calibration` -publishers and the `auto_reconnect` parameter. Trust the source, not `README.MD`. +`README.MD` was rewritten to match the source (v2, namespaced, full topic/service/parameter +tables). If in doubt, still trust the source. - Publishers: `imu` (`sensor_msgs/Imu`), `temperature`, `magnetic_field`, `euler` (`std_msgs/Float32`, yaw in **radians**), `gravity` (`geometry_msgs/Vector3Stamped`), @@ -107,5 +107,6 @@ Full write-up with line references: `~/.claude/plans/quiet-launching-nygaard.md` `.get_parameter_value()._value`. - Tests are ament lint boilerplate only (`test_flake8`, `test_pep257`, `test_copyright`) — there is no functional test suite to lean on. Verify changes against real hardware or by - running the node and inspecting topics. + running the node and inspecting topics. `test_flake8` / `test_pep257` exclude the frozen + `imu_brick_node.py`; keep `imu_brick_node_v2.py` and everything else lint-clean. - Do **not** run `graphify` on this workspace. diff --git a/README.MD b/README.MD index 75ea58c..5c55d9f 100644 --- a/README.MD +++ b/README.MD @@ -1,61 +1,115 @@ # idmind_imu ## Description -ROS2 package providing drivers for IMU devices used on IDMind robots. Currently supports the **TinkerForge IMU Brick 2.0** (BNO-055 sensor), which provides orientation, angular velocity, linear acceleration, magnetic field, and temperature data. -Tested on **Ubuntu 22.04** with **ROS2 Humble**. +ROS 2 package providing the driver for IMU devices used on IDMind robots. It currently +supports the **TinkerForge IMU Brick 2.0** (Bosch BNO-055 sensor), exposing orientation, +angular velocity, linear acceleration, magnetic field, gravity, temperature and calibration +state. -### Prerequisites (TinkerForge IMU Brick) -1. Install [BrickDaemon](https://www.tinkerforge.com/en/doc/Software/Brickd.html#brickd) -2. Install [Python3 TinkerForge bindings](https://www.tinkerforge.com/en/doc/Software/API_Bindings_Python.html#api-bindings-python-install) -3. Identify the device UID (use [BrickViewer](https://www.tinkerforge.com/en/doc/Software/Brickv.html#brickv) or `sudo lsusb -v -d 16d0:063d | grep iSerial`) +Tested on **Ubuntu 22.04 / ROS 2 Humble**. Build type: `ament_python`. ---- +### Prerequisites + +1. Install [BrickDaemon](https://www.tinkerforge.com/en/doc/Software/Brickd.html#brickd) and + make sure it is running (default TCP endpoint `localhost:4223`). +2. Install the Python dependencies that rosdep does not cover: + ```bash + pip install -r requirements.txt # tinkerforge bindings + ``` + The remaining dependencies are declared in `package.xml` and install with + `rosdep install --from-paths src -y`. +3. Identify the device UID if needed (via + [BrickViewer](https://www.tinkerforge.com/en/doc/Software/Brickv.html#brickv) or + `sudo lsusb -v -d 16d0:063d | grep iSerial`). The node auto-discovers a single IMU Brick, + so this is only for multi-device setups. -## Launch Files +--- -- **idmind_imu_brick.launch.py** - Starts the TinkerForge IMU Brick 2.0 driver node. +## Usage ```bash ros2 launch idmind_imu idmind_imu_brick.launch.py ``` +### Launch arguments + +| Name | Default | Description | +|---|---|---| +| `robot_name` | `""` | Namespace for the node; topics become `//idmind_imu/*` | +| `simulation` | `False` | Reserved for a uniform bringup API; not consumed by the node yet | +| `imu_cfg` | installed `config/idmind_imu.yaml` | Parameter file passed to the node | + +Parameter defaults live in `config/idmind_imu.yaml`. Treat that file as read-only defaults — +override it with your own copy rather than editing it in place. + --- ## Nodes -### imu_brick_node -Driver node for the TinkerForge IMU Brick 2.0 (BNO-055). +Two console scripts are installed. **`imu_brick_node_v2` is the maintained node** and the one +the launch file starts. `imu_brick_node` is a frozen legacy version, used by no launch file +and kept only for reference. + +### imu_brick_node_v2 + +Node name: `idmind_imu`. All topic and service names below are relative to the node, i.e. +`/idmind_imu/` (or `//idmind_imu/` when `robot_name` is set). Responsibilities: -- Connects to the BrickDaemon and opens the IMU Brick device -- Reads orientation (quaternion), angular velocity, linear acceleration, magnetic field, and temperature -- Publishes sensor data as standard ROS messages -- Handles reconnection on timeout -#### Parameters -| Name | Description | Default | -|------|-------------|---------| -| `control_freq` | Main loop frequency (Hz) | `20.0` | -| `imu_freq` | IMU data stream frequency (Hz) | `20.0` | -| `imu_frame` | TF frame name for IMU data | `"imu"` | -| `imu_leds` | Enable the status LEDs on the IMU Brick | `False` | -| `imu_fusion_mode` | BNO-055 fusion mode (see TinkerForge docs) | `2` | -| `timeout` | Timeout before reporting a connection error (s) | `1.0` | - -#### Publishers +- Connect to BrickDaemon (retrying in the background) and discover the IMU Brick 2.0. +- Stream sensor data as standard ROS messages at `imu_freq`. +- Apply parameter changes to the device and watchdog the data stream, reporting status on + `/diagnostics`. +- Suspend/resume publishing on request (`~/standby`). + +#### Published topics + | Topic | Type | Description | -|-------|------|-------------| -| `imu_brick_node/imu` | `sensor_msgs/msg/Imu` | Orientation, angular velocity, and linear acceleration | -| `imu_brick_node/temperature` | `sensor_msgs/msg/Temperature` | IMU chip temperature | -| `imu_brick_node/magnetic_field` | `sensor_msgs/msg/MagneticField` | Magnetometer reading | -| `imu_brick_node/euler` | `std_msgs/msg/Float32` | Yaw angle in degrees (Euler Z) | -| `imu_brick_node/timer` | `std_msgs/msg/Float32` | Loop timing diagnostic | -| `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Node diagnostic status | +|---|---|---| +| `~/imu` | `sensor_msgs/msg/Imu` | Orientation (quaternion), angular velocity (rad/s), linear acceleration (m/s²) | +| `~/temperature` | `sensor_msgs/msg/Temperature` | IMU chip temperature (°C) | +| `~/magnetic_field` | `sensor_msgs/msg/MagneticField` | Magnetometer reading (T) | +| `~/euler` | `std_msgs/msg/Float32` | Yaw angle in **radians** (Euler Z) | +| `~/gravity` | `geometry_msgs/msg/Vector3Stamped` | Gravity vector (m/s²) | +| `~/calibration` | `std_msgs/msg/UInt8MultiArray` | `[sys, gyro, acc, mag]`, each `0`–`3` | +| `~/timer` | `std_msgs/msg/Float32` | Main-loop heartbeat | +| `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Connection / data / calibration status | + +> The `euler`, `timer` and `calibration` topics use `std_msgs` primitives for historical +> reasons. New consumers should read orientation from `~/imu` and calibration from +> `/diagnostics`. #### Services + | Name | Type | Description | -|------|------|-------------| -| `imu_brick_node/ready` | `std_srvs/srv/Trigger` | Returns whether the node is ready | -| `imu_brick_node/standby` | `std_srvs/srv/SetBool` | `data: true` suspends publishing on the data topics; `data: false` resumes. Samples keep arriving from the device while suspended, so the connection watchdog stays quiet. | +|---|---|---| +| `~/ready` | `std_srvs/srv/Trigger` | Reports whether the node finished initialising | +| `~/standby` | `std_srvs/srv/SetBool` | `data: true` suspends publishing on the data topics; `data: false` resumes. Samples keep arriving from the device while suspended, so the connection watchdog stays quiet. | + +#### Parameters + +| Name | Type | Default | Description | +|---|---|---|---| +| `control_freq` | `double` | `20.0` | Main-loop / watchdog frequency (Hz) | +| `imu_freq` | `double` | `20.0` | IMU data-stream frequency (Hz) | +| `imu_frame` | `string` | `"imu"` | `frame_id` on published messages (`config/idmind_imu.yaml` sets `"imu2"`) | +| `imu_leds` | `bool` | `false` | Enable the IMU Brick status LEDs | +| `imu_fusion_mode` | `int` | `2` | BNO-055 sensor-fusion mode (see TinkerForge docs) | +| `timeout` | `double` | `1.0` | Seconds without data before an `ERROR` diagnostic is raised | +| `auto_reconnect` | `bool` | `false` | Enable the TinkerForge library's auto-reconnect | + +All parameters are dynamically reconfigurable (`ros2 param set /idmind_imu `). + +--- + +## Build & test + +```bash +colcon build --symlink-install --packages-select idmind_imu +colcon test --packages-select idmind_imu && colcon test-result --all --verbose +``` + +Tests are `ament_flake8` / `ament_pep257` / `ament_copyright` lint checks. The legacy +`imu_brick_node.py` is excluded from the style linters (see `test/`). diff --git a/idmind_imu/imu_brick_node_v2.py b/idmind_imu/imu_brick_node_v2.py index d806244..2292539 100644 --- a/idmind_imu/imu_brick_node_v2.py +++ b/idmind_imu/imu_brick_node_v2.py @@ -108,8 +108,10 @@ def __init__(self): self.imu = None self.ipcon = IPConnection() self.ipcon.set_auto_reconnect(self.auto_reconnect) - self.ipcon.register_callback(IPConnection.CALLBACK_CONNECTED, self.brick_daemon_connection) - self.ipcon.register_callback(IPConnection.CALLBACK_DISCONNECTED, self.brick_daemon_disconnection) + self.ipcon.register_callback( + IPConnection.CALLBACK_CONNECTED, self.brick_daemon_connection) + self.ipcon.register_callback( + IPConnection.CALLBACK_DISCONNECTED, self.brick_daemon_disconnection) self.last_imu_msg = None self.in_loop = False @@ -126,7 +128,7 @@ def __init__(self): ################### def report_ready(self, _req, resp): - """Simple service callback to confirm the node is alive.""" + """Report whether the node has finished initialising.""" self.log("Replying to 'ready' request", 2) resp.success = self.ready resp.message = self.get_name() + " is " + ("ready" if self.ready else "not ready") @@ -165,14 +167,14 @@ def update_parameters(self, params): return SetParametersResult(successful=True) def brick_daemon_connection(self, reason): - """Registered for IPConnection.CALLBACK_CONNECTED.""" + """Handle IPConnection.CALLBACK_CONNECTED by enumerating for the IMU Brick.""" self.log("Connected to BrickDaemon", 2) self.ipcon.register_callback(IPConnection.CALLBACK_ENUMERATE, self.enumerate_callback) self.ipcon.enumerate() return True def brick_daemon_disconnection(self, reason): - """Registered for IPConnection.CALLBACK_DISCONNECTED.""" + """Handle IPConnection.CALLBACK_DISCONNECTED by dropping the cached IMU handle.""" self.log("Disconnected from BrickDaemon", 2, alert="warn") with self._imu_lock: self.imu = None @@ -181,7 +183,7 @@ def brick_daemon_disconnection(self, reason): def enumerate_callback(self, uid, con_uid, pos, hd_version, firmware_version, device_identifier, enumeration_type): - """Registered for IPConnection.CALLBACK_ENUMERATE.""" + """Handle IPConnection.CALLBACK_ENUMERATE by binding to the IMU Brick.""" if enumeration_type == IPConnection.ENUMERATION_TYPE_DISCONNECTED: self.log("IMU Brick disconnected during enumeration", 2) return @@ -209,7 +211,11 @@ def _apply_config_once(self): def publish_imu(self, acc, mag, ang_vel, euler, quat, linear_acc, gravity, temp, calibration_status): - """Registered for BrickIMUV2.CALLBACK_ALL_DATA — runs in a TinkerForge thread.""" + """ + Convert one IMU sample into ROS messages and publish it. + + Registered for ``BrickIMUV2.CALLBACK_ALL_DATA`` and runs on a TinkerForge thread. + """ try: now = self.get_clock().now().to_msg() @@ -281,10 +287,10 @@ def publish_imu(self, acc, mag, ang_vel, euler, quat, linear_acc, gravity, temp, # --- Calibration status [sys, gyro, acc, mag], each 0–3 --- # calibration_status is a uint8 bitmask: bits[7:6]=sys,[5:4]=gyro,[3:2]=acc,[1:0]=mag - cal_sys = (calibration_status >> 6) & 0x03 + cal_sys = (calibration_status >> 6) & 0x03 cal_gyro = (calibration_status >> 4) & 0x03 - cal_acc = (calibration_status >> 2) & 0x03 - cal_mag = (calibration_status >> 0) & 0x03 + cal_acc = (calibration_status >> 2) & 0x03 + cal_mag = (calibration_status >> 0) & 0x03 calib_list = [cal_sys, cal_gyro, cal_acc, cal_mag] calib_msg = UInt8MultiArray() calib_msg.data = calib_list @@ -387,7 +393,8 @@ def shutdown(self): return True def connect_brick_daemon(self): - """Spawn a background thread for the initial (blocking) ipcon.connect() call. + """ + Spawn a background thread for the initial (blocking) ipcon.connect() call. Guards against spawning multiple threads if a connection attempt is already in progress. @@ -403,7 +410,7 @@ def connect_brick_daemon(self): return True def _connect_with_exception_handling(self): - """Wrapper for ipcon.connect() that handles exceptions gracefully.""" + """Run ipcon.connect(), swallowing the failure when brickd is not up yet.""" try: self.ipcon.connect(self.host, self.port) except Exception: @@ -412,7 +419,8 @@ def _connect_with_exception_handling(self): pass def update_config(self): - """Read current IMU config and apply any pending parameter changes. + """ + Read current IMU config and apply any pending parameter changes. Acquires a local reference to self.imu under the lock so hardware I/O is performed outside the lock. @@ -513,7 +521,7 @@ def main(args=None): try: executor.spin() except KeyboardInterrupt: - print('\033[91m' + "Shutting down IMUBrick Node" + '\033[0m') + imu_node.get_logger().info("Shutting down IMU Brick node") finally: executor.shutdown() imu_node.shutdown() diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..52817bc --- /dev/null +++ b/requirements.txt @@ -0,0 +1,7 @@ +# Python dependencies that are not available through rosdep. +# Install alongside the rosdep dependencies: +# pip install -r requirements.txt +# +# tinkerforge: official bindings for the IMU Brick 2.0. Not packaged for apt or rosdep; +# upstream ships it only on PyPI (https://www.tinkerforge.com/en/doc/Software/API_Bindings_Python.html). +tinkerforge>=2.1.34 diff --git a/setup.py b/setup.py index 46c5165..083d8b8 100644 --- a/setup.py +++ b/setup.py @@ -14,8 +14,10 @@ ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob('launch/*launch.py')), (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), - (os.path.join('share', package_name, 'models', 'imu_razor'), glob('models/imu_razor/*.*')), - (os.path.join('share', package_name, 'models', 'imu_brick_v2'), glob('models/imu_brick_v2/*.*')), + (os.path.join('share', package_name, 'models', 'imu_razor'), + glob('models/imu_razor/*.*')), + (os.path.join('share', package_name, 'models', 'imu_brick_v2'), + glob('models/imu_brick_v2/*.*')), ], install_requires=['setuptools', 'tinkerforge'], zip_safe=True, diff --git a/test/test_flake8.py b/test/test_flake8.py index 27ee107..23e1044 100644 --- a/test/test_flake8.py +++ b/test/test_flake8.py @@ -16,10 +16,15 @@ import pytest +# imu_brick_node.py is the frozen legacy node (see CLAUDE.md): kept for reference, started +# by no launch file, and deliberately not restyled. New code is held to the linters. +_EXCLUDE = ['idmind_imu/imu_brick_node.py'] + + @pytest.mark.flake8 @pytest.mark.linter def test_flake8(): - rc, errors = main_with_errors(argv=[]) + rc, errors = main_with_errors(argv=['--exclude', *_EXCLUDE]) assert rc == 0, \ 'Found %d code style errors / warnings:\n' % len(errors) + \ '\n'.join(errors) diff --git a/test/test_pep257.py b/test/test_pep257.py index b234a38..227b849 100644 --- a/test/test_pep257.py +++ b/test/test_pep257.py @@ -16,8 +16,13 @@ import pytest +# imu_brick_node.py is the frozen legacy node (see CLAUDE.md): kept for reference, started +# by no launch file, and deliberately not restyled. New code is held to the linters. +_EXCLUDE = ['idmind_imu/imu_brick_node.py'] + + @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) + rc = main(argv=['.', 'test', '--exclude', *_EXCLUDE]) assert rc == 0, 'Found code style errors / warnings'