Custom localizer issue

[Team14423]

We're trying to implement a custom, dual localizer and are having trouble getting data in and out of it. Here is the relevant code:

In MecanumDrive:

 public void initLocalizer(HardwareMap hardwareMap,Boolean red){
        //mecanumLocalizer= (MecanumLocalizer) this.getLocalizer();//tried this first and it didn't work
        Pose2d tempPose=getPoseEstimate();
        setLocalizer(new DualTrackingLocalizer(this, new MecanumLocalizer(this),UltrasonicTrackingLocalizer.makeUltrasonicTrackingLocalizer(hardwareMap,this,this.getLocalizer(),red)));
        setPoseEstimate(tempPose);
    }

Key part of our Dual constructor:

public DualTrackingLocalizer(SampleMecanumDrive robotdrive, Localizer mecanumLocalizer, Localizer ultrasonicLocalizer){

        this.mecanumLocalizer=mecanumLocalizer;
        this.ultrasonicLocalizer = ultrasonicLocalizer;
        drive = robotdrive;

Relevant portions of our update:

public void update() {
        mecanumLocalizer.setPoseEstimate(ourPose);
        mecanumLocalizer.update();
        ourPose=mecanumLocalizer.getPoseEstimate();

getPoseEstimate, as you might guess, returns ourPose.

So, here's the deal:

1. getPostEstimate works fine - we can return ourPose (i,0,0) and iterate i+=.1 each cycle
2. update AND mecanumLocalizer.setPoseEstimate and getPoseEstimate work fine - same deal- we send in a new (i,0,0) each cycle and it feeds us the new Pose when we get it.

So, we know that our localizer has been set, and that get/set/update are being called from drive

Now, here's the problem. mecanumLocalizer.update() does not seem to update our positions. We run .update, and then mecanumLocalizer.getPoseEstimate() doesn't move. It doesn't reset either. If we start somewhere else, it stays there, even if we move the robot.

We have no idea why. Looking at the code in mecanumdrive.kt, it appears to be calling drive.getwheelpositions, so it makes me wonder whether there is some problem with how we are passing the drive object into the mecanum localizer OR into our localizer. We tried using both drive.getLocalizer() to pass in the default localizer (which has already been initialized and working) or creating a new one (and passing in "this"), but neither seem to work for the mecanum localizer to call the drive encoder positions.

Are we doing something wrong? Is there a problem with the code? We haven't had a chance to test this with our own localizer yet, as it won't matter if we can't get this to give us data. Thanks for any input.

[rbrott]

The method MecanumLocalizer#setPoseEstimate() clears the last known set of wheel positions. The clumsy internal state prevents you from exchanging pose estimates like that. You'll probably have to copy the MecanumLocalizer code into a new localizer to circumvent this limitation.

[Team14423]

Aha - I saw that today. We decided to instead just create and manage a second localizer and updated them both right from the drive. But even then, we want to set the mecanumLocalizer to our alternate version when we get a fixed location. BTW, this is modeled on the Vuforia sample you did in response to an issue - where you set the "hifreqlocalizer" at the start of the update. https://github.com/acmerobotics/road-runner-quickstart/blob/ab08fd6986649aee4073d161e169d6e11dc1891d/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/ComplementaryVuforiaLocalizer.java

All that aside even if you reset the last wheel positions, won't it pick up from there since it's using deltas?

[rbrott]

BTW, this is modeled on the Vuforia sample you did in response to an issue - where you set the "hifreqlocalizer" at the start of the update.

Yes, I've been hoisted by my own petard 😆

All that aside even if you reset the last wheel positions, won't it pick up from there since it's using deltas?

Sure thing. That's the least invasive change to the source.

[Team14423]

Sure thing. That's the least invasive change to the source.

Well, the code gave us inspiration. But what I meant was why do we need to change the source? When you set the poseEstimate, even if the wheel positions reset, when you move again it will calculate a delta from 0 and adjust the new position. In our case, the problem was that it never changed at all. I think we figured out why, but in any event, we have something working, though our sensors are not, so that's the next step.

[amarcolini]

The MecanumLocalizer needs 2 update cycles for any change to be registered. You’re setting the pose estimate (and clearing the previous wheel positions) every update cycle. To fix this you can instead set the pose estimate periodically.

Also, it looks like your ultrasonic localizer already takes a MecanumLocalizer in its constructor, so you might not need the dual localizer.

[Team14423]

The MecanumLocalizer needs 2 update cycles for any change to be registered. You’re setting the pose estimate (and clearing the previous wheel positions) every update cycle. To fix this you can instead set the pose estimate periodically.

Thanks, good point.

Also, it looks like your ultrasonic localizer already takes a MecanumLocalizer in its constructor, so you might not need the dual localizer.

Nice catch - that's a vestige from when we tried to do it all in a single localizer (importing the mecanum into another one, rather than having one parent with two children). We had since removed it since it never actually got called in that other localizer anymore.