IMU code

[Russ76]

Where is the code that includes the IMU?

[fjp]

Hey @Russ76, the IMU part is on the todo/future work list and therefore not available yet.

For now I just can tell you that I will soon make use of RoboticArts/ros_imu_bno055 or dheera/ros-imu-bno055. Probably the latter one because it uses the i2c protocol.

Also there will be an Extended Kalman Filter to fuse the IMU and encoder odometry data and hopefully improve the odometry.

[fjp]

And in case you are interested in solving this, your PR for this or other things is more than welcome :-)

[Russ76]

That sounds great! My yardbot (45 kg) uses the MPU6050 but the code also is still in the todo list. My robot uses Jetson TX1 and ZED camera, and is therefore restricted to Ubuntu 18.04 for now. I would like to try using RasPi4 (and your code) for the main brain and leave TX1 to handle camera stream. But I need Sabertooth for the motors, because they are large. Arduino Mega handles data between that hardware and Jetson.

[fjp]

Nice, sounds like a very interesting project! The code in this repo should also work on 18.04 but then you will probably need to install ROS melodic instead of noetic, which is intented for Ubuntu 20.04. For better vision capabilities I am also planning to use a Jetson Nano soon (instead of the RPi) running melodic and noetic in the future when 20.04 is available for this board (or use an already available, unofficial custom image). Another thing on the todo list ;-)

Please note that this project is for an indoor mobile robot which heavily relies on its 360 RPLidar for SLAM and navigation. So the diffbot_navigation and diffbot_slam might not be directly useful to you without having a sensor_msgs/LaserScan topic like DiffBot has. Either this topic or sensor_msgs/PointCloud is required for the navigation stack, in case you plan to use it. Though you might be able to use even these two packages (diffbot_navigation and diffbot_slam) when you convert your depth camera image to a laser scan message using for example ros-perception/depthimage_to_laserscan.

However, the code for the ROS control hardware interface should help you indeed and also the IMU part including the EKF, when it is done.

By the way this blog post about turtlebot might already help you with the sensor fusion of encoder odometry and imu data using the EKF. And ROS support for your MPU6050 seems to be there in fsteinhardt/mpu6050_serial_to_imu.

Finally, the code of Linorobot might be also helpful to you.

[Russ76]

That's funny, I was about to comment about Linorobot, was just looking at it! He has put a lot of good work into this. I am also making a smaller rover, a tracked version for indoors. And yes, I have used the depthimage_to_laserscan module before. Thanks

[Russ76]

I have an Intel D435 that might work well with the Nano. Both small!

[Russ76]

The Linorobot code base has problems with Python at this point. He uses Platformio and it now wants version 3.6 or newer, and ROS still has 2.7 in its code. Therefore I wasn't able to get the Teensy code to install.

[fjp]

Just for my reference, you mentioned that you are not using ROS noetic (which runs also on python 3.x)?

He uses Platformio and it now wants version 3.6 or newer

I use the Arduino IDE and the Teensy Loader because I also had troubles with Platformio and its python environment that it seems to create inside VS Code - it messed up my ROS python plugin of VS Code. So I removed the Platformio plugin again and used the Arduino IDE instead. I agree it is not the most comfortable IDE but that is what worked for me.

I think it should also be possible to use Linorobot's code and flash it using the Arduino IDE. You just need to install the latest ROS serial library to get Teensy board support.

[Russ76]

I am building a small indoor tank that can use Noetic. I have Mate installed on a 64G micro card and am following your directions, with Noetic. Raspberry Pi 4, 4Gig. YDlidar and Intel D435. The motors that came with the tank base have quadrature encoders.

On another micro card I have Ubuntu 18.04 with Melodic and tried to install Linorobot. He actually has machine code within his library files for the encoder!
Thanks

[fjp]

I am building a small indoor tank that can use Noetic. I have Mate installed on a 64G micro card and am following your directions, with Noetic. Raspberry Pi 4, 4Gig. YDlidar and Intel D435. The motors that came with the tank base have quadrature encoders.

Nice, then you should be more or less good to go using the Teensy Encoder library that I have also in use for my quadrature encoders. You can find the script here. Please note that I am using a custom encoder message, which is basically just an array of ints, because ROS doesn't have this type of sensor message. Before that I starte publishing just two ints, one for each encoder tick count. This also worked, however, it is best practice to give semantic meaning to your messages, hence the custom Encoder.msg.

He actually has machine code within his library files for the encoder!

I am not too much into Linobot's code but to me it seems that he uses an older version of the Teensy Encoder lib here because his latest commit in this encoder code is from 2018. I am not sure why he needs the additionaly utility code though.

Thanks

You are welcome.

[Russ76]

