Add ability to have intake and score algae locations

[linglejack06]

[linglejack06]

@jkleiber thsi doesn't relate to this ticket but don't forget to push the changes

[jkleiber]

@linglejack06 @aidnem pushed changes, @PChild and I did some mentor built snake screen stuff in the climb branch but we had problems with NT seemingly ignoring algae mode again. Maybe give it a try in sim and see if it's just a networking issue?

[PChild]

Ok I talked to Shorya and tracked this down in sim a bit more, it appears to be a problem with publishing order. I don't 100% understand it yet but it's something like if snakescreen publishes default values to NT before robot code starts and then robot code creates publishers they appear to assume that they're the only publisher and then the subscriber in robot code only listens to them. If snakescreen is started AFTER robot code the system behaves correctly. For now I'm going to remove default value setting from Snakescreen, this should fully resolve this issue but we'll validate at shop today. Leaving this issue open until we're positive of the fix.

[aidnem]

@PChild would re initializing all of the subscribers (and maybe publishers) in teleop init fix this issue if removing default value setting from Snakescreen doesn't?

[avidraccoon]

@aidnem At least with the way I understood what Preston said I believe that would just make it worse.

[PChild]

That would not help, I don't think. What I would like is for you all to add a function that sets sane defaults on teleInit: L4 coral, Smart autonomy, Reef algae score, coral mode, maybe which intake station

[PChild]

@avidraccoon can you push code from tonight please

[linglejack06]

@jkleiber wasn't this issue partially solved late last week? if so, what is left to add, i can open a PR with what is left

[jkleiber]

@linglejack06 I don't think we fixed this since the OTF positions were on the left(?) for every algae intake. We'd want them in the center

Idk if we are doing lineup after algae otf, but if we are then we need to add a camera offset for that to make it work

[linglejack06]

@jkleiber algae lineup is a thing (already has configured cross track as well called lineupCrossTrackAlgaeOffset i think)

do you want me to make otf poses that go to center? i was just using left that way we would for sure have at least one camera with an estimate for lineup (not sure if lineup will work once centered between two cameras at end)

[jkleiber]

@linglejack06 yeah we need to be centered because by the time we get to the OTF pose, we are nearly touching the algae with the claw.

I think both cameras should be able to see the tag sonce we'll be far enough away. Do we have an along track offset for algae as well?

[linglejack06]

theres no along track offset, since i figured it would be the same as coral (against the reef)

my only concern with lineup is that once we get close enough it might lose measurements when centered