I’ve rewritten the tricycle plugin so that it now produces good navigation using rqt commands in Gazebo, with good correspondence to maps produced in rviz using rtabmap. You can download my code to experiment with here
My inspiration came from the Instantaneous Centre of Rotation discussion here.
Input is greatly appreciated! Let me know if there are other files you need in order to play around with it.
- I’m concerned that my sign conventions for x and y seem to be reversed (compared to what I imagined them to be) in the code (see line 357 onward).
Motion looks good in simulations, but I worry that this is producing the troubles I’ve experienced reaching 2D nav goals in rviz (aborts goals, tries to “back in” to them).
It could be a deeper problem with the teb_local_planner not really being designed for Ackerman robots, but I definitely don’t see it helping if the robot wants to “back in” to all sent goals.
From what I can tell, the conversion from angular velocity commands to steering angle that is built into the teb_local_planner is incorrect when applied to Ackerman robots: it ought to be angle = asin(omega / v * wheelbase), as opposed to angle = atan(omega / v * wheelbase).
This change would move the point of interest for angular velocity commands from the midpoint of the rear wheels (as in a diff drive robot), to the point of contact with the ground of the steering wheel.
- EDIT: WOoooooooo… The answer to was as simple as setting: angle = - asin(omega / v * wheelbase)
- Negatives, man…
- Possibly related is the fact the velocities in odom seem to be given in the global frame. With the differential drive plugin, you only ever see linear x velocity being non-zero (linear y velocity = 0 always).
Could this be because the odom updates (for the steering wheel location/orientation) are being defined by reference to the “special point” between the rear wheels (see line 401)?
Also possibly related is an apparent (systematic) mismatch between the twist message in odom and the cmd_vel values? These values match when using the differential drive plugin, but does the difference in kinematics for an Ackerman robot make a mismatch here totally normal?
- Another concern is drift. Errors accumulate when switching between hard clockwise and counterclockwise turns. There may be an issue with how I’ve handled setting the steering angle to avoid jittering around angle = 0 (see line 288).
- Alternatively, it might be a problem with the friction in my model?
EDIT: BIG ISSUE (EDIT2: NEVERMIND, I’M AN IDIOT):
TURNS OUT MY IF STATEMENT IN LINE 293 WAS if (diff_angle < target_angle) INSTEAD OF if (diff_angle < 0)
- I’ve noticed that when changing steering directions, the target/applied angle suddenly doubles! The effect is zeroed out whenever an rqt command steering angle of 0 is sent.
Example: the target angle perfectly matches the rqt steering angle as you increase it from 0 to, say, PI/3. But then if you try to lessen the rqt steering angle to PI/4, it will actually INCREASE to PI/2. If you then lessen the rqt steering angle to -PI/4, it will perfectly match that. But then, increasing the rqt steering angle to -PI/6 gives you -PI/3.
What in the world could this be coming from?!?