Added option to obtain velocity directly from joint_state.

[tappan-at-git]

Per issue #159, added the option to use joint_state velocity instead of the difference between joint_state positions in diff_drive_controller Odometry. I noticed a previous pull request for this feature, but it seems to have died.

• The default behavior is to continue estimating velocity by comparing position data.
• Setting the diff_drive_controller/estimate_velocity_from_position parameter to false will make the diff_drive_controller use a different Odometry::update method which uses joint velocity for its velocity estimates.
  • Any update cycle that includes NaN velocities will use the original, position-based update method instead.
  • These velocity estimates are integrated to find position. The joint position data is stored but ignored, unless velocity is NaN for that update step.¹
• I have implemented two tests for this new parameter (discussion on previous pull request):
  • diff_drive_raw_velocity.test : The controller runs correctly if estimate_velocity_from_position is set to false.
  • diff_drive_raw_velocity_invalid.test: The controller still runs correctly if estimate_velocity_from_position is set to something_invalid, defaulting to comparing position data. If another behavior is expected please let me know so I can fix this.
  • While @bmagyar mentions a third test where estimate_velocity_from_position is explicitly true, my instinct was the existing diff_drive_controller.test was probably sufficient to test this default behavior. If there's interest in a test with the parameter set explicitly, I'll add it.

1: I considered adding options to support the current behavior for position while also smoothing joint velocity, but decided that rabbit hole was too deep to explore without a clear need. If nothing else, figuring out what to name all the parameters was horrifying. ↩

[efernandez]

Thanks for your contribution, but I'd like to have clearpathrobotics#10 merged upstream. Do you think it'll also fix your needs? I think I could have all clearpathrobotics merged upstream in a couple of days, and then you can add extra features here. What do you think?

[tappan-at-git]

Sounds good! That seems to address everything I tried to.

[efernandez]

I'm working on:

1. merging all pending PRs on clearpathrobotics fork
2. adding a unit test suite for the speed_limiter in order to diagnose and fix a problem with a jerk limits integration test that sometimes fails and is preventing a new release for ros_control
3. sync clearpathrobotics fork with upstream and move it upstream, so any new development is done upstream

I expect to finish all this by the end of next week.

[efernandez]

I have something like this in clearpathrobotics@22b106a and a bunch of critical issues addressed in later commits. That's the only reason I'd refrain for merging this... it'd be difficult to bring my stuff upstream, but if we diverge here it'd be even more painful.

[wxmerkt]

Hi @efernandez et al.
Any update on having a proper fix for #159 merged? We noticed the fix in the Clearpath fork and wondered whether/when it will be upstreamed.

Thanks,
Wolfgang

cc @majcote @tonybaltovski - per the phone call just now

[tonybaltovski]

👍

[efernandez]

@wxmerkt I'm quite busy, but @tonybaltovski will help me to get the bits you need upstream.

I'd try to bring other features soon as well, as the covariance, etc.

My major concern is that some commits introduced a few bugs that got fix. Somehow I'd prefer to get everything go upstream together, as the latest code has been tested for almost 1 year (for some features) with no changes and giving no problems.
What's your feeling on this @bmagyar ? :)

[bmagyar]

I'm at bit reluctant about bringing everything upstream as a single update as there are plenty of robots from various companies & labs using this controller. I'd prefer the usual approach of one feature + tests at a time.

[wxmerkt]

We've done some more testing locally and noticed that e.g. the naming of some parameters changed and some behaviours regressed compared to the current debians. The addition of the dynamic reconfigure is nice and should become a default in my opinion.

While we are keen to explore getting the use-velocities-for-odometry-pose-estimates in, I'd recommend against pushing the entire fork indigo-devel head upstream at once and go feature by feature - we've seen a few regressions e.g. when it came to raw orientation (@majcote was there during testing and may be able to comment some more) so more testing would be recommended. Also, @efernandez are you using your fork internally or the debians for the research bots?

[efernandez]

@bmagyar @wxmerkt Fair enough, especially because each feature will have to be validated and tested. I don't think the fork is used at all (or has been tested) in research platforms.

I still need some feedback to understand what regression we faced. I know the params for the wheel joint source are new and different from anything that were before. I'd have to look into it to understand the problem though. @majcote Keep me up to date with your findings, please.

[wxmerkt]

Here's a few loosely collected observations:

1. With the same rosparam configs, the velocity of a robot with a non-1.0-radius/1.5-distance-multiplier will be different than with the upstream controller - about half as fast unless also set to left_wheel_radius/right_wheel_radius. Similarly for angular velocities.
2. The raw odometry pose for only-angular and only-linear driving will be different - independent of whether we'd do the pose-from-position or pose-from-velocity
3. In general, with upstream we've noticed that angular pose estimate is bad from wheel estimate and good from EKF (as expected), and linear pose estimate is good from wheel odometry and bad from EKF (the latter surprising). Cant exactly remember the with-fork case but it was different.
4. There's 2 ROS bags with the upstream controller I've shared with Tony and Martin.

[efernandez]

@wxmerkt

1. I think you mean the left_wheel_radius_multiplier and right_wheel_radius_multiplier. Yes, they were added, and I think in your test they were in. If you where using multipliers other than 1.0, you need to update the params.

clearpathrobotics@9683ef1#diff-380f38b77ed7f79a1de5cf7de13f429eL166

2. How much different?

---

I think we should cherry-pick a single change at a time and for param changes I can try to provide a backwards compatibility solution, so things don't break unexpectedly.

https://github.com/clearpathrobotics/ros_controllers/pull/18/files is the PR that provided the wheel joint source to compute the robot space pose/twist. However, is on top of other changes. I'd start pushing them in order one by one, but if you're in a rush, maybe it's better to get this PR and I'd handle any conflicts later.

Also note that in later changes, I fix how the twist is computed; that's why I said I can only guarantee the latest version of our fork works well, while getting intermediate versions could show different (and sometimes bad) behavior.

[wxmerkt]

Thanks for looking into this Enrique. We changed the radius multiplier to match our gearbox, but the generic wheel_radius_multiplier appeared to not have been picked up. We didn't do any extensive digging/change comparison though. We tried both the tip of your fork as well as the specific commit you recommended. I'm out at a conference for the next few days and won't have time to extensively debug afterwards, I am afraid. Martin dropped me a message saying they found some smoking gun in the bags we sent so will work off that. Apologies for not being more helpful with this at the present stage.