Update Follow-the-path scoring

[caguero]

Fixes #623 .

This patch updates the Follow-the-path task according to the discussion in issue #623 .

How to test it?

Launch the teleoperation node:

ros2 launch vrx_gz usv_joy_teleop.py

Launch a practice world. E.g.:

ros2 launch vrx_gz competition.launch.py world:=practice_2022_follow_path2_task

Verify the following things:

• Make sure that if you skip the first gate, nothing happens (no gates crossed or invalid reported on the console).
• Cross the starting gate (+10 points).
• Make sure that if you cross another gate you get 10 points + a bonus. The bonus is declared as a parameter in the SDF.
• Make sure that if you skip a gate, you'll get 10 points for crossing the next gate and no bonus.
• Make sure that if you invalidate a gate (crossing it backwards), it no longer produces points even if you later cross it properly.
• Make sure that when you cross the last gate, the task terminates.
• Make sure that if you hit an obstacle, you get a penalty of -3 points.

Some notes:

• I bumped the teleoperation speed a bit.
• I left a few gzdbg in the plugin to help us debug the bonuses, penalties, etc.

[M1chaelM]

• Make sure that if you invalidate a gate (crossing it backwards), it no longer produces points even if you later cross it properly.

@caguero In the documentation it says going backwards across a gate will result in a deduction of 5 points. "Invalidating" the gate is an interesting alternative. In this case, what happens if you go through a gate correctly and then go through it backward?

[caguero]

• Make sure that if you invalidate a gate (crossing it backwards), it no longer produces points even if you later cross it properly.

@caguero In the documentation it says going backwards across a gate will result in a deduction of 5 points. "Invalidating" the gate is an interesting alternative. In this case, what happens if you go through a gate correctly and then go through it backward?

Oh, I see. We can add the penalty as well. Currently, it doesn´t grant you any points and reset the bonus for crossing consecutive gates.

[M1chaelM]

Here is the output I got from running through world 1, crossing all 6 gates in order and hitting one obstacle:

[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:509] New gate crossed!
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:539] Score: 10
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:509] New gate crossed!
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 0
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:532] New bonus: 1
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:539] Score: 21
[ruby $(which gz) sim-1] [Dbg] [ScoringPlugin.cc:631] [1] New collision counted between [wamv::wamv/base_link::wamv/base_link_fixed_joint_lump__right_float_collision_12] and [short_navigation_course_1::red_bound_1::link::collision_outer]
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:472] New penalty. score: 18
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:509] New gate crossed!
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 1
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:532] New bonus: 2
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:539] Score: 30
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:509] New gate crossed!
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 2
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:532] New bonus: 4
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:539] Score: 44
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:509] New gate crossed!
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 3
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:532] New bonus: 7
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:539] Score: 61
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:509] New gate crossed!
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 4
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:532] New bonus: 11
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:539] Score: 82
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:545] Course completed!

It seems like the number of consecutive gates crossed is off by 2.

[M1chaelM]

Oh, I see. We can add the penalty as well. Currently, it doesn´t grant you any points and reset the bonus for crossing consecutive gates.

OK, if we are going to deduct a penalty I'm not sure whether we should also invalidate the gate. Not that it's unreasonable, but it just seems like a lot of rules. What do you think? Do we need it?

[caguero]

OK, if we are going to deduct a penalty I'm not sure whether we should also invalidate the gate. Not that it's unreasonable, but it just seems like a lot of rules. What do you think? Do we need it?

The main reason is to avoid a situation where you cross a gate backwards, and then forward getting 5 points. It seems wrong not to approach it the way it's supposed to be.

[caguero]

It seems like the number of consecutive gates crossed is off by 2.

I just tweaked the message. It should now be:

