DopplerVelocityLog sensor not publishing custom frame_id

[davidorchansky]

Environment

• OS Version: Ubuntu 22.04
• Source: branch gz-sensors8; commit #3275bb680a75

Description

DVL messages published by the DopplerVelocityLog sensor do not contain the custom frame_id specified in the <gz_frame_id> tag.

• Expected behavior: when the DVL sensor definition contains the tag <gz_frame_id>, the frame id defined in the tag is included in the header of the published message.
• Actual behavior: the header of the published message does not contain a frame_id.

Steps to reproduce

1. Add a DVL sensor similarly to the one defined in the bottomless_pit.sdf file:

        <sensor
            element_id="base_link" action="add"
            name="teledyne_pathfinder_dvl"
            type="custom" gz:type="dvl">
          <pose degrees="true">-0.60 0 -0.16 0 0 180</pose>
          <always_on>1</always_on>
          <update_rate>1</update_rate>
          <topic>/dvl/velocity</topic>
          <gz:dvl>
            <type>phased_array</type>
            <arrangement degrees="true">
              <beam id="1">
                <aperture>2</aperture>
                <rotation>45</rotation>
                <tilt>30</tilt>
              </beam>
              <beam>
                <aperture>2</aperture>
                <rotation>135</rotation>
                <tilt>30</tilt>
              </beam>
              <beam>
                <aperture>2</aperture>
                <rotation>-45</rotation>
                <tilt>30</tilt>
              </beam>
              <beam>
                <aperture>2</aperture>
                <rotation>-135</rotation>
                <tilt>30</tilt>
              </beam>
            </arrangement>
            <tracking>
              <bottom_mode>
                <when>best</when>
                <noise type="gaussian">
                  <!-- +/- 0.4 cm/s precision at 10 m/s within 2 stddevs -->
                  <stddev>0.002</stddev>
                </noise>
                <visualize>false</visualize>
              </bottom_mode>
            </tracking>
            <!-- Roughly 1 m resolution at a 100m -->
            <resolution>0.01</resolution>
            <maximum_range>100.</maximum_range>
            <minimum_range>0.1</minimum_range>
            <!-- ENU to SFM -->
            <reference_frame>0 0 0 0 0 -1.570796</reference_frame>
          </gz:dvl>
        </sensor>

2. Add a tag for the custom frame_id after the <topic> tag:
   <gz_frame_id>custom_dvl_frame</gz_frame_id>

3. Echo the /dvl/velocity topic

Output

header {
  stamp {
    sec: 7181
    nsec: 366000000
  }
  data {
    key: "seq"
    value: "50550"
  }
}
type: DVL_TYPE_PHASED_ARRAY
target {
  type: DVL_TARGET_BOTTOM
  range {
    mean: 27.256721496582028
  }
}
velocity {
  reference: DVL_REFERENCE_SHIP
  mean {
    x: 0.00076449104065136515
    y: 0.00075631276485152172
    z: 0.0017034900176310379
  }
  covariance: 7.9999999999999979e-06
  covariance: 8.8817841970012519e-22
  covariance: 2.220446049250313e-22
  covariance: 8.8817841970012519e-22
  covariance: 8.0000000000000081e-06
  covariance: 6.6613381477509394e-22
  covariance: 2.220446049250313e-22
  covariance: 6.6613381477509394e-22
  covariance: 1.3333333333333336e-06
}
beams {
  id: 1
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: 0.0010906611427130254
      y: 0.0010906604298682667
      z: 0.0026715624088749176
    }
    covariance: 5.0000032679489648e-07
    covariance: 4.9999999999989262e-07
    covariance: 1.2247452716318973e-06
    covariance: 4.9999999999989262e-07
    covariance: 4.9999967320510231e-07
    covariance: 1.2247444711511488e-06
    covariance: 1.2247452716318973e-06
    covariance: 1.2247444711511488e-06
    covariance: 3.0000000000000005e-06
  }
  range {
    mean: 27.408052444458004
  }
  locked: true
}
beams {
  id: 2
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: 0.00014363219642423718
      y: -0.00014363229030080549
      z: 0.00035182570684945185
    }
    covariance: 4.9999967320510348e-07
    covariance: -4.9999999999989346e-07
    covariance: 1.2247444711511498e-06
    covariance: -4.9999999999989336e-07
    covariance: 5.00000326794897e-07
    covariance: -1.2247452716318975e-06
    covariance: 1.2247444711511498e-06
    covariance: -1.2247452716318975e-06
    covariance: 2.9999999999999988e-06
  }
  range {
    mean: 27.256721496582028
  }
  locked: true
}
beams {
  id: 3
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: -0.00014158775239012079
      y: 0.00014158784493046071
      z: 0.0003468178605216507
    }
    covariance: 4.9999967320510327e-07
    covariance: -4.9999999999989262e-07
    covariance: -1.22474447115115e-06
    covariance: -4.9999999999989262e-07
    covariance: 5.0000032679489563e-07
    covariance: 1.224745271631896e-06
    covariance: -1.22474447115115e-06
    covariance: 1.224745271631896e-06
    covariance: 3.0000000000000005e-06
  }
  range {
    mean: 28.074352264404293
  }
  locked: true
}
beams {
  id: 4
  velocity {
    reference: DVL_REFERENCE_SHIP
    mean {
      x: -0.00071046006642145883
      y: -0.00071045960207216262
      z: 0.0017402640766470919
    }
    covariance: 5.0000032679489679e-07
    covariance: 4.9999999999989336e-07
    covariance: -1.2247452716318973e-06
    covariance: 4.9999999999989346e-07
    covariance: 4.9999967320510358e-07
    covariance: -1.22474447115115e-06
    covariance: -1.2247452716318973e-06
    covariance: -1.22474447115115e-06
    covariance: 2.9999999999999988e-06
  }
  range {
    mean: 27.917726516723633
  }
  locked: true
}

[iche033]

added support for frame to dvl sensor in #444. I'll backport to gz-sensors8 (harmonic) and make it compatible with <gz_frame_id> once that PR is merged.

[iche033]

backported fix in #446