Problems Implementing DVL sensor plugin standalone

[scole02]

Ubuntu 20.04, Ignition Garden (Source)

Hi I am attempting to add the DVL sensor to an ROV sdf. I've compiled the lrauv_ignition_plugins, added the plugin tag to my world file, and added the sensor tag to my model file.

However, after starting the simulation nothing is being published at my dvl topic ("/model/dvl"). And the topic does not appear when ign topic -l is ran at the command line.

Running the simulation with debug logs, only two logs are printed that refer to the DVL:

[Dbg] [SystemManager.cc:55] Loaded system [tethys::DopplerVelocityLogSystem] for entity [1]
[Dbg] [DopplerVelocityLogSystem.cc:250] Found custom sensor [hd_manual::base_link_rov::teledyne_pathfinder_dvl] of 'dvl' type!

I added debug print statements at the top of DopplerVelocityLog::Load(), DopplerVelocityLog::Update(), DopplerVelocityLog::PostUpdate() and it appears that this code is never reached.

Possible Problems Are there additional custom lrauv plugins that the DVL sensor relies on (Comms)?
Could errors in the rendering be preventing the sensor code form running?

Any help would be much appreciated!

[hidmic]

@scole02 did you fully configure the sensor like it is done in lrauv_description? Are you running off of the latest Gazebo?

[scole02]

I built gazebo garden from source and have all the dependencies for the lrauv_ignition_plugins package including gz-plugin2

Heres the model.sdf file where I add the sensor as was done in the tethys_equipped model. I have also tried adding the sensor in the original sdf file with identical results.

<?xml version="1.0" ?>
<sdf version="1.9">
  <model name="hd_manual">  
    <include merge="true">
      <uri>model://hd_manual</uri>
      <experimental:params>
        <sensor
            element_id="base_link_rov" action="add"
            name="teledyne_pathfinder_dvl"
            type="custom" ignition:type="dvl">
          <pose degrees="true">0 0 0 0 0 180</pose>
          <always_on>1</always_on>
          <update_rate>1</update_rate>
          <topic>/tethys/dvl/velocity</topic>
          <ignition:dvl>
            <arrangement degrees="true">
              <beam id="1">
                <aperture>2.1</aperture>
                <rotation>45</rotation>
                <tilt>30</tilt>
              </beam>
              <beam>
                <aperture>2.1</aperture>
                <rotation>135</rotation>
                <tilt>30</tilt>
              </beam>
              <beam>
                <aperture>2.1</aperture>
                <rotation>-45</rotation>
                <tilt>30</tilt>
              </beam>
              <beam>
                <aperture>2.1</aperture>
                <rotation>-135</rotation>
                <tilt>30</tilt>
              </beam>
              <resolution>0.01</resolution>
              <visualize>false</visualize>
            </arrangement>
            <visualize>false</visualize>
            <type>phased_array</type>
            <noise type="gaussian">
              <!-- +/- 0.5 cm/s precision within 2 stddevs -->
              <stddev>0.0025</stddev>
            </noise>
            <!-- Approximate maximum range @ ~14.4v -->
            <maximum_range>80.</maximum_range>
            <!-- ENU to SFM -->
            <reference_frame>0 0 0 0 0 -1.570796</reference_frame>
          </ignition:dvl>
        </sensor>
      </experimental:params>
    </include>
  </model>
</sdf>

Here's a screenshot of the gazebo simulator where you can see the sensor has been created as an entity and is visible, but no sensor topic has been assigned to it.

[hidmic]

@scole02 I'm unable to reproduce in main. I'll go ahead and guess you're perhaps missing the systems::Sensors plugin.

[scole02]

@hidmic yep that was part of the issue, after adding the ignition::gazebo::systems::Sensors plugin to my world file I am able to see dvl messages being published using the LRAUV_example_dvl_velocity executable. However I've now run into a strange problem where none or only a couple of the beams will have their data populated in the message based on the visual geometry/mesh used in my model.sdf file.

Using lrauv/lrauv_description/models/tethys_equipped/meshes/tethys.dae as the visual geometry/mesh for my model.sdf file outputs dvl messages that look identical to messages published in the example worlds.

