The question is the following, I have a robot that currently recieves odometry from the encoders of the motor controller. I also can get odometry based on the laser scans. So I have two sources of odometry which are both pretty accurate. Now, what approach should I take if I want to switch between different sources of odometry. For example, I receive wheel odometry for 5 minutes and after I want to switch to laser-odometry for another period of time and after that switch back to wheel-odometry. I know that this can be achieved using behaviour trees, but I am not very familiar with this structure and it is quite complicated to modify, is there an alternative? Thank you in advance.
Just an idea that I would to share with you. This may not be a solution but could be an improvement.
Since they both are “pretty accurate”, why not try and use a sensor fusion algorithm to make use of both the odometries. That is, if you are not constrained by memory or space, you can try an algorithm like Kalman Filter.
I said this only because you said you are not much comfortable with Behavior Trees as it is bit complicated.
Again, this is just a suggestion, not a solution, but could be a solution too!
Hi Girishkumar,
Thank you for your response. I am already making use of the robot_localization library that is great when fusing different sensors. The idea was that in my case, I would like to switch between different odometries given the requirement that I have. And amcl needs odometry for comparing last known pose(If I am not mistaken).
Great!, I wanted to tell you that earlier - about the robot_localization package.
I have yet another suggestion for you.
AMCL needs odometry, yes, correct. But if you are sure that odometry from encoders is as accurate as odometry from laser scanner, then you can create parameter that can be switched between encoder odometry and laser scanner odometry.
Have a program whose only task is to check for this switchable parameter. Since odometry message will be the same structure, have this program switch between the two odometry methods when the parameter changes.
So, here is an outline.
Start with the parameter set to encoder odometry or laser scan odometry.
AMCL needs odomtery input from a topic, let’s call this /custom_odom.
Create a program that has two subscribers - one subscribes to odometry from encoders, the other subscribes to the odometry from laser scanner.
Activate the subscriber for odometry from the initial method you chose in step 1.
When you want to change the odometry source, just change the ROS parameter.
Detect the change in the parameter in this odometry switching program and activate the subscriber of the other odometry method.
Write the odometry output into /custom_odom which will be used by AMCL or Nav stack.
The output from both the odometry methods would be the Odometry message that AMCL will use.
This way you can switch between your odometry sources quickly.
I hope my explanation is clear. Let me know if you can’t understand.
This is just a suggestion, not a solution. This can become the solution if you decide to make this a solution!
I implemented a node that subscribes to both sources of odometry and I publish the values to wheel-odom and lidar-odom respectively. Both of them them are making use of the robot localization library (going through a kalman filter) and publishing to /odom. The problem is, eventhough I switch between them, whel odom still publish zero values to odom. I need a way to pause this node from robot localization library so it only listens to one source of odometry. Hopefully you can understand what my problem is…Please let me know if there is a solution.
You said that you want to use either encoder odometry or laser scanner odometry, right?
Providing both odometry to Kalman filter might produce zeros if one of them is not working or sending zeros.
The problem might be at the source of sensors. Perhaps one of them or both of them are not publishing Odometry values properly into the wheel-odom and lidar-odom. Check if you are getting proper values from wheel-odom and lidar-odom. rostopic echo /wheel-odom -n1 and rostopic echo /lidar-odom -n1.
Whichever is giving you zeros, needs to be fixed.
If you have fused wheel-odom and lidar-odom into Kalman filter, then switching them will only give you bad results. Use one of them as input to the robot_localization package, not both.
Let me know when you have both wheel-odom and lidar-odom working properly.
Yes, both of them provide odometry values if run independently, I did set a timer and I am trying to switch between different types of odometry based after 1 min. Perhaps the error is somewhere in the code. I will take a closer look at it. Thank you for your help!!