Integrate target video confirmation with comms

[iche033]

Depends on:

• Adds new RFComms system gazebosim/gz-sim#1428
• ros_ign 's ros2 branch - needs the ros_ign_interfaces::msg::StringVec msg type Update: ros_ign'sgalactic branch

Competition rule: Teams need to stream video to base station / viewing gallery for operators to confirm targets.

This PR provides 2 ROS2 topics:

• /<robot_name>/mbzirc/target/stream/start - Teams will need to publish image stream to this topic before reporting targets
• /<robot_name>/mbzirc/target/stream/report - API to report vessel, small and large target objects

These 2 topics will relay the data to a new BaseStation plugin over comms, which will in turn forward the data to the GameLogicPlugin.

Main changes:

• Added a video_target_relay relay node for providing the 2 ROS2 topics mentioned above
• Added BaseStation plugin with comms endpoint.
• Added mbzirc_ros/examples/video_stream_publisher.cc - an example to show how to stream image to base station
• Renamed competition.launch to competition_local.launch - the score, clock, etc bridges in this launch file should really be created for each vehicle. For local testing, 1 bridge is enough so renamed to have the _local suffix.
• Added CommsEndPoint to quadrotor only. Other endpoints are added in Integrate comms #82

The testing procedure is similar to #59 except we'll now use ros2 topics instead of ign service calls.

To test, I find it easiest to make the UAV and vessel static first, e.g.

patch

diff --git a/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb b/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb
index c016f5e..2efecff 100644
--- a/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb
+++ b/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb
@@ -75,7 +75,7 @@ end
 <?xml version="1.0"?>
 <sdf version='1.9'>
   <model name="<%= $model_name%>">
-    <static>false</static>
+    <static>true</static>
 
     <!-- Platform base model-->
     <include merge="true">
diff --git a/mbzirc_ign/worlds/simple_demo.sdf b/mbzirc_ign/worlds/simple_demo.sdf
index 3ea4f3d..c48c246 100644
--- a/mbzirc_ign/worlds/simple_demo.sdf
+++ b/mbzirc_ign/worlds/simple_demo.sdf
@@ -200,6 +200,7 @@
     </include>
 
     <include>
+      <static>true</static>
       <name>Vessel A</name>
       <pose>25 25 0.3 0 0.0 -1.0</pose>
       <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Vessel A</uri>

Make sure to build and install the workspace.

1. Launch simple demo world:

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

2. spawn 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

3. 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}'

4. In a separate terminal, stream video to the /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

   Open /tmp/ign/mbzirc/logs/events.yml and see a stream_start_request event logged

5. Publish a message to the /mbzirc/target/stream/report ros2 topic report target vessel in stream

   # 640, 480 is center of image
   ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["vessel", "640", "480"]}'

6. Check /tmp/ign/mbzirc/logs/events.yml to see that the vessel is successfully reported. You should see an event with data: vessel_id_success

7. Repeat the process for small and large target objects

   # 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.5, y:26.2, z:2}'
   
   # report small target object
   ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["small", "640", "480"]}'
   
   # move UAV above large target object
   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.57, y:24.54, z:2}'
   
   # report large target object
   ros2 topic pub --once /quadrotor_1/mbzirc/target/stream/report ros_ign_interfaces/msg/StringVec '{data: ["large", "640", "480"]}'

8. You can also try reporting wrong image pos and you should see *_object_id_failure_* events logged. Simulation stops after 3 invalid attempts.

[caguero]

Works for me as expected!