Abstract class for keeping track of field items

[wraftus]

🚀 Feature Request

We want some way to represent the status of game pieces and different objects on the field, along with a certainty of them being in that state.

[matthew-reynolds]

I prefer using ros msgs internally as storage types as opposed to making structs that duplicate the msg, since duplication is fragile and we need the messages anyways.

I propose we store/publish this data as a list of each known entity type. If a entity doesn't have any special additional parameters, it can use the GenericEntity type, otherwise it extends the GenericEntity with additional info.

EntityArray.msg

std_msgs/Header header
GenericEntity cargo[]
GenericEntity hatches[]
RobotEntity robots[]
GenericEntity unknown_entities[]

GenericEntity.msg

std_msgs/Header header
uint64 uuid               # Unique ID for this entity
float32 confidence        # Confidence the object exists? Is at this position? I'm not sure. Perhaps we need multiple confidence fields.
geometry_msgs/Pose pose   # 3D Point and Quaternion
geometry_msgs/Twist twist # 3D linear and angular velocity
shape_msgs/SolidPrimitive # Bounding box of entity (Maybe shape_msgs/Mesh, but probably not)

RobotEntity.msg

uint8 ALLIANCE_UNKNOWN = 0
uint8 ALLIANCE_OPPOSING = 1
uint8 ALLIANCE_PARTNER = 2

std_msgs/Header header
GenericEntity entity
uint8 alliance
uint8 team_number
float32 has_cargo_confidence
float32 has_hatch_confidence

Alternatively, we could use a more generic form, but then it becomes harder to store type-specific data. I definitely prefer the form above, but just for completeness, here's another potential structure:

EntityArray.msg

std_msgs/Header header
Entity entities[]

Entity.msg

uint8 TYPE_UNKNOWN = 0
uint8 TYPE_HATCH = 1
uint8 TYPE_CARGO = 2
uint8 TYPE_PARTNER_ROBOT = 3
uint8 TYPE_OPPONENT_ROBOT = 4

std_msgs/Header header
uint64 uuid               # Unique ID for this entity
uint8 type                # Entity type, one of the above constants
string additional_data    # Key/Value pairs, for example. (This is super ugly but it could get the job done)
float32 confidence        # Confidence the object exists? Is at this position? I'm not sure. Perhaps we need multiple confidence fields.
geometry_msgs/Pose pose   # 3D Point and Quaternion
geometry_msgs/Twist twist # 3D linear and angular velocity
shape_msgs/SolidPrimitive # Bounding box of entity (Maybe shape_msgs/Mesh, but probably not)

Looking for feedback, please tell me what you do/don't like so we can get a version of this in place. In particular, I know the confidence fields need refinement since I don't really know what needs to be stored.

[wraftus]

Our original plan was to make an abstract class and have robots and game pieces extend those, but the ros msg way is probably easier and would probably make our lives simpler in the long run.

[wraftus]

Oh forgot to add, the only additional thing is we want the confidence to decay over time, and the function that describes that may be different depending on the game piece. Exposing this function to whatever planning part reads these msgs might not be needed, but we'd still need someway to link each msg to its decay function, and this was kind of the main motivation behind the abstract class thing (since we'd just have the decay function be overwritten for each type and be called every so often to update the confidence)

[matthew-reynolds]

Gotcha, I see what you mean. From a code-structure point of view, if we go with ros messages as internal data structures, the decay functions could be done with a map of entity type to decay functions rather than member/static class functions. I agree that nothing outside of perception really needs to know that decay function so it probably doesn't make sense in the message.

Can you help me understand what confidence field(s) we will need for a generic entity? Is it as simple as "Confidence that this item exists in this location" or does a generic entity need more values than that?

[wraftus]

I feel like there would be confidence in our initial measurement (might not be needed all that much) which would be time independent. The part that would decay over time is our confidence that it is still there, which could be tied to some radius of places it could be, if that makes sense

[matthew-reynolds]

Makes sense, from that description I agree it sounds like the first is much less important especially because the "initial measurement" is being constantly updated while the item is in view.

But the second certainly seems critical. My understanding if it is:

• Every frame we see the item and confirm it is still there, the "Yup it exists" confidence jumps back up to some high value corresponding with how confident our classifier was.
• Every frame we see the location where we thought the item was, but we can no longer perceive it at/near that location, the "Yup it exists" confidence drops dramatically.
• Every second we are unable to see the item due to obstructions (Other robots passing by, us being on the other side of the field, etc), our "Yup it exists" confidence decays according to some function

