Bosh BNO055 IMU implementation

[YohanBorda]

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.

[rsilverioo]

@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.

odom0: /diffbot/mobile_base_controller/odom

odom0_config: [false, false, false, # X, Y, Z
               false, false, false, # roll, pitch, yaw
               true,  true,  true,  # X',Y',Z'
               false, false, true,  # roll',pitch',yaw'
               false, false, false] # X'',Y'',Z''
odom0_differential: false
odom0_queue_size: 10

imu0: /diffbot/imu/processed
imu0_config: [false, false, false,  # X, Y, Z
              true,  true,  true,   # roll, pitch, yaw
              false, false, false,  # X',Y',Z'
              true,  true,  true,   # roll',pitch',yaw'
              false, false, false]  # X'',Y'',Z''

Also check that everything works correctly. I'm currently tuning the navigation stack and I'm following these guides:
http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide
https://kaiyuzheng.me/documents/navguide.pdf

If I am convinced by the results of the tests, I will tell you about them here ^^

[YohanBorda]

@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. :)

[fjp]

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 encoder.ino script (encoder_imu.ino in your case). There is a new base_controller which I plan to adapt to publish the IMU data, here in this loop:

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

Lines 53 to 70 in e04c7dd

	// This block publishes the IMU data based on a defined imu rate
	ros::Duration imu_dt = nh.now() - base_controller.lastUpdateTime().imu;
	if (imu_dt.toSec() >= base_controller.publishRate().period().imu_)
	{
	// 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();

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.