Competition Tasks and Time Penalties - osrf/mbzirc GitHub Wiki

This page is intended for testing and development purposes.

This tutorial shows how to manually trigger time penalties in simulation. It involves using Ignition transport topics and services, as well as inspecting simulation logs (events.yml and score.yml), all of which will not be available in final evaluation. You should only use ROS2 topics for controlling robots and getting data and competition feedback in your code.

Boundary Penalties

Geofence

The entire competition area is geofenced. If at any time a vehicle, UAV or USV, moves outside the boundary of the competition area, a penalty is incurred. There is a buffer distance for the vehicle to move back by before another penalty is incurred. This is so that we do not immediately trigger another penalty, e.g. when the USV moves back and forth at the perimeter due to wave motion.

The Game Logic plugin provides parameters for users to specify the geofence area. For example:

      <geofence>
        <!-- center: x, y, z in world frame -->
        <center>0 0 10.96</center>
        <!-- size: x, y, z in world frame -->
        <size>3162.28 3162.28 221.92</size>
      </geofence>

The boundary for the coast world is defined in the coast.sdf world file. Search for <geofence>.

Testing geofence boundary

The simulator currently provides Ignition services for testing boundary time penalties. These services are for testing purpose only and will not be available during simulation evaluation.

To test:

launch coast world

ros2 launch mbzirc_ros competition_local.launch.py ign_args:="-v 4 -r coast.sdf"

Spawn the USV

ros2 launch mbzirc_ign spawn.launch.py name:=usv world:=coast model:=usv x:=-1462 y:=-16.5 z:=0.3 R:=0 P:=0 Y:=0

In a separate terminal, monitor the events.yml log file, e.g.
```
tail -f /tmp/ign/mbzirc/logs/events.yml
```
Move the USV out of bounds and see 2 events triggered: started and exceed_boundary_1 in events.yml. A time penalty of 300 is also added to the total_score (total score is elapsed sim time + penalty).
```
ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "usv", position: {y: 1600.0}'
```

Move the USV back within bounds, e.g.

ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "usv", position: {y: 1000.0}'

Move the USV out of bounds again and see 2 more events triggered: exceed_boundary_2 and finished. The 2nd exceed_boundary causes termination of the run.

ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "usv", position: {y: 1600.0}'

Note that if the USV or any UAVs continue to move beyond the geofence boundary (by ~25m), a 'kill switch' will be activated. The simulation will freeze the vehicle and you will not be able to control it any more.

Target Identification Penalties

Reporting targets

Targets are to be reported by streaming live video imagery to the operator. For more information, see the Identifying Targets section in the Competition API page.

Testing target reports

The Game Logic plugin provides parameters for users to specify the target vessel, target small object, and target large object. For example:

      <target>
        <vessel>Vessel A</vessel>
        <small_object>small_blue_box_a</small_object>
        <small_object>small_blue_box_a_duplicate</small_object>
        <large_object>large_dry_box_a</large_object>
        <large_object>large_dry_box_a_duplicate</large_object>
      </target>

The targets are defined in the world file. For this section, we will use the simple_demo.sdf world. There 2 small objects and 2 large objects. You will only need to successfully identify one of each.

The instructions below involves positioning a UAV above the targets before report them. To prevent the quadrotor from falling and the target vessel from moving, you will need to first make the mbzirc_quadrotor static and turn off wind and wave effect:

Edit mbzirc_quadrotor/model.sdf.erb file and set the model to be static by changing the <static>false</static> parameter to true.
Edit simple_demo.sdf world file and

a. set wind linear velocity to zero by changing the <linear_velocity> parameter to 0 0 0 (search for <linear_velocity>).

b. set wave gain to zero by changing the <gain> parameter to 0.0 (search for <gain>).

Build and install these changes

colcon build --merge-install --packages-select mbzirc_ign

Here are steps to test target reporting:

launch simple demo world

ros2 launch mbzirc_ros competition_local.launch.py ign_args:="-v 4 -r simple_demo.sdf"

Spawn a quadrotor with downward looking camera:

# slot3 is downward looking camera
ros2 launch mbzirc_ign spawn.launch.py name:=quadrotor_1 world:=simple_demo model:=mbzirc_quadrotor type:=uav x:=0 y:=0 z:=1.2 R:=0 P:=0 Y:=0 slot3:=mbzirc_hd_camera

The UAV should not drop to the ground because it is static.

Move the quadrotor above vessel:

ign service -s /world/simple_demo/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "quadrotor_1", position: {x: 25.0, y:25, z:80}'

Open the simulator's Image Display plugin by going to top right 3 dots menu button and search for Image Display to open it. You should see the image from the UAV camera on the bottom right corner of the window. The target vessel should be in the center of the image.
Before identifying the target, listen to the ros2 stream status topic.
```
ros2 topic echo /mbzirc/target/stream/status
```

You can also monitor the score.yml and events.yml log files:

