search

simondlevy / BreezySLAM

Simple, efficient, open-source package for Simultaneous Localization and Mapping
GNU Lesser General Public License v3.0
758 stars 251 forks source link

Why computeVelocities returns doubled distance? #25

Open topin89 opened 6 years ago

topin89 commented 6 years ago

in vehicles.py distance calculates as:

    dxyMillimeters =  self.wheelRadiusMillimeters * \
            (math.radians(leftDiffDegrees) + math.radians(rightDiffDegrees))

Obviously, if two 1mm radius wheels rotate by 1rad we get 2mm distanse instead of one.

yet return value described as dxyMillimeters forward distance traveled, in millimeters

As far as I understand, rotation is also doubled, but I'm not sure. Maybe CoreSLAM uses doubled value, maybe degrees should be halved, maybe it's just error, I don't know. So, why?

ghormann commented 6 years ago

There is similar code in the C++ code and based on testing, the calculated distance and rotation didn't seem correct. 

poseChange.dtheta_degrees = this->wheel_radius_mm / this->half_axle_length_mm *
        (right_diff_degrees - left_diff_degrees);

When I modified Robot::extractOdometry() of my robot to calculate degrees rotate of each wheel to be 1/2, the calculations seems much closer physical measurements. I too would love to know if this is a bug, or something I'm not understanding.

simondlevy commented 6 years ago

This is legacy code that I haven't looked at in years. If you want to modify and do a pull request, that'd be great. Maybe an optional scaling parameter would be the best solution.