gymkhana scoring plugin

[M1chaelM]

To test

Launch the task:

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

In a separate terminal, launch the teleop:

ros2 launch vrx_gz usv_joy_teleop.py

In a third terminal, echo the /vrx/task/info topic:

ros2 topic echo /vrx/task/info

Test Navigation Channel

• Switch back to the first terminal.
• Wait for the task to enter onrunning state.
• Use the gamepad to drive the WAMV through the gates in the course.
• Check that a "New gate crossed!" message is printed to terminal each time a gate is crossed.
• Check that the navigation scoring plugin exits after all 3 gates are crossed, but not the simulation. You should see a message that says:

  [Err] [ScoringPlugin.cc:330] VRX_EXIT_ON_COMPLETION and <per_plugin_exit_on_completion> both not set, will not shutdown on ScoringPlugin::Exit()
  

• Check that the score is reasonable. If you went through relatively quickly, your score should be about 88 when you exit the channel.
• Check that the score matches the mean pose error.

ros2 topic echo /vrx/gymkhana_blackbox/mean_pose_error

It should give you the same value.

• Check that the pinger output is reasonable:

  ros2 topic echo /wamv/pingers/pinger/range_bearing
  

  If your wamv is at the end of the navigation channel, the range should be about 86.
• Check that the range approximately matches the pose error:

  ros2 topic echo /vrx/gymkhana_blackbox/pose_error
  

  It should also be about 86. If you wait long enough, all 4 of the above values will converge.
• Pause the simulation.

Test Stationkeeping plugin

With the simulation still paused, use the GUI to teleport the WAMV to the pinger position:

X = -485
Y= 295.5

• Unpause.
• Check /wamv/pingers/pinger/range_bearing and /vrx/gymkhana_blackbox/pose_error. The range and the pose error should be similar and near zero.
• Check /vrx/task/info and /vrx/gymkhana_blackbox/mean_pose_error. These should be the same and should be steadily decreasing.

Known Limitations

• WAMV is not locking properly. See issue WAMV is not locking before OnRunning #545
• Collisions don't seem to be detected yet. I will address this in a separate PR.

[M1chaelM]

Not currently being bridged:

/vrx/gymkhana_blackbox/goal
/vrx/gymkhana_blackbox/mean_pose_error
/vrx/gymkhana_blackbox/pose_error
/vrx/gymkhana_blackbox/task/info
/vrx/gymkhana_channel/task/info
/vrx/release

[M1chaelM]

@caguero Just an FYI, it appears that the bridges are currently hard-coded for specific topic strings (e.g. /vrx/stationkeeping/goal so if these are changed in the sdf file to something else (e.g. /vrx/gymkhana_blackbox/goal) the bridge will not be created. This means the topic names in bridges.py have to be manually edited to match whatever is in the sdf.

For now, I will add some bridges for the topic names we use in the gymkhana task. Eventually, it would be nice if there was a more automated way of getting the topic name that is used in the sdf and using that for the bridge.

[M1chaelM]

Draft TODO List

• The WAMV does not seem to be locked in the OnReady state.
  • Created issue WAMV is not locking before OnRunning #545 to address this.
• The ros bridges are not set up yet.
• The range_bearing topic is empty in ros and not showing up on the gz side.
  • Fixed with [caguero] Add pinger plugin to wamv_pinger.xacro #543
• set_pinger_position is in two places on the gz side: /wamv/pinger/pingers/ and /wamv/sensors/pingers/pinger/
• Some topics and possibly updates don't pause when the simulation is paused.
  • Added checks to the preupdate functions of the Gymkhana, Navigation, Stationkeeping and Wayfinding plugins so they will return if the simulation is paused. Anything else?

[caguero]

It works for me as expected. I only added a few very minor comments.

[caguero]

Could you document the _msg parameter?

[M1chaelM]

Done! I realized our taskMsg is not documented anywhere so I also added this to ScoringPlugin.cc

[M1chaelM]

I also realized this plugin itself does not seem to have ever really been documented, so I put a description in GymkhanaScoringPlugin.hh

[caguero]

Great! Thanks

[caguero]

Out of curiosity, what's the reason for checking this?

[M1chaelM]

I don't know... maybe it isn't necessary? (That's what my TODO is reminding me to ask about.) I was trying to port the code from our previous implementation in gymkhana_scoring_plugin.cc, which uses:

if (msg)

I assumed the intent was to check whether a null message had been passed into the callback, but the syntax produces an error in gazebosim, which will not automatically cast msg as a boolean, so I added IsInitialized() thinking this was the intent. As to why the callback would be receiving an empty message... I don't know. Seems like a workaround for a bug, maybe. Should I take it out?

[caguero]

I don't think this is necessary here. In the past maybe _msg was a pointer and we wanted to make sure it wasn't null but it's safe here.