# Open a separate terminal and monitor score.yml file
watch -n 1 -d cat /tmp/ign/mbzirc/logs/score.yml

# Open another separate terminal and monitor events.yml log file:
tail -f /tmp/ign/mbzirc/logs/events.yml

In a separate terminal, stream the video from the quadrotor to the /quadrotor_1/mbzirc/target/stream/start ros2 topic:
```
ros2 run image_transport republish raw --ros-args --remap in:=/quadrotor_1/slot3/optical/image_raw --remap out:=/quadrotor_1/mbzirc/target/stream/start
```
You should see a stream_started string published to the ros2 stream status topic. You will also see a stream_start_request event logged in events.yml.
Test reporting incorrect target vessel. You should see a vessel_id_failure string published to the ros2 topic, and a vessel_id_failure event in events.yml log. A time penalty should also be added to score.yml (score is current time + time penalty so you should see an increase of 180s in score)
```
ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["vessel", "0", "0"]}'
```
Report the correct target vessel. You should see a vessel_id_success string in the ros2 topic and the in events.yml log. Note that the 2D position must be specified with respect to the original resolution of the camera and not the resolution of the image stream (if the stream has been compressed or reduced in size).
```
ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["vessel", "640", "480"]}'
```
You will need to source your mbzirc workspace for the above command otherwise you may see a The passed message type is invalid message.

Once the target vessel is identified, you will be able to move onto identifying the small target object. Move the quadrotor over the small target object:

# set low z pos so it can see the obj without hitting the mainsail part of the boat
ign service -s /world/simple_demo/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "quadrotor_1", position: {x: 24.4, y:26.25, z:1.5}'

The command below incorrect reports the small target object by specifying the wrong image position. An small_object_id_failure_1 event should be triggered and published to the ro2 topic, logged in events.yml, and the score will increase by 240s.
```
ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["small", "10", "10"]}'
```

Now report the correct small target object and see an small_object_id_success event triggered.

ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["small", "640", "480"]}'

Move the quadrotor above the large target object to report it.

# set low z pos so it can see the obj without hitting the mainsail part of the boat
ign service -s /world/simple_demo/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "quadrotor_1", position: {x: 25.2, y:24.2, z:1.5}'

You can either report its correct position to finish the run or incorrectly report it 3 times to see large_object_id_failure_1, large_object_id_failure_2, and large_object_id_failure_3 events logged.
```
# correct report
ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["large", "640", "480"]}'
```
```
# incorrect report
ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["large", "0", "0"]}'
```
If you made 3 incorrect reports, the run should terminate and simulation should now be paused. The events.yml should also show a finished event and the score.yml will show a very large value.

Target Retrieval Penalties

Testing target retrieval

launch coast world:

ros2 launch mbzirc_ros competition_local.launch.py ign_args:="-v 4 -r coast.sdf"

spawn the USV

ros2 launch mbzirc_ign spawn.launch.py name:=usv world:=coast model:=usv x:=-1462 y:=-16.5 z:=0.3 R:=0 P:=0 Y:=0

For testing, we can skip target identification and go straight to the end of the target identification phase:
```
ign service -s /mbzirc/skip_to_phase --reqtype ignition.msgs.StringMsg --reptype ignition.msgs.Boolean --timeout 300 --req 'data: "large_object_id_success"'
```
You can confirm this by checking the current phase using ros2 topic echo /mbzirc/phase and it should print out data: large_object_id_success.
We will also need to detach all the objects on the target vessel.
```
ign topic -t "/Vessel_B/detach" -m ignition.msgs.Empty -p "unused: true"
```
Note that in a normal run, this happens automatically when the vessel is identified.

Open separate terminals and monitor events.yml and score.yml logs

tail -f  /tmp/ign/mbzirc/logs/events.yml

watch -n 1 -d cat /tmp/ign/mbzirc/logs/score.yml

Test unsuccessful small object retrieval. Drop small_box_a into ocean:
```
ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "small_case_b", position: {x:-1462, y:-10, z: 2.0}'
```
Wait a few secs and you should see a small_object_retrieve_failure_1 event logged in events.yml and the score in score.yml jumps by 120s
Test successful small object retrieval. Drop small_case_b_duplicate onto USV:
```
ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "small_case_b_duplicate", position: {x:-1462, y:-17.85, z: 2.0}'
```
You should see a small_object_retrieve_success event logged in events.yml and the score continues to increment by 1s at a time.
Test unsuccessful large object retrieval. Drop large_grey_box_b into ocean:
```
ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "large_grey_box_b", position: {x:-1461, y:-10, z: 2.0}'
```
Wait a few secs and you should see a large_object_retrieve_failure_1 event logged in events.yml and the score in score.yml jumps by 120s
Test successful large object retrieval. Drop large_grey_box_b_duplicate onto USV:
```
ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "large_grey_box_b_duplicate", position: {x:-1462, y:-16.5, z: 2.0}'
```
You should see a large_object_retrieve_success event and a finished event logged in events.yml. Simulation should now be paused to indicate the end of the run.