Before experimenting with Pose estimation with AprilTags, I had a pretty simple task: generate a trajectory to an AprilTag and follow it.
You can see the code for my GoToTagCmd here.
The basic idea is that whenever I want to go to an AprilTag, I reset the robot’s odometry to 0,0 and set the ending pose of my trajectory to the AprilTag’s 3D pose. Two interior waypoints are created and a path planner path is constructed for the robot to follow.
This has worked well; you can see it in action here.
For the past few days, I’ve wanted to upgrade this by incorporating the Pose estimation with the April tags so I don’t have to reset the robot’s odometry to zero every time. I also wanted to leverage this so that if the robot knows where it is on the field, my auto routines can be more accurate and robust.
I’m using the SwerveDrivePoseEstimator class and the .addVisionMeasurement method to update my odometry periodically. I tested it out, and I think it’s working okay. For example, if I start without looking at an AprilTag, the robot starts at 0,0. Then, when I rotate the robot and the camera detects the AprilTag, the robot automatically goes to the field-relative position (ID7 is the speaker tag). You can see a demo of what I’m talking about here.
My question is, how would I change my GoToTag code to account for the robot’s field-relative pose? I don’t want to reset my start pose in the odometry to 0,0 anymore; I just want to be able to go to different places on the field. I tried using Pathfinding, but I’m getting some really weird results. For example, in my RobotContainer, I’ve bound the Pathfinding command to a button to be only while held. I just entered the pose to be (7, 5.55, 60 degrees), which is like 7 meters away from tag 7 on the field but still the same exact Y dimension. So my idea is that I detect the tag, and then I want to go 7 meters away from it with Pathfinder. The problem is that the Pathfinding command generates a weird trajectory that is just a bunch of zigzags and is incorrect. You can see what I’m talking about here. I kept spamming the button to see if the trajectory would update at all, but it just stays as a bunch of zig zags and is definitely not the shortest and most optimal bath shown in the Pathfinder documentation on Path Planner’s website.
Vision code is here.
Swerve code is here.
I’m honestly really lost on how to integrate this new pose estimation. The estimate from the tag seems to initially place the robot in the correct place on the field, but I just need help getting more advanced features like pathfinding working and how I could use that to align with the speaker instead of resetting the odometry and creating a trajectory like I was earlier. Worst case scenario, I’ll just keep resetting odometry and path following to the speaker with the April tag, but I would really like to implement some sort of localization to effectively use vision in auto as well.
I know this post is kind of convoluted, but any help/strategies I can implement is greatly appreciated!
