Merge 6 -> main

.github/workflows/ci.yml

@@ -16,3 +16,12 @@ jobs:
           codecov-enabled: true
           cppcheck-enabled: true
           cpplint-enabled: true
+  jammy-ci:
+    runs-on: ubuntu-latest
+    name: Ubuntu Jammy CI
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v2
+      - name: Compile and test
+        id: ci
+        uses: ignition-tooling/action-ignition-ci@jammy

include/ignition/sensors/ImuSensor.hh

@@ -33,9 +33,30 @@ namespace ignition
 {
   namespace sensors
   {
-    // Inline bracket to help doxygen filtering.
+    /// Inline bracket to help doxygen filtering.
     inline namespace IGNITION_SENSORS_VERSION_NAMESPACE {
-    //
+
+    /// \brief Reference frames enum
+    enum class IGNITION_SENSORS_VISIBLE WorldFrameEnumType
+    {
+      /// \brief NONE : To be used only when <localization>
+      /// reference orientation tag is empty.
+      NONE = 0,
+
+      /// \brief ENU (East-North-Up): x - East, y - North, z - Up.
+      ENU = 1,
+
+      /// \brief NED (North-East-Down): x - North, y - East, z - Down.
+      NED = 2,
+
+      /// \brief NWU (North-West-Up): x - North, y - West, z - Up.
+      NWU = 3,
+
+      /// \brief CUSTOM : The frame is described using custom_rpy tag.
+      CUSTOM = 4
+    };
+
+    ///
     /// \brief forward declarations
     class ImuSensorPrivate;
 
@@ -138,6 +159,13 @@ namespace ignition
       /// \return Gravity vectory in meters per second squared.
       public: math::Vector3d Gravity() const;
 
+      /// \brief Specify the rotation offset of the coordinates of the World
+      /// frame relative to a geo-referenced frame
+      /// \param[in] _rot rotation offset
+      /// \param[in] _relativeTo world frame orientation, ENU by default
+      public: void SetWorldFrameOrientation(
+        const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo);
+
       IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
       /// \brief Data pointer for private data
       /// \internal

include/ignition/sensors/Sensor.hh

@@ -149,6 +149,14 @@ namespace ignition
       /// \return Name of sensor.
       public: std::string Name() const;
 
+      /// \brief FrameId.
+      /// \return FrameId of sensor.
+      public: std::string FrameId() const;
+
+      /// \brief Set Frame ID of the sensor
+      /// \param[in] _frameId Frame ID of the sensor
+      public: void SetFrameId(const std::string &_frameId);
+
       /// \brief Get topic where sensor data is published.
       /// \return Topic sensor publishes data to
       public: std::string Topic() const;
@@ -206,6 +214,19 @@ namespace ignition
       public: void PublishMetrics(
         const std::chrono::duration<double> &_now);
 
+      /// \brief Get whether the sensor is enabled or not
+      /// \return True if the sensor is active, false otherwise.
+      /// \sa SetActive
+      public: bool IsActive() const;
+
+      /// \brief Enable or disable the sensor. Disabled sensors will not
+      /// generate or publish data unless Update is called with the
+      /// '_force' argument set to true.
+      /// \param[in] _active True to set the sensor to be active,
+      /// false to disable the sensor.
+      /// \sa IsActive
+      public: void SetActive(bool _active);
+
       IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
       /// \internal
       /// \brief Data pointer for private data

src/AirPressureSensor.cc

@@ -159,7 +159,7 @@ bool AirPressureSensor::Update(
   *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   // This block of code comes from RotorS:
   // https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp

src/AltimeterSensor.cc

@@ -146,7 +146,7 @@ bool AltimeterSensor::Update(const std::chrono::steady_clock::duration &_now)
   *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   // Apply altimeter vertical position noise
   if (this->dataPtr->noises.find(ALTIMETER_VERTICAL_POSITION_NOISE_METERS) !=

src/CMakeLists.txt

@@ -27,6 +27,7 @@ if (MSVC)
   # TODO(louise) Remove this once warnings are suppressed in ign-rendering
   set_source_files_properties(
       ${rendering_sources}
+      SegmentationCameraSensor.cc
     COMPILE_FLAGS "/wd4251"
   )
 endif()

src/CameraSensor.cc

@@ -400,7 +400,7 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
     *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
     auto frame = msg.mutable_header()->add_data();
     frame->set_key("frame_id");
-    frame->add_value(this->Name());
+    frame->add_value(this->FrameId());
     msg.set_data(data, this->dataPtr->camera->ImageMemorySize());
   }
 
@@ -606,7 +606,7 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf)
   // can populate it with arbitrary frames.
   auto infoFrame = this->dataPtr->infoMsg.mutable_header()->add_data();
   infoFrame->set_key("frame_id");
-  infoFrame->add_value(this->Name());
+  infoFrame->add_value(this->FrameId());
 
   this->dataPtr->infoMsg.set_width(width);
   this->dataPtr->infoMsg.set_height(height);

src/DepthCameraSensor.cc

@@ -545,7 +545,7 @@ bool DepthCameraSensor::Update(
   *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
   msg.set_data(this->dataPtr->depthBuffer,

src/ImuSensor.cc

@@ -71,6 +71,23 @@ class ignition::sensors::ImuSensorPrivate
   /// \brief Flag for if time has been initialized
   public: bool timeInitialized = false;
 
+  /// \brief Orientation of world frame relative to a specified frame
+  public: ignition::math::Quaterniond worldRelativeOrientation;
+
+  /// \brief Frame relative-to which the worldRelativeOrientation
+  //  is defined
+  public: WorldFrameEnumType worldFrameRelativeTo = WorldFrameEnumType::ENU;
+
+  /// \brief Frame relative-to which the sensor orientation is reported
+  public: WorldFrameEnumType sensorOrientationRelativeTo
+         = WorldFrameEnumType::NONE;
+
+  /// \brief Frame realtive to which custom_rpy is specified
+  public: std::string customRpyParentFrame;
+
+  /// \brief Quaternion for to store custom_rpy
+  public: ignition::math::Quaterniond customRpyQuaternion;
+
   /// \brief Previous update time step.
   public: std::chrono::steady_clock::duration prevStep
     {std::chrono::steady_clock::duration::zero()};
@@ -116,24 +133,6 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf)
     return false;
   }
 
-  // Set orientation reference frame
-  // TODO(adityapande-1995) : Add support for named frames like
-  // ENU using ign-gazebo
-  if (_sdf.ImuSensor()->Localization() == "CUSTOM")
-  {
-    if (_sdf.ImuSensor()->CustomRpyParentFrame() == "")
-    {
-      this->SetOrientationReference(ignition::math::Quaterniond(
-        _sdf.ImuSensor()->CustomRpy()));
-    }
-    else
-    {
-      ignwarn << "custom_rpy parent frame must be set to empty "
-                "string. Setting it to any other frame is not "
-                "supported yet." << std::endl;
-    }
-  }
-
   if (this->Topic().empty())
     this->SetTopic("/imu");
 
@@ -166,6 +165,26 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf)
     }
   }
 
