Add GPS sensor to sdf9 (#453)

Signed-off-by: Dre Westcook <dre.west@outlook.com> Signed-off-by: Dre Westcook <sir.dre.west@outlook.com> Signed-off-by: Ashton Larkin <ashton@openrobotics.org> Co-authored-by: Louise Poubel <louise@openrobotics.org> Co-authored-by: Ashton Larkin <ashton@openrobotics.org>
gazebosim · Aug 14, 2021 · b889d93 · b889d93
1 parent 42f8fad
commit b889d93
Show file tree

Hide file tree

Showing 13 changed files with 650 additions and 15 deletions.
diff --git a/.gitignore b/.gitignore
@@ -2,3 +2,4 @@
 build
 build_*
 *.*.sw?
+.vscode
diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt
@@ -29,6 +29,7 @@ set (headers
   Material.hh
   Mesh.hh
   Model.hh
+  NavSat.hh
   Noise.hh
   Param.hh
   parser.hh

diff --git a/include/sdf/NavSat.hh b/include/sdf/NavSat.hh
@@ -0,0 +1,164 @@
+/*
+ * Copyright 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef SDF_NAVSAT_HH_
+#define SDF_NAVSAT_HH_
+
+#include <sdf/Error.hh>
+#include <sdf/Element.hh>
+#include <sdf/Noise.hh>
+#include <sdf/sdf_config.h>
+
+#include <ignition/math/Angle.hh>
+
+namespace sdf
+{
+  // Inline bracket to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  class NavSatPrivate;
+
+  /// \brief NavSat contains information about a NavSat sensor.
+  /// This sensor can be attached to a link. The NavSat sensor can be defined
+  /// SDF XML by the "navsat" type.
+  ///
+  /// # Example SDF XML using navsat type:
+  ///
+  /// ~~~{.xml}
+  /// <sensor name="navsat_sensor" type="navsat">
+  ///   <pose>1 2 3 0 0 0</pose>
+  ///   <topic>/navsat</topic>
+  ///   <navsat>
+  ///     <position_sensing>
+  ///       <horizontal>
+  ///         <noise type="gaussian">
+  ///             <mean>0.98</mean>
+  ///             <stddev>0.76</stddev>
+  ///         </noise>
+  ///       </horizontal>
+  ///       <vertical>
+  ///         <noise type="gaussian">
+  ///             <mean>0.98</mean>
+  ///             <stddev>0.76</stddev>
+  ///         </noise>
+  ///       </vertical>
+  ///     </position_sensing>
+  ///     <velocity_sensing>
+  ///       <horizontal>
+  ///         <noise type="gaussian">
+  ///             <mean>0.98</mean>
+  ///             <stddev>0.76</stddev>
+  ///         </noise>
+  ///       </horizontal>
+  ///       <vertical>
+  ///         <noise type="gaussian">
+  ///             <mean>0.98</mean>
+  ///             <stddev>0.76</stddev>
+  ///         </noise>
+  ///       </vertical>
+  ///     </velocity_sensing>
+  ///   </navsat>
+  /// </sensor>
+  /// ~~~
+  class SDFORMAT_VISIBLE NavSat
+  {
+    /// \brief Default constructor
+    public: NavSat();
+
+    /// \brief Copy constructor
+    /// \param[in] _navsat NavSat to copy.
+    public: NavSat(const NavSat &_navsat);
+
+    /// \brief Move constructor
+    /// \param[in] _navsat NavSat to move.
+    public: NavSat(NavSat &&_navsat) noexcept;
+
+    /// \brief Destructor
+    public: ~NavSat();
+
+    /// \brief Assignment operator
+    /// \param[in] _navsat The navsat to set values from.
+    /// \return *this
+    public: NavSat &operator=(const NavSat &_navsat);
+
+    /// \brief Move assignment operator
+    /// \param[in] _navsat The navsat to set values from.
+    /// \return *this
+    public: NavSat &operator=(NavSat &&_navsat) noexcept;
+
+    /// \brief Load the navsat based on an element pointer. This is *not*
+    /// the usual entry point. Typical usage of the SDF DOM is through the Root
+    /// object.
+    /// \param[in] _sdf The SDF Element pointer
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error.
+    public: Errors Load(ElementPtr _sdf);
+
+    /// \brief Get a pointer to the SDF element that was used during
+    /// load.
+    /// \return SDF element pointer. The value will be nullptr if Load has
+    /// not been called.
+    public: sdf::ElementPtr Element() const;
+
+    /// \brief Set the noise values for the horizontal position sensor
+    /// \param[in] _noise Noise values to set to
+    public: void SetHorizontalPositionNoise(const Noise &_noise);
+
+    /// \brief Get noise value for horizontal position sensor
+    /// \return Noise values
+    public: const Noise &HorizontalPositionNoise() const;
+
+    /// \brief Set the noise values for the vertical position sensor
+    /// \param[in] _noise Noise values to set to
+    public: void SetVerticalPositionNoise(const Noise &_noise);
+
+    /// \brief Get noise value for vertical position sensor
+    /// \return Noise values
+    public: const Noise &VerticalPositionNoise() const;
+
+    /// \brief Set the noise values for the horizontal velocity sensor
+    /// \param[in] _noise Noise values to set to
+    public: void SetHorizontalVelocityNoise(const Noise &_noise);
+
+    /// \brief Get noise value for horizontal velocity sensor
+    /// \return Noise values
+    public: const Noise &HorizontalVelocityNoise() const;
+
+    /// \brief Set the noise values for the vertical velocity sensor
+    /// \param[in] _noise Noise values to set to
+    public: void SetVerticalVelocityNoise(const Noise &_noise);
+
+    /// \brief Get noise value for vertical velocity sensor
+    /// \return Noise values
+    public: const Noise &VerticalVelocityNoise() const;
+
+    /// \brief Return true if both NavSat objects contain the same values.
+    /// \param[_in] _navsat NavSat value to compare.
+    /// \return True if 'this' == _navsat.
+    public: bool operator==(const NavSat &_navsat) const;
+
+    /// \brief Return true this NavSat object does not contain the same
+    /// values as the passed in parameter.
+    /// \param[_in] _navsat NavSat value to compare.
+    /// \return True if 'this' != _navsat.
+    public: bool operator!=(const NavSat &_navsat) const;
+
+    /// \brief Private data pointer.
+    private: NavSatPrivate *dataPtr;
+  };
+  }
+}
+#endif
diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh
@@ -38,6 +38,7 @@ namespace sdf
   class Imu;
   class Lidar;
   class Magnetometer;
+  class NavSat;
   class SensorPrivate;
   struct PoseRelativeToGraph;
 
@@ -109,7 +110,10 @@ namespace sdf
     RGBD_CAMERA = 19,
 
     /// \brief A thermal camera sensor
-    THERMAL_CAMERA = 20
+    THERMAL_CAMERA = 20,
+
+    /// \brief A NavSat sensor, such as GPS.
+    NAVSAT = 21
   };
 
   /// \brief Information about an SDF sensor.
@@ -319,6 +323,17 @@ namespace sdf
     /// \sa SensorType Type() const
     public: const Camera *CameraSensor() const;
 
+    /// \brief Set the NAVSAT sensor.
+    /// \param[in] _navsat The NAVSAT sensor.
+    public: void SetNavSatSensor(const NavSat &_navsat);
+
+    /// \brief Get a pointer to a NAVSAT sensor, or nullptr if the sensor
+    /// does not contain an NAVSAT sensor.
+    /// \return Pointer to the sensor's NAVSAT, or nullptr if the sensor
+    /// is not an NAVSAT.
+    /// \sa SensorType Type() const
+    public: const NavSat *NavSatSensor() const;
+
     /// \brief Set the IMU sensor.
     /// \param[in] _imu The IMU sensor.
     public: void SetImuSensor(const Imu &_imu);

diff --git a/sdf/1.7/navsat.sdf b/sdf/1.7/navsat.sdf
@@ -0,0 +1,40 @@
+<element name="navsat" required="0">
+  <description>These elements are specific to the NAVSAT sensor.</description>
+
+  <element name="position_sensing" required="0">
+    <description>
+      Parameters related to NAVSAT position measurement.
+    </description>
+    <element name="horizontal" required="0">
+      <description>
+        Noise parameters for horizontal position measurement, in units of meters.
+      </description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+    <element name="vertical" required="0">
+      <description>
+        Noise parameters for vertical position measurement, in units of meters.
+      </description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+  </element>
+
+  <element name="velocity_sensing" required="0">
+    <description>
+      Parameters related to NAVSAT position measurement.
+    </description>
+    <element name="horizontal" required="0">
+      <description>
+        Noise parameters for horizontal velocity measurement, in units of meters/second.
+      </description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+    <element name="vertical" required="0">
+      <description>
+        Noise parameters for vertical velocity measurement, in units of meters/second.
+      </description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+  </element>
+
+</element>
diff --git a/sdf/1.7/sensor.sdf b/sdf/1.7/sensor.sdf
@@ -22,6 +22,7 @@
                   logical_camera,
                   magnetometer,
                   multicamera,
+                  navsat,
                   ray,
                   rfid,
                   rfidtag,
@@ -30,7 +31,7 @@
                   thermal_camera, thermal,
                   wireless_receiver, and
                   wireless_transmitter.
-      The "ray" and "gpu_ray" types are equivalent to "lidar" and "gpu_lidar", respectively. It is preferred to use "lidar" and "gpu_lidar" since "ray" and "gpu_ray" will be deprecated. The "ray" and "gpu_ray" types are maintained for legacy support.
+      The "ray", "gpu_ray", and "gps" types are equivalent to "lidar", "gpu_lidar", and "navsat", respectively. It is preferred to use "lidar", "gpu_lidar", and "navsat" since "ray", "gpu_ray", and "gps" will be deprecated. The "ray", "gpu_ray", and "gps" types are maintained for legacy support.
     </description>
   </attribute>
 
@@ -61,6 +62,7 @@
   <include filename="lidar.sdf" required="0"/>
   <include filename="logical_camera.sdf" required="0"/>
   <include filename="magnetometer.sdf" required="0"/>
+  <include filename="navsat.sdf" required="0"/>
   <include filename="ray.sdf" required="0"/>
   <include filename="rfid.sdf" required="0"/>
   <include filename="rfidtag.sdf" required="0"/>

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -43,6 +43,7 @@ set (sources
   Material.cc
   Mesh.cc
   Model.cc
+  NavSat.cc
   Noise.cc
   parser.cc
   parser_urdf.cc
@@ -122,6 +123,7 @@ if (BUILD_SDF_TEST)
     Material_TEST.cc
     Mesh_TEST.cc
     Model_TEST.cc
+    NavSat_TEST.cc
     Noise_TEST.cc
     parser_urdf_TEST.cc
     Param_TEST.cc