Scoring plugin part 3 - Lock/Release the WAM-V

[caguero]

This pull request adds the ability to lock/release the WAM-V programmatically.

How to test it?

1. Apply the following patch to lock the vehicle during the first minute of the simulation (the initial state).

   patch

   diff --git a/vrx_ign/models/wam-v/model.sdf.erb b/vrx_ign/models/wam-v/model.sdf.erb
   index a232cd53..ef9bd1e2 100644
   --- a/vrx_ign/models/wam-v/model.sdf.erb
   +++ b/vrx_ign/models/wam-v/model.sdf.erb
   @@ -17,7 +17,7 @@ end
      max_velocity_knots = 15 # Maximum velocity in knots.
      max_velocity_mps = max_velocity_knots * 0.5144 # convert knots to m/s
      max_total_thrust = (x_u + x_uu * max_velocity_mps) * max_velocity_mps
   -  locked = false
   +  locked = true
    %>
   
    <?xml version="1.0"?>
   diff --git a/vrx_ign/worlds/sydney_regatta.sdf b/vrx_ign/worlds/sydney_regatta.sdf
   index d36afaf9..8facdf4f 100644
   --- a/vrx_ign/worlds/sydney_regatta.sdf
   +++ b/vrx_ign/worlds/sydney_regatta.sdf
   @@ -681,7 +681,7 @@
          <vehicle>wamv</vehicle>
          <task_name>testing_task</task_name>
          <task_info_topic>/vrx/task/info</task_info_topic>
   -      <initial_state_duration>20</initial_state_duration>
   +      <initial_state_duration>60</initial_state_duration>
          <ready_state_duration>10</ready_state_duration>
          <running_state_duration>300</running_state_duration>
          <release_topic>/vrx/release</release_topic>
   

2. Launch the simulation:

ros2 launch vrx_ros competition_local.launch.py ign_args:="-v 4 -r sydney_regatta.sdf"

3. Load the WAM-V:

ros2 launch vrx_ign spawn.launch.py name:=wamv world:=sydney_regatta model:=wam-v x:=-532 y:=162 z:=0 R:=0 P:=0 Y:=1

4. Launch the usv_joy_teleop launch file:

ros2 launch vrx_ign usv_joy_teleop.py

Try to teleop the WAM-V and verify that you can't move it. The WAM-V should still move up/down and roll/pitch following the waves. After the first minute (when transitioning to ready state) verify that you can teleoperate the robot. Restoring the locked argument to false in models/wam-v/model.sdf.erb should go back to the normal behavior where the WAM-V doesn't start locked to the world.

[j-herman]

All works as advertised. One note for future tests - the wamv is loaded automatically in this branch, so you don't need a separate launch - it took me a surprisingly long time to notice that I had two vehicles running :)