How to us April Tag for re-localization #101
Description
(Using meepmeep here - hopefully that isn't the issue... Any help is appreciated! I tried reddit, but didn't get any takers.
Hopefully I'm not too far out in left field...)
Two thoughts on how to integrate April Tags into our auto - using roadrunner 1.0... (btw, rr + meepmeep looks great!)
- In a large auto TrajectorySecuence, use addDisplacementMarker when we want to re-localize. This doesn't work. I'm assuming (?) that when the TrajectorySecuence did the 'build()' it baked in poses? Or is this a meepmeep interaction? When the new bot pose is set, meepmeep flashes the robot in the new position, then immediately continues ignoring the changed position.
// Drop off second one .splineTo(new Vector2d(aprilTagDropoff.getX(),aprilTagDropoff.getY()),aprilTagDropoff.getHeading()) // Read AprilTag for re-localization .addDisplacementMarker(() -> { // This runs after the last splineTo completes - we want to be mostly stopped for april tags to work currentPose = getAprilTagPose() // current pose if we don't see the AT // Testing - re-localize enough so can see on meepmeep Pose2d testPose = new Pose2d(currentPose.getX()-20,currentPose.getY(),currentPose.getHeading()); myBot.setPose(testPose); // relocalized - but not working }) - Use multiple TrajectorySequence (s) - with the AT read between them. Similar to 0.56 . This isn't working for me - in the example below only the second/last TrajectorySequence is run.
myBot.followTrajectorySequence(tsAlpha); myBot.setPose(getAprilTagPose()); // will return currentPose if AT not found TrajectorySequence tsToDropOff = myBot.getDrive().trajectorySequenceBuilder(myBot.getPose()) // Drop off second one .splineTo(new Vector2d(aprilTagDropoff.getX(),aprilTagDropoff.getY()),aprilTagDropoff.getHeading()) .build(); myBot.followTrajectorySequence(tsToDropOff);