If that understanding is correct, then it sounds as if a generic entity only requires a single confidence field, right?

[wmmc88]

A single confidence field yes. The confidence for the actual "initial" measurement, I imagined to be an internal thing. Every frame when we take in data, we could be like that looks like a hatch sort of but not entirely sure, so it'd bump up the confidence a lil bit. So the confidence field increases and decreases amounts proportional to the confidence in the new reading itself. The decay would probably be some sort of Bayesian probability model, but more research to be done

[wmmc88]

I would prefer an even more specific version of ur proposed msg because type safety and also some objects I think shouldn't have certain fields (ex. Twist of hatches). I see no benefit of having a generic entity over specific ones

[wmmc88]

Also with the 3d pose pertaining to the origin of the object, I assume the shape msg is to define its actual volume in space? That would be static correct? Not sure we should include that in the message itself

[wmmc88]

wrt the decay functions, we could be getting state data from various sources and could processed in different nodes, but the decay should be standard for all of them. We could technically add it as a function that's associated with the msg itself by adding it to the msg header. Perhaps we might want to provide additional functions associated with the message as well.

[wmmc88]

I also would probably separate the state arrays for pieces that are still in play vs pieces that are scored

[matthew-reynolds]

I think hatches should still have twist data. Theoretically, they can roll, but even ignoring that I would prefer to explicitly tell the rest of the stack that it is stationary rather than just assuming that all hatches are always stationary.

I see the generic entity as a "base class"-ish, since inheritance isn't possible with messages. I like the idea of enforcing that all Entities will contain this set of fields, and I think it's definitely poor design to create multiple identical messages. If you're worried about type safety (I'm not, I think accessing EntityArray::cargo[] is perfectly OK), then I would prefer adding a type field to the message rather than creating multiple copies of the message. type could either be an enum or just a string.

You're right, the shape is a 3D volume that we'd use as a bounding box for the entity (a prism, sphere, cylinder, etc). It definitely should be in the message for Robot entities, since they can change their shape, and it is certainly required for Unknown entities. I see where you're coming from with thinking it's redundant for cargo and hatch panels, but I think it still belongs in every message - Even if it is a bit redundant, it's definitely safer and cleaner than trying to have every package in the system access a common 'Hatch Panel Shape' definition somewhere. It's also not a large message - Spheres are defined solely by a uint8 type and a float64 radius. Cylinders are a uint8 type, float64 height, and float64 radius. Boxes are a uint8 type and three float64 x, y, zs.

I'm not sure what you mean by adding it to the message header, but if the decay functions will all be handled by nodes in the same package, which seems likely, then I feel it makes sense for the decay functions to be defined in a yaml config file. You definitely don't want them hard-coded into the messages, because then every time you change the decay function you have to change the message definition and that's a bad time. So they have to be defined somewhere else anyways, and at that point, it seems to me that the node publishing the entity would also be the node handling its decay, so I don't see any reason to publish the current decay function. Perhaps I'm missing something here though.

I agree 100% with separating gamepieces in play with those scored, I had in mind that this list of entities this issue is tracking would only be the unscored game pieces on the field. Game pieces that are scored, in the HP station, or in another robot I imagine should all be tracked separately.

[matthew-reynolds]

What is the conclusion on this? As #80 winds down, I'd like to get messages for field state and entities into the repo in the next couple of days.

[wraftus]

I think having just the Generic Message is fine, makes our lives easier and really don't see what differences there will be between Cargo and Hatch messages. I agree that having the decay function in the msg is not needed, since I just see it as having a field_element_node or something that subscribes to hatch perception, cargo perception, etc. and does all the storing, decaying, and other stuff internally and only publishes the arrays msg.

For the shape, I don't really have a preference either way, if its not a lot of memory then I don't see why not, but I also don't see including it for the GenericEntity being too important. I see this msg used exclusively by planning to decide where to get the next game piece from, and where that is, so it won't be passed on to the controls part to intake it (which I'm assuming will need to know the dimensions and such)

[matthew-reynolds]

I actually think the controls code will require the entities as well, I think the planning will probably start up an action like "pickup cargo w/ entity ID 49" and then the action will subscribe to the entity array so that it can get up-to-date coordinates and information about the entity.

[wraftus]

Okay, that makes sense, if you think controls will want that then I'm all for it