Teensy IMU code is posted in Russ76/Diffbot_results now. It worked well before my Rosserial complained of a version mismatch and stopped working. I compiled the Teensy code on the laptop with Ubuntu 18.04 and Diffbot ran on Raspi with 20.04! I also have a timeout function in there that is in "todo mode." I doubt that the timing of the main loop is ideal; this could be improved. But you may be able to use the IMU portion in Diffbot! Good luck

[fjp]

Thank you Russ! I will get back to this. Currently I only have the Bosch BNO055 at hand but I heard the MPU6050 is easier to integrate. I'll try first with the Bosch IMU via i2c. Depending on how it works, I'll eventually switch to the MPU6050 or add support for both.

It worked well before my Rosserial complained of a version mismatch and stopped working. I compiled the Teensy code on the laptop with Ubuntu 18.04 and Diffbot ran on Raspi with 20.04!

Interesting, I wasn't aware that this would cause problems for rosserial. Because of the different Ubuntu versions I assume you are running different ROS versions on your laptop and Diffbot? Diffbot is usually running on ROS noetic and I guess you have ROS Melodic on your Laptop with Ubuntu 18.04? It should be straight forward to get rid of the rosserial version mismatch if you can compile your teensy code under Ubuntu 20.4/noetic. But I guess this is what you already did before when it was working.

[Russ76]

Yes, Noetic on robot and Melodic on laptop. Next I'll try installing the Melodic Rosserial on robot. I couldn't get the IMU code to compile on Arduino IDE on robot, so had to switch to laptop. On the Raspberry Pi I had to press the Teensy button each time to get program to upload, and it would often upload just one time, forcing me to restart the IDE repeatedly. I tried using a micro card with a copy of Raspbian on it but ROS won't download binaries to that, forcing me to compile all of ROS. Then I found that adding another couple of ROS packages forced the install routine to recompile ALL of the packages. What a slow procedure! This was on a copy of Raspbian from Intel, with the Realsense library already installed.

The MPU6050 has another advantage, that it can be purchased with a plug and four wires from Adafruit, so no soldering necessary on the board. It is inexpensive.

[Russ76]

Franz, what I see online leads me to think that the error is a performance issue. Perhaps my main loop is sending too much information for Rosserial communication. I may need to slow it down to a decent rate.
(Next day) I changed the Delay(5) to 10, and it runs OK now on Noetic. Teensy CPU overwhelming the serial connection!

[fjp]

I did also run into problems when the delay was set too low. I added the error that I got when the delay was set too low in the comments of the encoders.ino script:

https://github.com/fjp/diffbot/blob/f764bcbef82c64ef08568acaba32668c6599e0c2/diffbot_base/scripts/encoders/encoders/encoders.ino#L66-L83

The loop() function will work at a very high speed (processor's max speed) without a delay, but the I/O device can't communicate at that speed. Similar to a while(true) {;} in c++, one of the CPU cores will be used entirely.

Linobot is not really working with delay() like we do. Instead he uses code like this:

//this block drives the robot based on defined rate
if ((millis() - prev_control_time) >= (1000 / COMMAND_RATE))
{
    moveBase();
    prev_control_time = millis();
}

This is not blocking the entire loop, but updating each function according to the duration.

I think it makes more sense to do it like Linobot and avoid using delay(). I hope to find time to improve the code running on the teensy soon. Although it might make sense to look into #4 (comment), which suggests that a microcontroller is not necessarily required. The driver that they use on the RPi requires a kernel module, which needs to be built. After that, it seems to be possible to go without MCU and read the encoder ticks or sonar sensors directly with the RPi. I am not sure about the IMU but it might work as well.

[Russ76]

You're right. The timing of the (firmware) main loop is critical. Teensy has an advantage there with its speed. The error message in Rosserial could be more instructional about the different ways it gets thrown. Thanks!

[adityasivaraj]

Hey guys! I am looking for advise on a similar project. I am trying to create an AGV too. In my case my ros navigation commands will go via arduino (uno/due) to the motor controller. Should I connect IMU to the Raspberry Pi directly or to the arduino? Because I know arduino is a single threaded device, and may not be able to handle both tasks...

[fjp]

Hi @adityasivaraj, I guess both options will work but I plan on connecting the IMU to the mcu (e.g., arduino) instead of the Rasbperry Pi. The code to run the imu at a fixed rate on the mcu is basically available here in diffbot/diffbot_base/scripts/base_controller/src/main.cpp. However, the actual IMU code to get and publish the data (publishIMU()) is not yet implemented:

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

Lines 52 to 70 in ff78f49

	// 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 is similar how linorobot is doing it (also on the mcu).

Whichever option you choose (mcu or RPi) it would be interesting to know which one worked out for you :-)

[Russ76]

Hi! I know it has been a long time... I'm working on getting a diffbot copy to work for my tank robot to prove "teach-repeat" repo that I found. I made pull request for Teensy code for use with the Grove motor controller, not the version with the feather wing.

Russ