Repository navigation
Bosh BNO055 IMU implementation - #37
YohanBorda wants to merge 1 commit into
Conversation
|
@YohanBorda Why did you use this configuration for robot_localization? Did you do any more tests at the end? I am going to start from this configuration, which is intended for a good IMU and poor odometry, and test if the odometry calculated with the encoders is good enough to add it or not. Also check that everything works correctly. I'm currently tuning the navigation stack and I'm following these guides: If I am convinced by the results of the tests, I will tell you about them here ^^ |
|
@rsilverioo I did that configuration as a starting point, it is not fully tuned and it changes depending on your robot. For example in my project, I will be using another EKF stack with GPS as this is more suitable for my application, so in my case, these values will be different due to the GPS will cover some parameters that the IMU doesn't need to. Also, I have been working on the hardware (calibrating the IMU, setting PID, procuring reliable motor drivers, and motors ) before I start tunning the EKF. I consider it is better to start correcting from the low level. To be honest I haven't reached the point to calibrate EKF as we are trying to get the low level and hardware as stable as possible before we move on to the filters. So, getting your feedback on how you tuned the navigation stack would be well appreciated. :) |
|
Thanks a lot @YohanBorda for this PR and please excuse the long wait on reviewing this. I plan to include at least some files/code you have provided! However I need some time to test this. Please also note that you modified the deprecated diffbot/diffbot_base/scripts/base_controller/src/main.cpp Lines 53 to 70 in e04c7dd This will be known as the low level approach. In case you are looking for a high level solution, there is also another open PR #60 which connects the IMU directly to the SBC via i2c. I think you might find this useful as well. Anyway, thanks again for this PR, I will definitely try both, the low level solution you provided and the high level approach. |
## What and why The robot's computer should be able to fuse the wheel odometry with the turn rate of an IMU (the EKF follows in its own pull request). This is the first part: the Teensy reads a Bosch BNO055 ([datasheet](https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf)) on its I2C bus and publishes its angular velocity and linear acceleration as [`sensor_msgs/Imu`](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Imu.html) on `imu/data_raw` (`/diffbot/imu/data_raw`) at 50 Hz. - **The driver,** [`bno055.h`](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/bno055.h) and [`bno055.cpp`](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/bno055.cpp): starts the sensor step by step without waiting for the chip (the I2C transfers themselves are synchronous), in the datasheet's IMU mode (IMUPLUS in Bosch's and Adafruit's libraries), where the chip's fusion corrects the gyroscope's and the accelerometer's offsets. It reads rate and acceleration in one burst, checks the sample transfers, reads its settings back, including the register page and the internal clock, and returns no sample instead of old values when a read fails. The reset and fusion waits have deadlines. At or after either deadline, the next update fails the start-up without another poll. The chip runs on its internal clock. The breakout's external crystal follows in #170. [The README](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/README.md) explains the fusion mode and why the driver is its own: the [Adafruit library](https://github.com/adafruit/Adafruit_BNO055) used in #37 and #66 waits with `delay()` and, after a reset, polls without a time limit. - **The message** follows [REP 145](https://www.ros.org/reps/rep-0145.html) in [REP 103](https://www.ros.org/reps/rep-0103.html) units, with the orientation marked unavailable (`orientation_covariance[0] = -1`), because the EKF uses only the yaw rate. The stamp estimates the sample's time from the ROS time when the read begins. - **The publisher,** [`imu_publisher.h`](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/imu_publisher.h): publishes only fresh samples, times its rate with [`millis()`](https://docs.arduino.cc/language-reference/en/functions/time/millis/), and publishes only after [rosserial's time sync](https://github.com/ros-drivers/rosserial/blob/noetic-devel/rosserial_client/src/ros_lib/ros/node_handle.h#L354-L385), when the Teensy has asked the robot's computer for the ROS time. [`SyncedNodeHandleT`](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/synced_node_handle.h) tells when that answer has arrived. A missing or failed sensor is tried again every `retry_ms`, and each try's outcome is logged. Ten failed reads in a row start the sensor again, and any successful read resets that count, also one too slow to publish. - **Settings:** [`imu_config.h`](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/imu_config.h) defines `ImuPublisherConfig`, with a check of its values. Remo's values are `IMU_PUBLISHER_CONFIG` in `diffbot_base_config.h`, checked at compile time, and the publisher gets them through its constructor. Its settings no longer use preprocessor definitions. With `enabled` false, the robot drives without an IMU. Without the sensor, the firmware warns and looks for it again every 10 s. - **Unit tests on the PC** (52 cases), in PlatformIO's [native environment](https://docs.platformio.org/en/latest/platforms/native.html) with [Unity](https://github.com/ThrowTheSwitch/Unity): the driver with a fake sensor, the publisher, the time sync with rosserial's real parser, and [`WireBusT`](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/wire_bus.h), which implements [`I2cBus`](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/i2c_bus.h) with the Teensy's [Wire library](https://www.pjrc.com/teensy/td_libs_Wire.html). `.docker/firmware/run.sh pio test -e native` runs them in the firmware container, and the Firmware workflow's job `unit tests (native)` in CI. The native tests build as C++17, as the Teensy builds do, and the Teensy platform is pinned to 6.0.0. **Built on earlier work.** Russ Hall (@Russ76) read a BNO055 on the Teensy and published it over rosserial in #66, and Yohan Borda (@YohanBorda) put the IMU on the microcontroller in #37. This PR follows their approach in the current base_controller firmware, with a new driver. ## How it was tested - Both boards build at 34bf766 (`.docker/firmware/run.sh pio run -e teensy31`, `-e teensy40`), with `IMU_PUBLISHER_CONFIG.enabled` true and false. - `.docker/firmware/run.sh pio test -e native`: 52 of 52 pass. - Earlier review validation: review runs compiled the firmware's loop, controller and IMU code on the PC with stand-ins for the hardware and rosserial: 20 control steps and 50 IMU messages per second, also when ROS time jumps and when `millis()` returns to zero. - On Remo, with a Teensy 3.2 and this pull request at 0b18e36 (inside #155's 1a3b547): the IMU starts and publishes at 50 Hz with current ROS stamps. z points up, and a left turn gives a positive `angular_velocity.z`. Unplugged, the topic stops while the wheel control keeps its 20 Hz. Plugged back in, the IMU is found again within one 10 s retry. The results and logs are in [the bench comment on #160](#160 (comment)). [The README](https://github.com/ros-mobile-robots/diffbot/blob/feat/imu-teensy/diffbot_firmware/lib/imu/README.md) lists the bench checks. - On Remo again at 34bf766, the Teensy 3.2 with `teensy31`: a warm start (the Teensy flashed, the IMU kept powered) and a cold start both start the IMU, with `imu/data_raw` at 50 Hz, current stamps and the wheel states at 20 Hz. With the IMU's power unplugged, the firmware warns once per 10 s retry and finds the IMU again after it's plugged back in. The results are in [the recheck on #169](#169 (comment)). - Not on hardware yet: Remo's own Teensy 4.0 still needs the `teensy40` firmware checks in #169. DiffBot's hardware validation remains open in #160. ## Documentation ros-mobile-robots/ros-mobile-robots.github.io#68 documents the IMU series (this, the simulated IMU and the EKF) and merges after all three. Part of #21. Part of #157: the firmware unit tests run in the container locally and in CI. Updating Testing and CI remains open. Co-authored-by: Russ Hall <7713301+Russ76@users.noreply.github.com> Co-authored-by: Yohan Borda <73143745+YohanBorda@users.noreply.github.com>
## What and why The wheel odometry estimates the robot's turn from the wheels, so it's off when a wheel slips or the wheel separation isn't quite right. The IMU's gyroscope measures the turn rate directly. With `ekf:=true`, robot_localization's EKF fuses both, on the robot and in the simulation. **Before this change:** on `noetic-devel`, both robots publish the IMU on `/diffbot/imu/data_raw`, from the firmware (#152) and from the simulated IMU (#153). No node reads it. The only estimate of the robot's motion is the drive controller's wheel odometry, `/diffbot/mobile_base_controller/odom`. The controller also publishes the transform `odom → base_footprint`, and move_base and Move Base Flex read the controller's odometry ([`move_base.launch`](https://github.com/ros-mobile-robots/diffbot/blob/a19aae4c2475487a63b91f3a241c82a7c7b8d27b/diffbot_navigation/launch/move_base.launch)). **With this pull request:** 1. The launch argument `ekf`, `false` by default, switches the EKF on, on the robot (`bringup.launch`, `bringup_with_laser.launch`) and in the simulation (`diffbot_gazebo`'s `diffbot.launch`, `diffbot_navigation`'s `diffbot.launch`). 2. With `ekf:=true`, the IMU sample validator and robot_localization's EKF start in `/diffbot`. The EKF fuses the wheel odometry's velocities with the IMU's yaw rate. It publishes its estimate on the topic `/diffbot/odometry/filtered` and the transform `odom → base_footprint` on the topic `/tf`. 3. With `ekf:=true`, the drive controller doesn't publish `odom → base_footprint`, so that only one node publishes that transform. 4. With `ekf:=true`, move_base and Move Base Flex read `/diffbot/odometry/filtered`. 5. Without the argument, the robots run as on the base branch. **Who sends what to whom:** the firmware, or the simulation's adapter, publishes `imu/data_raw`. [`imu_validator.py`](https://github.com/ros-mobile-robots/diffbot/blob/13e20569fb9c98ba0f9bd8f3a4395d58a83a2721/diffbot_control/scripts/imu_validator.py) subscribes to `imu/data_raw` and forwards the valid samples on `imu/data_validated`. The validator's part ends there. The EKF, `ekf_localization`, subscribes to `imu/data_validated` and to `mobile_base_controller/odom`. To turn the IMU's yaw rate into the robot's frame, the EKF looks up the transform between `imu_link` and `base_footprint`, which TF gets from `robot_state_publisher`. **Which launch file starts what:** on the robot, `bringup.launch` and `bringup_with_laser.launch` include `minimal.launch`, which includes `diffbot_base`'s `diffbot.launch`, and each passes `ekf` to the file that it includes. `diffbot_base`'s `diffbot.launch` sets the controller's `enable_odom_tf` and includes [`ekf.launch`](https://github.com/ros-mobile-robots/diffbot/blob/13e20569fb9c98ba0f9bd8f3a4395d58a83a2721/diffbot_control/launch/ekf.launch) if `ekf` is true. The two bringup files start `robot_state_publisher`. In the simulation, `diffbot_gazebo`'s `diffbot.launch` passes `ekf` to `diffbot_control.launch`, which sets the parameter, includes `ekf.launch` and starts `robot_state_publisher`. `minimal.launch`, `diffbot_base`'s `diffbot.launch`, `diffbot_control.launch`, `ekf.launch`, the two planners' `move_base` launch files and the tests' `sim_drive.launch` say at their top which files include them. **The parts:** - **[`diffbot_control/config/ekf.yaml`](https://github.com/ros-mobile-robots/diffbot/blob/13e20569fb9c98ba0f9bd8f3a4395d58a83a2721/diffbot_control/config/ekf.yaml):** the EKF in 2D. It fuses the drive controller's velocities (forward speed, the sideways speed of 0 that a differential drive has, and the yaw rate) with the IMU's yaw rate. Its frequency is 50 Hz, the IMU's rate. The comments say what a `_config` list is: 15 booleans, one for each variable of the filter's state. Each row of the two lists ends with a label of its three variables, and each fused value is named with its message field. - **The switch:** `ekf.launch` starts the validator and the EKF. The launch files that load the drive controller's configuration include `ekf.launch` with `ekf:=true` and set the controller's `enable_odom_tf` in both positions of the switch. So a restart without the EKF gets the controller's transform back. - **The IMU sample validator**, `imu_validator.py`: - The class `ImuValidator` has the checks and gets the ROS time as an argument. A valid sample arrives at a nonzero ROS time, and its frame is `imu_link`. Its angular velocity and linear acceleration are finite. Its angular velocity covariance is finite, with positive variances. Its stamp is at most 0.1 s older than the ROS time and at most 0.02 s ahead of it, and later than the stamp of the last valid sample. When the clock goes back, the order check starts over. - The node `ImuValidatorNode` forwards the valid samples, unchanged, and drops the others. It checks, counts and publishes inside one lock, and a diagnostics report takes its numbers in one snapshot inside the same lock. - The diagnostics have the count of forwarded samples and the counts of dropped samples by reason. The status is an error when no sample was valid for more than 1 s, or since the clock went back, and a warning when samples were dropped since the last report. The function `health()` decides that, without ROS communication. - The reason for dropping a sample is a member of the enum `DropReason`, whose values are the texts on the topic `/diagnostics`. - What the validator doesn't do: it doesn't judge whether a value is plausible. That is a statistical test inside robot_localization, which a rejection threshold switches on. `ekf.yaml` sets none. - Why it is needed: the EKF's per-input timestamp check allows equal stamps and doesn't limit a sample's age. A delayed sample can pass if its stamp isn't earlier than the preceding stamp on that input. If it also passes the remaining input checks, the filter can use it to correct its current estimate without rewinding. - **Comments:** the top comments of the launch files end with the address of the docs figure that shows which launch file includes which. `bringup.launch`, `bringup_with_laser.launch` and `diffbot_gazebo`'s `diffbot.launch`, the files that a user starts, have a top comment with that line. `ekf.launch` lists the validator's five checks, and check 4 names its two time limits with the reason for each. The comments name `/diagnostics`, `/tf` and `/diffbot/odometry/filtered` as topics, and use sentences instead of semicolons. - **Install rules** for `diffbot_bringup`, `diffbot_navigation` and `diffbot_mbf`: their launch folders, and the configuration, maps, RViz and camera files that those launch files load. industrial_ci tests from an install space, and the switch test reads those launch files. This is part of #113. `diffbot_base` already has its rule on `noetic-devel`. - **Tests:** - `test_imu_validator.py`: 27 unit tests without a ROS master, for the checks, the time since the last valid sample, the status of the diagnostics and the counts of a report. The two values just beyond the time limits are derived from the limits. - `ekf_synthetic.test`: the validator and the EKF on a stepped simulated clock, with synthetic wheel odometry and IMU samples. One test runs seven phases on one filter and stops at the first phase that fails, because each phase needs the state of the ones before. The docstring of `step()` lists the five things that one step of the clock does, and `publish_inputs()`, `wait_for_forwarded()` and `advance_clock()` each do one of them. A step's start is a `StepStart`. The messages that the subscribers' threads receive are kept in an `ArrivalLog` behind a lock. - `sim_ekf_diffbot.test` and `sim_ekf_remo.test`: both robots turn in Gazebo with a wheel separation that is 25 % wrong. One test checks that the EKF follows the true turn, the other that only the EKF publishes `odom → base_footprint` on the topic `/tf`. The two tests don't depend on each other. A `HeadingObserver` keeps the latest heading from Gazebo, from the odometry and from the EKF behind a lock, and returns them as one `Headings`. - `test_odom_tf.py` in both `sim_drive_<model>.test` files: without the EKF, only the drive controller publishes `odom → base_footprint`. In the simulation, the drive controller runs inside Gazebo, so the publisher's node name is `/gazebo`. - `test_ekf_switch.py`: both positions of the switch and the default, read from the launch files without starting them. With `ekf:=true`, the two bringup files and `diffbot_gazebo`'s `diffbot.launch` also have to start `robot_state_publisher`. The planners' launch files start no EKF, so the test checks the odometry topic that their planner reads. - The three EKF test files name the same thing alike: a constant for a topic ends in `_TOPIC`, for a node in `_NODE`, for a parameter in `_PARAMETER`. **Limits:** - **Not tested on the robot yet.** The IMU is mounted on Remo and passed its bench checks (#169), but the EKF hasn't run on a robot. Driving with navigation on the filtered odometry hasn't been tried either. - **The launch file on the robot.** `minimal.launch` and `diffbot_base`'s `diffbot.launch` take the `ekf` argument, because `bringup.launch` passes it through them. They start no `robot_state_publisher`. With one of them alone and `ekf:=true`, TF has no transform between `imu_link` and `base_footprint`, and the EKF drops every IMU sample and follows the wheels. The argument's help text names `bringup.launch` and `bringup_with_laser.launch`. Starting `robot_state_publisher` in the lower files would collide with the node of the same name in the bringup files. - **The robot container** starts `bringup.launch` without `ekf` and has no setting for it. - **The wheel odometry's rate on the robot.** In the simulation, both inputs arrive at 50 Hz, and the tests feed them at that rate. On the robot, `diffbot_base`'s control loop runs the drive controller at 10 Hz, and the controller publishes at most one odometry message per run. So the wheel odometry arrives at most 10 times per second there. No test covers that case yet. - **A reset of the simulation.** After `/gazebo/reset_simulation`, the simulated IMU, the validator and the EKF go on from the new time. The drive controller's odometry comes back only when the clock has passed its value from before the reset (`diff_drive_controller` keeps the time of its last message). In a run that was reset after 9.7 s, the EKF's first output after the reset was stamped 0.08 s, and the wheel odometry's first message 9.75 s. - **The validator's time limits and the robot's variances are starting values.** The validator's 0.1 s and 0.02 s wait for latency measurements on the robot. The firmware's and the drive controller's fixed variances aren't checked while driving. - **No rejection threshold.** robot_localization's test of a measurement against the filter's prediction is off. #205 evaluates the thresholds on recorded drives. - **Frontier exploration.** `diffbot_slam`'s `diffbot_frontier_exploration.launch` starts a move_base of its own on the controller's odometry and has no `ekf` argument. This pull request doesn't touch that file. - Not in this pull request: fusing the acceleration or a magnetic heading. **Built on earlier work.** Joe User (@joeuser846) ran robot_localization's EKF with a BNO055 in #60 and Discussion #59, and turned off the controller's odom TF so that the EKF owns it. Yohan Borda (@YohanBorda) proposed an EKF configuration in #37. This pull request takes their setup into the current packages, as one launch switch, with tests. ## How it was tested In the dev container's image (ROS Noetic, Gazebo 11.15.1, robot_localization 2.7.7), with the simulated IMU from #153 and Remo's model at `2.5.0`. - **CI at the head `13e2056`:** [CI](https://github.com/ros-mobile-robots/diffbot/actions/runs/43243901156) and [Dev container](https://github.com/ros-mobile-robots/diffbot/actions/runs/43243901227) pass. Both industrial_ci jobs report 81 tests, 0 errors, 0 failures. The Firmware, Robot container and PR description workflows pass too. - **Locally at `6362e4c`,** one test file after the other. After that, `d314266` changes one docstring of `test_ekf_synthetic.py`, where `ekf_synthetic.test` passes too, and the head `13e2056` changes comments of `ekf.yaml`: - `test_imu_validator.py`: 27 tests pass. - `test_ekf_switch.py` and `test_geometry_consistency.py`: 22 tests pass. - `ekf_synthetic.test`: its test passes. The estimate is 0.4987 rad/s with valid samples, 0.4005 rad/s after 1 s of stale ones, and 0.4987 rad/s after the clock went back. The validator's diagnostics count 402 forwarded samples and 50, 50 and 100 dropped ones. The test passed in six more runs while its file changed. - `sim_drive_diffbot.test` and `sim_drive_remo.test`: 7 tests each pass. - `sim_ekf_diffbot.test` and `sim_ekf_remo.test`: 2 tests each pass. With a wheel separation that is 25 % wrong, in a 3 s left turn, DiffBot turned 1.848 rad, its odometry said 1.478 rad and the EKF 1.848 rad. Remo turned 1.873 rad, its odometry said 1.498 rad and the EKF 1.874 rad. A second run of both files passes too. - **Both orders of `test_ekf.py`'s two tests,** at `48c8643`, on both robots, three fresh launches each: all 12 runs pass. The EKF's turn differed from the true one by at most 0.004 rad (limit 0.05 rad). The odometry fell behind by at least 0.369 rad (limit 0.15 rad). The validated IMU arrived at 50.0 Hz. At `6362e4c`, both robots pass with the transform test before the turn test, in one launch each. - **The synthetic test with a broken validator,** at `c270a10`: with the check for stamps from the future switched off, the test fails in its fourth phase, `check_validator_drops_samples_from_the_future`, and doesn't run the later phases. - **The switch test's check for `robot_state_publisher`:** called for `minimal.launch` and for `diffbot_base`'s `diffbot.launch` with `ekf:=true`, `assert_ekf_on()` fails with "starts the EKF without robot_state_publisher". For `bringup.launch` it passes. - **The comments in the launch files:** for 16 combinations of a launch or test file and its arguments, roslaunch's loader reads the same files and gives the same nodes and the same `enable_odom_tf` at `6362e4c` as at `1950a2c`, the head before the comments changed. At the head `13e2056`, `rosparam.load_file` and roslaunch's loader read `ekf.yaml`, with the labels inside its two lists, to the same 17 parameters as at `3ef4a91`. - **Style:** flake8 with ament's settings and mypy 1.10.1 for Python 3.8 pass on the script and the test files. - **A reset with the EKF on,** at `c202fed`, DiffBot with `ekf:=true`: after `/gazebo/reset_simulation` at 9.7 s, the validated IMU's first stamp was 0.02 s and the EKF's first output 0.08 s. The wheel odometry came back at 9.75 s. - **Install space:** industrial_ci builds and tests from an install space, so the CI run above covers the install rules. ## Merge order #153, which this pull request builds on, is merged. This pull request's base is `noetic-devel`. ## Documentation ros-mobile-robots/ros-mobile-robots.github.io#68 documents the IMU series: the firmware on the Teensy (#152), the simulated IMU (#153) and this EKF (#154). #152 and #153 are merged, so the docs wait for #154. [Which launch file starts what](https://ros-mobile-robots.com/pr-preview/pr-68/packages/imu/robot_localization/#which-launch-file-starts-what) shows the launch files and their nodes as two figures. [What the filter fuses](https://ros-mobile-robots.com/pr-preview/pr-68/packages/imu/robot_localization/#what-the-filter-fuses) explains the two `_config` lists of `ekf.yaml`, [The five checks](https://ros-mobile-robots.com/pr-preview/pr-68/packages/imu/robot_localization/#the-five-checks) the validator's checks, and [What the validator doesn't check](https://ros-mobile-robots.com/pr-preview/pr-68/packages/imu/robot_localization/#what-the-validator-doesnt-check) the rejection threshold that `ekf.yaml` doesn't set. This is the series' last pull request, so it closes #21. Co-authored-by: Joe User <1274372+joeuser846@users.noreply.github.com> Co-authored-by: Yohan Borda <73143745+YohanBorda@users.noreply.github.com>
Hi fjp,
The IMU has been implemented to the "low-level", using an Arduino Mega, the same implementation should work with you hardware (teensy board).
I found some challenges with your new encoders implementation (now using the EncoderStamped message) as the message format is now different, which introduces the a problem as the hardwareinterface is not being able to subscribe to the encoders topic published by the MCU, maybe you can review it and fix this linking problem. If possible, can you please share the solution?
I manage to verify that the IMU was publishing before and after the EKF filter successfully, however there is more potential adjustments required. We will be doing some tests with our hardware in the next weeks, if I find something extra required or nice to haves, I will let you know.
Changes:
New .ino file added with the encoders + IMU code. This compiled successfuly using Visual Code Arduino Plug-in (by Microsoft). Missing calibration process, we are planning to perform some testing first and then do all calibration required. WIll add calibration documentation once we get the calibration process set.
DiffBotHardwareInterface modified to subscribe to messages comming and re-format the messages to sensor_msgs/Imu, which is the format accepted by the EKF filter.
-EKF Filter added to diffbot_control/conf, this needs testing to adjust it to your hardware.