+  std::string localization = _sdf.ImuSensor()->Localization();
+
+  if (localization == "ENU")
+  {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::ENU;
+  } else if (localization == "NED") {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NED;
+  } else if (localization == "NWU") {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NWU;
+  } else if (localization == "CUSTOM") {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::CUSTOM;
+  } else {
+    this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NONE;
+  }
+
+  this->dataPtr->customRpyParentFrame =
+      _sdf.ImuSensor()->CustomRpyParentFrame();
+  this->dataPtr->customRpyQuaternion = ignition::math::Quaterniond(
+      _sdf.ImuSensor()->CustomRpy());
+
   this->dataPtr->initialized = true;
   return true;
 }
@@ -233,7 +252,7 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now)
   msg.set_entity_name(this->Name());
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
-  frame->add_value(this->Name());
+  frame->add_value(this->FrameId());
 
   if (this->dataPtr->orientationEnabled)
   {
@@ -292,6 +311,75 @@ math::Pose3d ImuSensor::WorldPose() const
   return this->dataPtr->worldPose;
 }
 
+//////////////////////////////////////////////////
+void ImuSensor::SetWorldFrameOrientation(
+  const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo)
+{
+  this->dataPtr->worldRelativeOrientation = _rot;
+  this->dataPtr->worldFrameRelativeTo = _relativeTo;
+
+  // Set orientation reference frame if custom_rpy was supplied
+  if (this->dataPtr->sensorOrientationRelativeTo == WorldFrameEnumType::CUSTOM)
+  {
+    if (this->dataPtr->customRpyParentFrame == "")
+    {
+      this->SetOrientationReference(this->dataPtr->worldRelativeOrientation *
+        this->dataPtr->customRpyQuaternion);
+    }
+    else
+    {
+      ignwarn << "custom_rpy parent frame must be set to empty "
+                "string. Setting it to any other frame is not "
+                "supported yet." << std::endl;
+    }
+    return;
+  }
+
+  // Table to hold frame transformations
+  static const std::map<WorldFrameEnumType,
+    std::map<WorldFrameEnumType, ignition::math::Quaterniond>>
+      transformTable =
+    {
+      {WorldFrameEnumType::ENU,
+        {
+          {WorldFrameEnumType::ENU, ignition::math::Quaterniond(0, 0, 0)},
+          {WorldFrameEnumType::NED, ignition::math::Quaterniond(
+            IGN_PI, 0, IGN_PI/2)},
+          {WorldFrameEnumType::NWU, ignition::math::Quaterniond(
+            0, 0, IGN_PI/2)},
+        }
+      },
+      {WorldFrameEnumType::NED,
+        {
+          {WorldFrameEnumType::ENU, ignition::math::Quaterniond(
+            IGN_PI, 0, IGN_PI/2).Inverse()},
+          {WorldFrameEnumType::NED, ignition::math::Quaterniond(0, 0, 0)},
+          {WorldFrameEnumType::NWU, ignition::math::Quaterniond(
+            -IGN_PI, 0, 0)},
+        }
+      },
+      {WorldFrameEnumType::NWU,
+        {
+          {WorldFrameEnumType::ENU, ignition::math::Quaterniond(
+            0, 0, -IGN_PI/2)},
+          {WorldFrameEnumType::NED, ignition::math::Quaterniond(IGN_PI, 0, 0)},
+          {WorldFrameEnumType::NWU, ignition::math::Quaterniond(0, 0, 0)},
+        }
+      }
+    };
+
+  if (this->dataPtr->sensorOrientationRelativeTo != WorldFrameEnumType::NONE &&
+      this->dataPtr->sensorOrientationRelativeTo != WorldFrameEnumType::CUSTOM)
+  {
+    // A valid named localization tag is supplied in the sdf
+    auto tranformation =
+      transformTable.at(this->dataPtr->worldFrameRelativeTo).at
+      (this->dataPtr->sensorOrientationRelativeTo);
+    this->SetOrientationReference(this->dataPtr->worldRelativeOrientation *
+      tranformation);
+  }
+}
+
 //////////////////////////////////////////////////
 void ImuSensor::SetOrientationReference(const math::Quaterniond &_orient)
 {