header {
  stamp {
    sec: 1
  }
  data {
    key: "seq"
    value: "1"
  }
}
type: DVL_TYPE_PHASED_ARRAY
target {
  type: DVL_TARGET_BOTTOM
  range {
    mean: 0.15693104267120359
  }
}
velocity {
  reference: DVL_REFERENCE_SHIP
  mean {
    x: -0.00030841196502251478
    y: 132.49305975876962
    z: 85.20329779945682
  }
}
beams {
  id: 1
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: 42.649677945154117
      y: 42.649650069768995
      z: 104.46991451943163
    }
  }
  range {
    mean: 0.159532830119133
  }
  locked: true
}
beams {
  id: 2
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: 9.5263959546193231
      y: -9.5264021809765236
      z: 23.334816802231479
    }
  }
  range {
    mean: 0.1589713990688324
  }
  locked: true
}
beams {
  id: 3
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: -42.6497379972455
      y: 42.649765872688015
      z: 104.47012989695372
    }
  }
  range {
    mean: 0.15959903597831726
  }
  locked: true
}
beams {
  id: 4
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: -9.526490108510453
      y: -9.5264838820957873
      z: 23.335032179753568
    }
  }
  range {
    mean: 0.15693104267120359
  }
  locked: true
}

However when I use my own STL files the result varies based on the file and pose used.

Here is the published message from an STL file with one pose:

---
header {
  stamp {
    sec: 3
  }
  data {
    key: "seq"
    value: "3"
  }
}
type: DVL_TYPE_PHASED_ARRAY
target {
  type: DVL_TARGET_BOTTOM
  range {
    mean: 0.2790340781211853
  }
}
beams {
  id: 1
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: 47.161650983064796
      y: 47.161620158701105
      z: 115.5219425837623
    }
  }
  range {
    mean: 0.2790340781211853
  }
  locked: true
}
beams {
  id: 2
}
beams {
  id: 3
}
beams {
  id: 4
}

Here is the published message from the same STL file with a different pose

---
header {
  stamp {
    sec: 1
  }
  data {
    key: "seq"
    value: "1"
  }
}
type: DVL_TYPE_PHASED_ARRAY
target {
  type: DVL_TARGET_BOTTOM
  range {
    mean: 0.13974738121032715
  }
}
velocity {
  reference: DVL_REFERENCE_SHIP
  mean {
    x: -0.00030289177738040962
    y: 130.00154927075567
    z: 85.758454648436427
  }
}
beams {
  id: 1
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: 42.508221255626808
      y: 42.508193472696306
      z: 104.12341792261971
    }
  }
  range {
    mean: 0.14836804568767548
  }
  locked: true
}
beams {
  id: 2
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: 10.007816775996375
      y: -10.007823317005414
      z: 24.514052551525108
    }
  }
  range {
    mean: 0.48975145816802979
  }
  locked: true
}
beams {
  id: 3
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: -42.508279816573179
      y: 42.508307599560041
      z: 104.12362942112949
    }
  }
  range {
    mean: 0.13974738121032715
  }
  locked: true
}
beams {
  id: 4
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: -10.007909660938688
      y: -10.00790311987322
      z: 24.5142640500349
    }
  }
  range {
    mean: 0.48567205667495722
  }
  locked: true
}

And here is another published message from a different STL file, I tried multiple poses for this one and the output was always the same.

---
header {
  stamp {
    sec: 1
  }
  data {
    key: "seq"
    value: "1"
  }
}
type: DVL_TYPE_PHASED_ARRAY
beams {
  id: 1
}
beams {
  id: 2
}
beams {
  id: 3
}
beams {
  id: 4
}

I thought this might be a result of the physical geometry being enclosed by the visual geometry? So I tried increasing the volume of the box used as collision geometry, which had no effect. Is visual geometry of a model even exposed to the sensor plugin code? Possibly as an attribute of sdf::Sensor &_sdf ?

[hidmic]

Circling back. Sorry for the late reply @scole02. Collision geometries have no effect in DVL sensor measurements, only visual geometries. What maximum range did you specify? Beams won't report anything if the "reflecting" surface is beyond their range.

[hidmic]

@scole02 friendly ping