Skip to content

Sbc imu - #60

Closed
joeuser846 wants to merge 2 commits into
ros-mobile-robots:noetic-develfrom
joeuser846:sbc-imu
Closed

joeuser846 wants to merge 2 commits into
ros-mobile-robots:noetic-develfrom
joeuser846:sbc-imu

Conversation

@joeuser846

Copy link
Copy Markdown
Contributor

These changes implement the Adafruit BNO055 IMU directly connected to the sbc (Rpi in my case) using I2C. The IMU data and motor encoder based odom is fused in robot_localization to create a much more robust odom.

Some of the changes in this PR are tangential and were just me trying various settings to clear problems with transform timings and "extrapolation into the future" errors. Those can be ignore or tweaked as appropriate. The key changes are in diffbot_control/config, ros-imu-bno055/launch, robot_localization/params, diffbot_description/urdf, and the diffbot_bringup/launch files that now also start the IMU and robot_localization, specifically: bringup_with_laser_imu.launch and bringup_all.launch.

Hope you find this useful. Thanks again for diffbot.

@fjp

fjp commented Mar 15, 2022

Copy link
Copy Markdown
Member

Thanks a lot @joeuser846, this looks very promising! I am planning to test it soon and provide detailed feedback to get this merged.

fjp added a commit that referenced this pull request Nov 10, 2026
## What and why