[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 1
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 2
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 3
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 4
[ruby $(which gz) sim-1] [Dbg] [NavigationScoringPlugin.cc:519] Num consecutive gates: 5

Note that in a 6-gate course, we should have a maximum of 5 consecutive gates.

[caguero]

OK, if we are going to deduct a penalty I'm not sure whether we should also invalidate the gate. Not that it's unreasonable, but it just seems like a lot of rules. What do you think? Do we need it?

The main reason is to avoid a situation where you cross a gate backwards, and then forward getting 5 points. It seems wrong not to approach it the way it's supposed to be.

Maybe a penalty of 10 points so the overall effect is 0 when you cross it forward in the right direction?

[M1chaelM]

Note that in a 6-gate course, we should have a maximum of 5 consecutive gates.

I think the standard meaning of "you passed through 5 consecutive gates" is that there were 5 gates total, rather than 6. For example, if you win a contest for 3 consecutive years, it means that you won it 3 times and the years came right after another. Usually no one says "1 consecutive ____," so the numbering should start at 2.

[M1chaelM]

Maybe a penalty of 10 points so the overall effect is 0 when you cross it forward in the right direction?

Ah, it's so tricky because I'm thinking about a time that crosses correctly first and then is blown back through the gate. For them, 10 seems too much. This in convincing me that the way you did it originally is probably better.

How about we keep it the way you did it and I'll just update the documentation.

To make sure I understand correctly, this would mean that we skip the point penalty entirely. If you go through a gate in the wrong direction it becomes invalidated. If you've already crossed it correctly, you get to keep your points but your consecutive gate count goes back to 0.

[caguero]

Note that in a 6-gate course, we should have a maximum of 5 consecutive gates.

I think the standard meaning of "you passed through 5 consecutive gates" is that there were 5 gates total, rather than 6. For example, if you win a contest for 3 consecutive years, it means that you won it 3 times and the years came right after another. Usually no one says "1 consecutive ____," so the numbering should start at 2.

That makes sense, thanks for the clarification.

[caguero]

To make sure I understand correctly, this would mean that we skip the point penalty entirely. If you go through a gate in the wrong direction it becomes invalidated. If you've already crossed it correctly, you get to keep your points but your consecutive gate count goes back to 0.

I just updated the code and here are some notes that hopefully will clarify the current score.

1. You can only cross one gate one time. If you cross it properly, you get your 10 points, otherwise 0.
2. After crossing a gate (properly or in the wrong direction), the gate "disappears" for scoring purposes.
3. An invalid gate cross will reset the consecutive gates count. You'll need two new consecutive gates crossed to start getting a bonus.

As a consequence of (1) and (2):

• Crossing the first gate backwards will enable the course.
• Crossing the last gate backwards will terminate the task.

And answering your questions:

• "If you go through a gate in the wrong direction it becomes invalidated.": Yes, although maybe it's easier to not introduce the term invalid gate anymore. All gates can only be crossed one time. In this case, you won't get any points and you'll reset your bonus.
• " If you've already crossed it correctly, you get to keep your points but your consecutive gate count goes back to 0.": Not in this new approach. If you've already crossed correctly, nothing will happen if you cross it again backwards. Similarly, if you cross a gate backwards (wrong) and then, forward (correct), you won't get any extra points and your bonus won't change at all.

[M1chaelM]

OK, making the gates "disappear" seems fine. Thanks for explaining.

• Crossing the first gate backwards will enable the course.
• Crossing the last gate backwards will terminate the task.

This doesn't seem like the behavior we want. Can we make an exception for the first gate? I think the course should not be enabled until the first gate is crossed in the correct direction. I'm on the fence about what should happen if you cross the last gate backward, but maybe it's better to terminate the run than to say it's no longer possible to complete the run.

[caguero]

This doesn't seem like the behavior we want. Can we make an exception for the first gate? I think the course should not be enabled until the first gate is crossed in the correct direction. I'm on the fence about what should happen if you cross the last gate backward, but maybe it's better to terminate the run than to say it's no longer possible to complete the run.

But if we make an exception with the first gate, you could approach it backwards and then, move forward a bit "enabling" the course. Is that something that we want?

[M1chaelM]

But if we make an exception with the first gate, you could approach it backwards and then, move forward a bit "enabling" the course. Is that something that we want?

Let's discuss but I guess the short answer is I think it's OK. I think it's an unlikely case either way, given the courses we tend to build.

[M1chaelM]

@caguero Since this comment is long I added a summary of what I'm actually proposing to the top. If this works for you let me know and I'll update the documentation.

Proposed Scoring

• 10 points for each gate
• 1 bonus point for correctly traversing a gate if the previous gate was also traversed.
• 1 bonus point for completing the course (passing through the last gate)
• 3 point deduction for each obstacle.
• Break ties by time.

Explanation

Per our discussion yesterday I calculated the outcomes of all possible paths through a 9-gate course using the "sum of runs" bonus. This method assigns a bonus equal to the sum of the lengths of all "runs", where a run is defined as any group of 2 or more consecutive gates the team traversed correctly.

The idea behind this metric is to reward staying on course. It was proposed to eliminate some of the arbitrariness of the "longest run bonus," which had the same goal but artificially increases the cost of missing a gate in the middle of the run.

Unfortunately, the "sum of runs" approach also produces a similar effect around the edges of the course. In particular, missing the second gate or the second to last gate is worse than missing the 3rd or 3rd to last gate because there are not enough gates on either side to form a run. E.g a team that misses just gate 2

1-3456789

would score 87 (80+7 bonus), but a team that misses just gate 3

12-456789

would score 88 (80+2 bonus + 6 bonus).

Looking at this helped to clarify for me that what we really want is to discourage teams from going off course and back on many times. I think the best way to do this is the consecutive pair bonus you originally proposed, in which a team gets a bonus for passing through a gate if they also passed through the previous gate. This only leaves one edge case: if the team misses only one gate they will do better if it's the last gate, since there is no bonus to lose. This means that

12345678-

would score 87 (80+7 bonus), but a team that misses just gate 3 (or any gate but the last one)

12-456789

would score 86 (80 + 1 + 5). This one case can be addressed by giving a bonus point for going through the last gate. I've persuaded myself that this scoring does a reasonably good job of capturing what we're trying to incentive by calculating it for all possible paths and looking through the results to check qualitatively that it rewards the teams that do a better job of staying on course.

[caguero]

@caguero Since this comment is long I added a summary of what I'm actually proposing to the top. If this works for you let me know and I'll update the documentation.

Proposed Scoring

• 10 points for each gate
• 1 bonus point for correctly traversing a gate if the previous gate was also traversed.
• 1 bonus point for completing the course (passing through the last gate)
• 3 point deduction for each obstacle.
• Break ties by time.

Thanks for the detailed explanation and the exercise figuring out the right scoring scheme. I just updated the code with the new strategy, looks good to me. 5d64919

[M1chaelM]

Everything is looking good except one rule we had in the documentation, which is that you can't skip a gate, pass through the next one, and then go back and get the gate you skipped. What we say in the documentation is:

A team may miss one or more gates and still receive points for traversing gates that come later in the course; however, once a gate is crossed, no further points will be awarded for crossing gates that come before it.

I think we should enforce this rule whenever a gate is crossed correctly OR incorrectly. What do you think?

[caguero]

Everything is looking good except one rule we had in the documentation, which is that you can't skip a gate, pass through the next one, and then go back and get the gate you skipped. What we say in the documentation is:

A team may miss one or more gates and still receive points for traversing gates that come later in the course; however, once a gate is crossed, no further points will be awarded for crossing gates that come before it.

I think we should enforce this rule whenever a gate is crossed correctly OR incorrectly. What do you think?

Just to be clear, no penalty either right?

I'm not 100% sure about it. What's the spirit of that rule? It gives the option to the teams to recover, which looks positive to me.

[caguero]

0ea40ed

As discussed offline, I've disabled the gates not crossed right before an already crossed gate.