Merge branch 'ign-sensors6' into merge67

.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:/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)
 {