BNO055 IMU integration

[joeuser846]

fjp recently merged a PR from Russ76 that adds the file "encoders-bno.ino" but there seem to be no changes to the SBC or Teensy code to include or reference it. Is there a suggested way to integrate this IMU code?

Any guidance is appreciated.

[joeuser846]

I just discovered that current Teensy code doesn't run at all. It stops immediately with "Emergency STOP" and "IMU failed to initialize. Check your IMU connection". This is because the Teensy current 'scripts/base_controller/src/main.cpp' logic checks if the IMU is initialized but the code to actually initialize it is commented-out/missing.

[fjp]

Hi @joeuser846 let me clarify the software architecture of diffbot first. There are currently two approaches where the PID controllers for the two motors are located:

1. high-level approach: PID controllers running on the SBC in the hardware interface / ROS control loop, where also this part of the write() method is relevant.
2. low-level approach: PID controllers running on the mcu/teensy in the scripts/base_controller, and communicating with the publisher in this part of the write() method.

The first approach (high-level PID) was the one I implemented first, with just a simple ecnoders.ino running on the mcu. This has the benefit that you can tune the high-level PIDs through ROS' dynamic reconfigure. You can find more about it here in the documentation, including a video.

The second approach (low-level PID) was the one I introduced later. It uses the main.cpp and libs in scripts/base_controller.

It is not so nice that I put basically two different approaches in the write() method of the hardware interface, but I didn't want to break anyone's code and preference to go for either the high- or low-level approach. You can read more about the differences in the documentation (low- vs high-level PID.

@Russ76 built on the high-level approach and kindly added the IMU into a separate encoders-bno.ino script (beside the original encoders.ino) you can flash onto the teensy mcu.

So the scripts folder is also split into using either the scripts in

1. encoders: high-level approach
2. base_controller: low-level approach

Hope this makes it a bit clearer because I agree that this can be really confusing to new users. Otherwise please let me know.

After adding some unit test, I plan to refactor the hardware interface because it is not clear for new users that there are basically two interfaces (two publishers in the write method) available. The refactoring would consist of adding two methods that distinguish between the two approaches and then call either or in the write() method. Maybe also some renaming should be done in the scripts folder and clarified in the docs which things should be flashed to the mcu.

[fjp]

Another option might be to use two separate branches on git, but I am still planning to experiment which approach to take as the default.

[fjp]

Also I haven't forgotten about your PR in #60, which seems to be closed automatically because the branch was cleaned up it seems. Anyway I think this is really useful and I will include it at some point. By the way, this PR would be based on the low-level approach which is more thoroughly tested on my side.

[joeuser846]

Thanks for the explanation. I've been using the low-level approach on my devices thus far (I have a tank-like diffbot and I am now building a remo style device), however the github code is currently broken for me. If I load the scripts/base_controller on my Teensy 4.0 it fails immediately when I run diffbot.launch because the IMU code that used to be commented-out is now being compiled, but is incomplete. It throws the "IMU failed to initialize. Check your IMU connection" error. It seems to be in a halfway state where the BNO IMU initialisation is checked but not actually implemented.

[fjp]

IMU code that used to be commented-out is now being compiled

There should be no new issue, because this didn't change since two years:

but is incomplete

That's correct, for the low-level implementation there is no IMU implementation yet. There is just the following in the base controller:

diffbot/diffbot_base/scripts/base_controller/src/main.cpp

Lines 57 to 70 in 8d1f5b7

	// Sanity check if the IMU is connected
	if (!imu_is_initialized)
	{
	//imu_is_initialized = initIMU();
	if(imu_is_initialized)
	nh.loginfo("IMU Initialized");
	else
	nh.logfatal("IMU failed to initialize. Check your IMU connection.");
	}
	else
	{
	//publishIMU();
	}
	base_controller.lastUpdateTime().imu = nh.now();

That gives you the info that there is no imu initialized. It should probably be completely removed for now because it will probably pollute the output with those log messages.

For the IMU implementation, what is needed is to create a "imu lib interface/wrapper" here and then instantiate it in the base_controller class (similar to how it is done for the encoders). Or even better to use dependency injection for everything the base_controller depends on (e.g., encoders, motor controllers, pids, imu). When that is done the publishIMU should be created and used to publish the IMU data at the specified rate for the EKF (robot_localization node), which you seem to have done already in #60.

Which launch file are you using? For the low level approach and running on the real hardware, it should be the one in the diffbot_bringup package:

roslaunch diffbot_bringup bringup_with_laser.launch 

I saw you also edited them in your PR #60, so it seems you are using the correct ones already.

It stops immediately with "Emergency STOP"

The emergency stop should only be triggered when there are no commands received from the hardware interface. In case you receive commands the commandsCallback here should be called and "reset" the emergency stop time.

[fjp]

And I think how you leverage the IMU in your PR is also fine. I think the IMU data can also be published from a node that is running on the SBC. It doesn't have to be implemented on the mcu/teensy. But depending on the IMU update frequency, the mcu is probably a better option, compared to the Raspi SBC.

So you could take what Russ started with the ecoders_bno.ino and create a imu lib in the scripts/base_controller, as I mentioned before. This is at least what I plan to do, when I get more time again to work on this project.