The EKF that fuses the wheel odometry with the IMU (#154) should run in the simulation as on the robot. So both robots get an IMU in Gazebo that publishes what the robot's firmware publishes (#152).

**Before this change:** on `noetic-devel`, neither robot has an IMU in Gazebo. DiffBot's description has an [`imu_sensor` macro](https://github.com/ros-mobile-robots/diffbot/blob/f5f282eed7b6a2b146d40c7d5fd641b8a339a77d/diffbot_description/urdf/include/common_sensors.xacro#L191-L264) that no model calls. The macro configures its plugin in ROS 2 syntax, which Gazebo Classic ignores. Remo's model is pinned at remo_description `2.0.0`, which has no IMU. Nothing publishes `/diffbot/imu/data_raw` in the simulation.

**With this pull request:**

1. Both robots' models have the link `imu_link` with Gazebo's IMU sensor on it.
2. The simulation publishes `/diffbot/imu/data_raw` at 50 Hz with the firmware's message: the angular velocity and the acceleration in `imu_link`, with their covariances, and the orientation marked unavailable (`orientation_covariance[0] = -1`).
3. The sensor's Gazebo plugin is the repository's own copy of gazebo_plugins' IMU plugin, in the new package `diffbot_gazebo_plugins`. The copy doesn't stop Gazebo's clock while the controllers load (#185), and it publishes again right after a reset of the simulation.
4. Both robots' simulation test files check the simulated IMU, also after a reset of the simulation.

**Who publishes what:** Gazebo's IMU sensor measures at `imu_link`. The plugin, [`gazebo_ros_imu_sensor.cpp`](https://github.com/ros-mobile-robots/diffbot/blob/5bad6d5b83280105e536173d0b93432ae310eda6/diffbot_gazebo_plugins/src/gazebo_ros_imu_sensor.cpp), adds noise and publishes `/diffbot/imu/gazebo`. The plugin's part ends there. The adapter, [`imu_raw_adapter.py`](https://github.com/ros-mobile-robots/diffbot/blob/5bad6d5b83280105e536173d0b93432ae310eda6/diffbot_gazebo/scripts/imu_raw_adapter.py), removes the orientation and publishes `/diffbot/imu/data_raw`. The EKF's side (#154) reads that topic.

**The parts:**

- **`diffbot_description`:** DiffBot's model calls the `imu_sensor` macro, as Remo's model does since ros-mobile-robots/remo_description#10: `imu_link` on `chassis_link`, with the IMU plugin from `diffbot_gazebo_plugins` at 50 Hz. DiffBot uses the same BNO055 board as Remo, moved between the robots. Its mounting on DiffBot isn't measured yet, so its position is a placeholder (xacro arguments `imu_xyz` and `imu_rpy`). The board weighs 3 g with its header pins. DiffBot's base mass was weighed without the board, so the IMU link adds the 3 g.
- **`diffbot_gazebo_plugins`, a new package:** the IMU's Gazebo plugin, `libdiffbot_gazebo_ros_imu_sensor.so`. It's a copy of gazebo_plugins' `libgazebo_ros_imu_sensor.so` (version 2.9.3) with the same parameters and message. The [change notice](https://github.com/ros-mobile-robots/diffbot/blob/5bad6d5b83280105e536173d0b93432ae310eda6/diffbot_gazebo_plugins/src/gazebo_ros_imu_sensor.cpp#L17-L42) at the top of the file lists every difference. Three of them change what the plugin does:
    - **No `ros::spinOnce()` after each message.** gazebo_plugins' plugin makes that call in Gazebo's physics thread. The call runs waiting ROS callbacks there, sometimes the controller manager's service that loads or starts a controller. That service waits for the physics thread, so Gazebo's clock stops for good (#185). The plugin only publishes, so it has no callbacks of its own. The spinner threads of `gazebo_ros` serve the others.
    - **A reset of the simulation.** `/gazebo/reset_simulation` sets Gazebo's clock back to zero. The plugin limits its rate by comparing the sensor's time with the time of its last update. After a reset that difference is negative, and gazebo_plugins' plugin publishes nothing until the clock has passed its value from before the reset. The copy starts the comparison from the new time ([lines 141–142](https://github.com/ros-mobile-robots/diffbot/blob/5bad6d5b83280105e536173d0b93432ae310eda6/diffbot_gazebo_plugins/src/gazebo_ros_imu_sensor.cpp#L141-L142)), as gazebo_plugins' `gazebo_ros_p3d` does. The IMU sample validator and the EKF of #154 start over when the clock goes back, so they need the IMU's samples right after a reset.
    - **A failed `Load()`.** `Load()` returns before it creates the node handle when the sensor isn't an IMU sensor, when `frameName` is missing or when ROS isn't initialized. gazebo_plugins' destructor then calls `shutdown()` through a pointer that was never set. The copy sets the pointer to `NULL` and checks it.

  The class is in its own namespace, because Gazebo loads plugin libraries with `RTLD_GLOBAL`.
- **License of the copy:** the original is under the Apache License 2.0, and so is the copy. It keeps the original's copyright notice and lists its changes at the top of both files. The package installs the license text (`diffbot_gazebo_plugins/LICENSE`), its `package.xml` names `Apache-2.0`, and the repository's license acknowledgements list gazebo_ros_pkgs.
- **`diffbot_gazebo`:** `diffbot_view.launch` starts the adapter. It also passes `imu_plugin:=libdiffbot_gazebo_ros_imu_sensor.so` to the robot model's xacro. Remo's model takes that argument (ros-mobile-robots/remo_description#17). DiffBot's model names the library itself.
- **Tests:** both robots' simulation test files, `sim_drive_diffbot.test` and `sim_drive_remo.test`, run two new test nodes.
    - [`test_imu.py`](https://github.com/ros-mobile-robots/diffbot/blob/5bad6d5b83280105e536173d0b93432ae310eda6/diffbot_gazebo/test/test_imu.py) checks the rate, the frame, the missing orientation, gravity at rest and a positive yaw rate in a left turn. `test_imu.py` waits for the drive controller before it drives. Its two tests share two helpers: `send_command()` sends a velocity command for a time, and `collect_messages()` returns the IMU's messages of that time.
    - [`test_imu_reset.py`](https://github.com/ros-mobile-robots/diffbot/blob/5bad6d5b83280105e536173d0b93432ae310eda6/diffbot_gazebo/test/test_imu_reset.py) lets the IMU publish for 3 s of simulation time, calls `/gazebo/reset_simulation` and waits for ten samples that are stamped within the first second of the new simulation time. Its limits are simulation time, read from the stamps. Its waits are wall time, because `rospy.sleep()` raises an exception when the clock goes back.
- **Remo's model:** both manifests pin remo_description `2.5.0`, and the checkout example in `CONTRIBUTING.md` names it. That version has the IMU with its orientation and position measured on Remo (ros-mobile-robots/remo_description#10 and ros-mobile-robots/remo_description#11), the colours of the real parts, the weighed IMU mass of 3 g and the xacro argument `imu_plugin` (ros-mobile-robots/remo_description#17).

**Limits:**

- A version of Remo's model before 2.5.0 doesn't declare `imu_plugin`. xacro ignores the argument then, and Remo's model loads gazebo_plugins' plugin. The pins name `2.5.0`.
- After `/gazebo/reset_simulation`, the IMU publishes again at once. The drive controller's odometry and the joint states don't: `diff_drive_controller` and `joint_state_controller` (ros_controllers 0.22.0) keep the time of their last message and publish again only when the clock has passed it. In a run that was reset at 8.66 s, the IMU's first stamp after the reset was 0.02 s, and the odometry's was 8.68 s. This pull request doesn't change the controllers.
- `catkin build` prints one new warning: catkin's deprecation notice for `gazebo_dev`, the package that provides Gazebo Classic's headers, which reached its end of life in January 2025.
- `raspi-setup.sh` builds every package of this repository on the robot's SBC, so the SBC compiles the plugin too and gets Gazebo's development headers through rosdep. The robot container builds a fixed list of packages without it.
- The documentation quotes the plugin and the tests by line ranges. ros-mobile-robots/ros-mobile-robots.github.io#89 pins them to this pull request's head and moves the pins to the merge commit.

**Built on earlier work.** Joe User (@joeuser846) added an IMU link to DiffBot's URDF in #60.

## How it was tested

In the dev container's image (ROS Noetic, Gazebo 11.15.1), with Remo's model at `2.5.0`.

- **CI at the head `5bad6d5`:** [CI](https://github.com/ros-mobile-robots/diffbot/actions/runs/42862671870) and [Dev container](https://github.com/ros-mobile-robots/diffbot/actions/runs/42862671837) pass. Both industrial_ci jobs report 37 tests, 0 errors, 0 failures, with `test_imu.py` and `test_imu_reset.py` for both robots. The head's commit changes only `test_imu.py`: the helpers `send_command()` and `collect_messages()` and clearer names. CI is this commit's test. It wasn't run in a local simulation.
- **The reset,** at `255a681`, the commit that adds the handling. The commits after `255a681` add the destructor's check and the helpers in `test_imu.py`.
    - With the plugin's source from the commit before, `5f87718`, `test_imu_reset.py` fails for DiffBot. After a reset at stamp 3.013 s, no sample is stamped earlier than 1 s in 30 s of wall time. The first samples after the reset are stamped 3.041, 3.061 and 3.081 s. The other tests of `sim_drive_diffbot.test` pass.
    - With the handling, `sim_drive_diffbot.test` and `sim_drive_remo.test` pass, 6 tests each. The first samples after the reset are stamped 0.020, 0.040 and 0.060 s, for both robots.
    - A probe on DiffBot after a reset: both IMU topics at 50 Hz from 0.02 s on, and a commanded left turn of 0.5 rad/s reads 0.52 rad/s.
- **A failed `Load()`,** at `a19aae4`, the commit that adds the check: a world whose IMU plugin has no `frameName`. `gzserver` ends with exit status 0. With the plugin's source from `255a681` it ends with a segmentation fault, exit status 139.
- **The stall (#185):** the loop that reproduced #185 starts the simulation with `sim_drive.launch` again and again in 8 containers and waits for the drive controller's first odometry. With the copy, at #154's head `67f8643`: 307 starts with DiffBot and 302 with Remo, 0 stalls. With gazebo_plugins' plugin, at `9bf8de6`: 13 of 166 starts stalled. #185 has the diagnosis, the backtraces and the counts.
- **Motion,** at `54698e3`: DiffBot drove 0.581 m and turned 1.489 rad, Remo 0.590 m and 1.500 rad, with odometry and Gazebo's true pose agreeing. The simulated IMU reads about +9.8 m/s² upward at rest.
- **Build:** the workspace builds, with the `gazebo_dev` notice from Limits as its only new warning. clang-tidy 18's `misc-include-cleaner` reports one name in the plugin's source, `ignition::math::Rand`. The file includes `<ignition/math/Rand.hh>` for it, which libignition-math6 6.15 redirects to `<gz/math/Rand.hh>`.
- **Earlier heads:** the runs of this pull request's earlier commits are on its [commits page](https://github.com/ros-mobile-robots/diffbot/pull/153/commits).

## Merge order

After ros-mobile-robots/remo_description#10, ros-mobile-robots/remo_description#11, ros-mobile-robots/remo_description#12, ros-mobile-robots/remo_description#13, ros-mobile-robots/remo_description#14 and ros-mobile-robots/remo_description#17, which are merged and tagged up to `2.5.0`. The EKF (#154) builds on this pull request.

## Documentation

Two docs pull requests cover this one. ros-mobile-robots/ros-mobile-robots.github.io#68 documents the IMU series: the firmware on the Teensy (#152), this simulated IMU (#153) and the EKF (#154). For this pull request it adds the IMU tests to [Testing and CI: Simulation test](https://ros-mobile-robots.com/pr-preview/pr-68/development/ci/#simulation-test). ros-mobile-robots/ros-mobile-robots.github.io#89 builds on #68 and explains the simulated IMU on an overview, [IMU: Simulation](https://ros-mobile-robots.com/pr-preview/pr-89/packages/imu/simulation/), with four sub pages. They cover [where the IMU sits on each robot](https://ros-mobile-robots.com/pr-preview/pr-89/packages/imu/simulation/position-and-axes/), [how a sample becomes a message](https://ros-mobile-robots.com/pr-preview/pr-89/packages/imu/simulation/measurement-to-message/), [why the plugin doesn't spin and how the plugin handles a reset](https://ros-mobile-robots.com/pr-preview/pr-89/packages/imu/simulation/plugin/), and [the tests](https://ros-mobile-robots.com/pr-preview/pr-89/packages/imu/simulation/tests/). #89 also shortens the [Simulation page's IMU section](https://ros-mobile-robots.com/pr-preview/pr-89/diffbot_gazebo/#inertial-measurement-unit-imu) to links to these pages. #152 is merged, so the docs wait for #153 and #154. #89 merges after #68.

Closes #185

Part of #21.

Co-authored-by: Joe User <1274372+joeuser846@users.noreply.github.com>
fjp added a commit that referenced this pull request Nov 12, 2026
## 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>
This pull request was closed.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants

⌂ 换网址