Support changing video streams when reporting targets

[iche033]

Signed-off-by: Ian Chen [email protected]

Fixes #193

Depends on gazebosim/gz-rendering#690

Currently target reporting only works with video stream from one camera. This PR adds support so that target report can be done from different cameras, e.g. verify vessel report from slot0 of quad_1 and then verify small object report from slot3 of quad_2.

To test:

1. Apply the following patch make the quadrotor static and move Vessel B closer to coast and make it static so it's easier to test target reporting:

   patch

   diff --git a/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb b/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb
   index a16a8de..0cde58b 100644
   --- a/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb
   +++ b/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb
   @@ -91,7 +91,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/coast.sdf b/mbzirc_ign/worlds/coast.sdf
   index c44a64d..b7dc7e9 100644
   --- a/mbzirc_ign/worlds/coast.sdf
   +++ b/mbzirc_ign/worlds/coast.sdf
   @@ -498,7 +498,7 @@
    
        <include>
          <name>Vessel B</name>
   -      <pose>0 0 0.4 0 0.0 0.0</pose>
   +      <pose>-1300 0 0.4 0 0.0 0.0</pose>
          <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Vessel B</uri>
          <plugin
            filename="libSurface.so"
   @@ -518,7 +518,7 @@
                <period>5</period>
                <number>3</number>
                <scale>1.5</scale>
   -            <gain>0.3</gain>
   +            <gain>0.0</gain>
                <direction>1 0</direction>
                <angle>0.4</angle>
                <tau>2.0</tau>
   @@ -548,6 +548,7 @@
            <nRR>0.0</nRR>
          </plugin>
    
   +      <!--
          <plugin
            filename="ignition-gazebo-trajectory-follower-system"
            name="ignition::gazebo::systems::TrajectoryFollower">
   @@ -557,15 +558,12 @@
              <waypoint>500 0</waypoint>
              <waypoint>700 300</waypoint>
            </waypoints>
   -        <!-- ~1 knot -->
   -        <!-- max_velocity_knots = 1 # Maximum velocity in knots -->
   -        <!-- max_velocity_mps = max_velocity_knots * 0.5144 # Knots to m/s -->
   -        <!-- (x_u + x_uu * max_velocity_mps) * max_velocity_mps -->
            <force>908</force>
            <torque>200</torque>
            <range_tolerance>5</range_tolerance>
            <bearing_tolerance>5</bearing_tolerance>
          </plugin>
   +      -->
        </include>
    
        <include>
   @@ -1401,7 +1399,7 @@
        </model>
    
        <wind>
   -      <linear_velocity>0.3 0 0</linear_velocity>
   +      <linear_velocity>0.0 0 0</linear_velocity>
        </wind>
    
        <!-- Load the plugin for the wind -->
                                              

2. build the workspace for changes to take place

   IGNITION_VERSION=fortress colcon build --merge-install
   

3. Launch the coast environment

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

4. Spawn 2 quadrotors. One with front facing camera (slot0) and one with downward facing camera (slot3)

   ros2 launch mbzirc_ign spawn.launch.py name:=quadrotor_1 world:=coast model:=mbzirc_quadrotor x:=-1500 y:=2 z:=4.3 R:=0 P:=0 Y:=0 slot0:=mbzirc_hd_camera
   ros2 launch mbzirc_ign spawn.launch.py name:=quadrotor_2 world:=coast model:=mbzirc_quadrotor x:=-1500 y:=-2 z:=4.3 R:=0 P:=0 Y:=0 slot3:=mbzirc_hd_camera
   

5. Position quadrotor_1 to look at vessel:

    ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "quadrotor_1", position: {x: -1310.0, y:0, z:1}'
   

6. In another terminal, start stream from quadrotor_1 using image_transport

   ros2 run image_transport republish raw --ros-args --remap in:=/quadrotor_1/slot0/optical/image_raw --remap out:=/quadrotor_1/mbzirc/target/stream/start
   

7. Report vessel

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

   Check tmp/ign/mbzirc/logs/events.yml and you should see vessel_id_success event

8. Kill the stream (image_transport process) for quadrotor_1 by pressing Ctrl+C.

9. Move quadrotor_2 above small target object

   ign service -s /world/coast/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 300 --req 'name: "quadrotor_2", position: {x: -1300.35, y:1.6, z:1.6}'
   

10. Start the stream from quadrotor_2

    ros2 run image_transport republish raw --ros-args --remap in:=/quadrotor_2/slot3/optical/image_raw --remap out:=/quadrotor_2/mbzirc/target/stream/start
    

11. Report small object

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

    Verify that in tmp/ign/mbzirc/logs/events.yml, you see a small_object_id_success event

[caguero]

I had to reload my vessels from Fuel but it worked as expected.