Ability to add a "wait time" for each stop point

[mjansen4857]

Discussed in #129

^{Originally posted by MrRedness October 25, 2022}
This way, you could change how long the robot waits at a marker/stop point without having to change it in code and redeploy. I would be happy to work on this when I have a little bit more time.

[WarrenReynolds]

I think I'm missing something here with adding a "stop time" to marker. Aren't the markers designed to be used along a path (ie between waypoints when the robot is still moving). If a time field is going to be added to a marker, maybe "stop time" is not the best wording. I can definitely see the benefit of having a "time" field, For example open the intake at this marker while the robot is travelling along the path and keep it open for X second before closing it with X being a numerical value defined in the marker. Then you could make changes to the robot operation by only adjusting the path file, and not have to recompile and push the robot code.

Really what I'd like to to see the ability to be able to add key value pairs to a marker point or waypoint. Then the code that is processing the markers/ waypoints could do whatever it likes with what every info you supply in the key value pairs. Eg You could create a Key pair called CmdToExecute with a value of OpenIntakeForTime, and another keypair call Parameter1 with a value of 3. Then, when the trajectory follower gets to that marker it could check if a CmdToExecute value exists and if it does then execute it with parameters if they exist eg run the OpenIntakeForTime command with the value of 3. If it was setup this way it is infinitely extendable without needing changes to PathPlanner base code.

[mjansen4857]

Yeah you're right. I meant to change the title to be just stop points and not include markers but forgot.

I can see the value in what you're suggesting for the key/value pairs but I just don't really see how that would be possible to implement (within PPLib itself anyways as PPLib can't instantiate your commands because it has no knowledge that they exist). The only way I could see this working at the moment is if you loop through the event markers of a path and create all the commands using the CmdToExecute string and given parameters and then add those to the event map you pass to the path following commands. This seems to add a lot of complexity when you could basically do the same thing with a bit less flexibility if you for example, have two markers: intakeSlow and intakeFast then in your event map:

eventMap["intakeSlow"] = new SetIntake(0.3);
eventMap["intakeFast"] = new SetIntake(0.8);

vs something like this with those key value pairs:

for(EventMarker marker: trajectory.getMarkers()){
    if(marker.commandToRun.equals("SetIntake")){
        eventMap[marker.name] = new SetIntake(marker.params[0]);
    } 
    ... other commands ...
}

Did I understand what you were suggesting correctly? Definitely agree it sounds useful but also its not to difficult to get the same results already.

[WarrenReynolds]

Yeh, I must admin I hadn't thought through exactly how I would implement what I was talking about and I can see the problems you are talking about with the follower not having any knowledge of the commands that it needs to execute. Could you create an array of pointers to commands that you are going to use while following the trajectory in your RobotInit or AutoInit and then pass this object into a modified version of your PP follower code that take this as an optional extra argument?

Create the array as a map object in this format
map<datatype_of_keys, datatype_of_values>name_of_the_map
So start with this line, maybe somewhere in the RobotContainer class.
map<string,*command>m_ListOfCommands;
Then for each command you are wanting to use at the various markers add an entry to your map.
m_ListOfCommand["OpenIntake"]= new OpenIntake()); //Where OpenIntake() is the command
Then when you execute your trajectory, have a constructor on the trajectory follower that takes an optional extra argument so that you can pass this map of command pointers into the follower.

Now your follower has the ability find a command by name (by iterating through the dictionary of pointers to commands until it finds a matching name to the value pair of the CmdToExecute pair in the path) and then dereference this command pointer to add the command to the scheduler.

I admit this is not very nice code wise but it does give a solution to accessing commands by name without a heap of if statements inside the follower.
If the map of pointers to commands wasn't going to be used again after you have finished following your trajectory, you should probably delete all the instances of the heap allocated commands after finishing the trajectory to release the memory. Or you might keep this map if you ever want the ability to launch a command by name somewhere else in your code.

I just re read what you said above. I'm not sure what you were referring to when you are talking about an EventMap. Maybe we are on the same page already.

Much food for thought here.

[WarrenReynolds]

Probably a dam lot easier to just create a straight array of pointers to commands and then use the marker number to find the entry in the array of pointers as a straight array index lookup instead of having to look up text names of commands in dictionary objects.

Although if you were to do it this way and you wanted to open the intake at say marker 3 and marker 7, this would require to two instances of the openIntake() command to be stored in your array of command pointers. Actually, no it doesn't, you could just reuse the same pointer value at position 3 and 7 in the array of pointers.

In all honesty I really have no idea what the best way would be to do this, but I think this is how I'll implement it, if the functionality isn't there by the 2023 season. This could be implemented with only a very slight change to the path follower, and it doesn't need any changes to the trajectories markers as they stand at the moment to make all this work. PS: I'd love a copy gof your C++ version of the follower as it stands at the moment, so I can make our custom version.

NB: Could get a bit painfully if you insert a new marker onto your path, It means you would need to change the code where the pointers to commands are created so that all commands after the newly inserted command would need their position in the array moved down by one.

Maybe adding key,value pairs to markers of type string,int is all that is needed and then let people do what they want with the key values. If they don't exist, it doesn't matter, if they do exist the end user is responsible to know what the key name is and what to do with the value that is associated with it. This adds all the functionality we have been talking about and leaves the implementation of the information in these key value pairs up to the end user.

[mjansen4857]

By "event map" i mean the map from marker names to commands that is passed to the path following commands. I wanted to avoid arrays because if you change the order of markers or add/remove them you will need to edit code like you mentioned. My idea of how markers would be best used was to just create a "global" map of marker names to commands and pass this map to every path following command you run. So regardless of what path or auto you run "intakeDown" will always run the same thing. If you build up this map with all the commands you would run in an auto you can freely edit the markers for a path and not need to touch any code to add/remove/change commands in an auto (the ones using markers at least).

[WarrenReynolds]

Sounds perfect.