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.
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>
.
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
andexceed_boundary_1
inevents.yml
. A time penalty of 300 is also added to thetotal_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
andfinished
. The 2ndexceed_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.
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.
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 totrue
. -
Edit simple_demo.sdf world file and
a. set wind linear velocity to zero by changing the
<linear_velocity>
parameter to0 0 0
(search for<linear_velocity>
).b. set wave gain to zero by changing the
<gain>
parameter to0.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
andevents.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 astream_start_request
event logged inevents.yml
. -
Test reporting incorrect target vessel. You should see a
vessel_id_failure
string published to the ros2 topic, and avessel_id_failure
event in events.yml log. A time penalty should also be added toscore.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 inevents.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 inevents.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
, andlarge_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 afinished
event and thescore.yml
will show a very large value.
-
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 outdata: 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
andscore.yml
logstail -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 inevents.yml
and the score inscore.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 inevents.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 inevents.yml
and the score inscore.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 afinished
event logged inevents.yml
. Simulation should now be paused to indicate the end of the run.