Add multichannel lookup for environment sensors.

src/systems/environmental_sensor_system/CMakeLists.txt

@@ -6,3 +6,10 @@ gz_add_system(environmental-sensor
     gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
     gz-math${GZ_MATH_VER}::eigen3
 )
+
+gz_build_tests(TYPE UNIT
+  SOURCES
+  TransformTypes_TEST.cc
+  LIB_DEPS
+  gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER}
+)

src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc

@@ -15,6 +15,7 @@
  *
 */
 #include "EnvironmentalSensorSystem.hh"
+#include "TransformTypes.hh"
 
 #include <gz/plugin/Register.hh>
 
@@ -27,6 +28,7 @@
 #include <gz/sim/components/ParentEntity.hh>
 #include <gz/sim/components/Sensor.hh>
 #include <gz/sim/components/SphericalCoordinates.hh>
+#include <gz/sim/components/LinearVelocity.hh>
 #include <gz/sim/Util.hh>
 
 #include <gz/sensors/SensorFactory.hh>
@@ -76,19 +78,89 @@ class EnvironmentalSensor : public gz::sensors::Sensor
     // Load common sensor params
     gz::sensors::Sensor::Load(_sdf);
 
-    this->pub = this->node.Advertise<gz::msgs::Double>(this->Topic());
+    // Handle the field type
+    if (_sdf.Element() != nullptr &&
+      _sdf.Element()->HasElement("output_format"))
+    {
+      std::string format = _sdf.Element()->Get<std::string>("output_format");
+      if (format == "scalar")
+      {
+        this->numberOfFields = 1;
+      }
+      else if (format == "vector3")
+      {
+        this->numberOfFields = 3;
+      }
+      else
+      {
+        gzerr << "Invalid output format " << format << "given."
+          << "valid options are scalar or vector3. "
+          << "Defaulting to 1." << std::endl;
+      }
+    }
+
+    // Handle the transform type
+    if (_sdf.Element() != nullptr &&
+      _sdf.Element()->HasElement("transform_type"))
+    {
+      auto res =
+        getTransformType(_sdf.Element()->Get<std::string>("transform_type"));
+
+      if (!res.has_value())
+      {
+        gzerr << "Invalid transform type of "
+          << _sdf.Element()->Get<std::string>("transform_type")
+          << " given. Valid types are ADD_VELOCITY_LOCAL, ADD_VELOCITY_GLOBAL,"
+          << " LOCAL, and GLOBAL. Defaulting to GLOBAL." << std::endl;
+      }
+      else
+      {
+        this->transformType = res.value();
+      }
+    }
+
+    switch(this->numberOfFields) {
+      case 1:
+        this->pub = this->node.Advertise<gz::msgs::Double>(this->Topic());
+        break;
+      case 3:
+        this->pub = this->node.Advertise<gz::msgs::Vector3d>(this->Topic());
+        break;
+      default:
+        gzerr << "Unable to create publisher too many fields" << "\n";
+    }
 
     // If "environment_variable" is defined then remap
     // sensor from existing data.
     if (_sdf.Element() != nullptr &&
+      this->numberOfFields == 1 &&
       _sdf.Element()->HasElement("environment_variable"))
     {
-      this->fieldName =
+      this->fieldName[0] =
         _sdf.Element()->Get<std::string>("environment_variable");
     }
+    else if (_sdf.Element() != nullptr &&
+      this->numberOfFields == 3)
+    {
+      if (_sdf.Element()->HasElement("environment_variable_x"))
+      {
+        this->fieldName[0] =
+          _sdf.Element()->Get<std::string>("environment_variable_x");
+      }
+      if (_sdf.Element()->HasElement("environment_variable_y"))
+      {
+        this->fieldName[1] =
+          _sdf.Element()->Get<std::string>("environment_variable_y");
+      }
+      if (_sdf.Element()->HasElement("environment_variable_z"))
+      {
+        this->fieldName[2] =
+          _sdf.Element()->Get<std::string>("environment_variable_z");
+      }
+    }
     else
     {
-      this->fieldName = type.substr(strlen(SENSOR_TYPE_PREFIX));
+      this->fieldName[0] = type.substr(strlen(SENSOR_TYPE_PREFIX));
     }
 
     // Allow setting custom frame_ids
@@ -98,8 +170,18 @@ class EnvironmentalSensor : public gz::sensors::Sensor
       this->frameId = _sdf.Element()->Get<std::string>("frame_id");
     }
 
-    gzdbg << "Loaded environmental sensor for " << this->fieldName
-      << " publishing on " << this->Topic() << std::endl;
+    if (this->numberOfFields == 1)
+    {
+      gzdbg << "Loaded environmental sensor for " << this->fieldName[0]
+        << " publishing on " << this->Topic() << std::endl;
+    }
+    else
+    {
+      gzdbg << "Loaded environmental sensor for [" << this->fieldName[0] << ", "
+        << this->fieldName[1] << ", " << this->fieldName[2]
+        << "] publishing on " << this->Topic() << std::endl;
+    }
+
 
     return true;
   }
@@ -113,30 +195,72 @@ class EnvironmentalSensor : public gz::sensors::Sensor
   {
     if (!this->ready) return false;
 
-    if (!this->session.has_value()) return false;
 
-    // Step time if its not static
-    if (!this->gridField->staticTime)
-      this->session = this->gridField->frame[this->fieldName].StepTo(
-        this->session.value(), std::chrono::duration<double>(_now).count());
+    std::optional<double> dataPoints[3];
+    for (std::size_t i = 0; i < this->numberOfFields; ++i)
+    {
+      if (this->fieldName[i] == "")
+      {
+        // Empty field name means the column should default to zero.
+        dataPoints[i] = 0;
+        continue;
+      }
+
+      if (!this->session[i].has_value()) return false;
+
+      // Step time if its not static
+      if (!this->gridField->staticTime)
+        this->session[i] = this->gridField->frame[this->fieldName[i]].StepTo(
+          this->session[i].value(),
+          std::chrono::duration<double>(_now).count());
+
+      if (!this->session[i].has_value()) return false;
 
-    if (!this->session.has_value()) return false;
+      dataPoints[i] = this->gridField->frame[this->fieldName[i]].LookUp(
+        this->session[i].value(), this->position);
+    }
 
-    gz::msgs::Double msg;
-    *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now);
-    auto frame = msg.mutable_header()->add_data();
-    frame->set_key("frame_id");
-    frame->add_value((this->frameId == "") ? this->Name() : this->frameId);
-    auto data = this->gridField->frame[this->fieldName].LookUp(
-      this->session.value(), this->position);
-    if (!data.has_value())
+    if (this->numberOfFields == 1) {
+      gz::msgs::Double msg;
+      *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now);
+      auto frame = msg.mutable_header()->add_data();
+      frame->set_key("frame_id");
+      frame->add_value((this->frameId == "") ? this->Name() : this->frameId);
+      auto data = dataPoints[0];
+      if (!data.has_value())
+      {
+        gzwarn << "Failed to acquire value perhaps out of field?\n";
+        return false;
+      }
+      msg.set_data(data.value());
+      // TODO(anyone) Add sensor noise.
+      this->pub.Publish(msg);
+    }
+    else if (this->numberOfFields == 3)
     {
-      gzwarn << "Failed to acquire value perhaps out of field?\n";
-      return false;
+      gz::msgs::Vector3d msg;
+      *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now);
+      auto frame = msg.mutable_header()->add_data();
+      frame->set_key("frame_id");
+      frame->add_value((this->frameId == "") ? this->Name() : this->frameId);
+
+      if (!dataPoints[0].has_value() || !dataPoints[1].has_value()
+        || !dataPoints[2].has_value())
+      {
+        gzwarn << "Failed to acquire value perhaps out of field?\n";
+        return false;
+      }
+
+      math::Vector3d vector(
+        dataPoints[0].value(), dataPoints[1].value(), dataPoints[2].value());
+      auto transformed = transformFrame(this->transformType, this->objectPose,
+        this->velocity, vector);
+
+      msg.set_x(transformed.X());
+      msg.set_y(transformed.Y());
+      msg.set_z(transformed.Z());
+      this->pub.Publish(msg);
     }
-    msg.set_data(data.value());
-    // TODO(anyone) Add sensor noise.
-    this->pub.Publish(msg);
     return true;
   }
 
@@ -150,28 +274,42 @@ class EnvironmentalSensor : public gz::sensors::Sensor
   {
     gzdbg << "Setting new data table\n";
     auto data = _data->Data();
-    if(!data->frame.Has(this->fieldName))
+    for (std::size_t i = 0; i < this->numberOfFields; ++i)
     {
-      gzwarn << "Environmental sensor could not find field "
-        << this->fieldName << "\n";
-      this->ready = false;
-      return;
+      // If field name is empty it was intentionally left out so the column
+      // should default to zero. Otherwise if wrong name throw an error and
+      // disable output.
+      if (this->fieldName[i] != "" && !data->frame.Has(this->fieldName[i]))
+      {
+        gzwarn << "Environmental sensor could not find field "
+          << this->fieldName[i] << "\n";
+        this->ready = false;
+        return;
+      }
     }
 
     this->gridField = data;
-    this->session = this->gridField->frame[this->fieldName].CreateSession();
-    if (!this->gridField->staticTime)
+    for (std::size_t i = 0; i < this->numberOfFields; ++i)
     {
-      this->session = this->gridField->frame[this->fieldName].StepTo(
-        *this->session,
-        std::chrono::duration<double>(_curr_time).count());
-    }
-    this->ready = true;
+      if (this->fieldName[i] == "")
+        continue;
 
-    if(!this->session.has_value())
-    {
-      gzerr << "Exceeded time stamp." << std::endl;
+      this->session[i] =
+        this->gridField->frame[this->fieldName[i]].CreateSession();
+      if (!this->gridField->staticTime)
+      {
+        this->session[i] = this->gridField->frame[this->fieldName[i]].StepTo(
+          *this->session[i],
+          std::chrono::duration<double>(_curr_time).count());
+      }
+
+      if(!this->session[i].has_value())
+      {
+        gzerr << "Exceeded time stamp." << std::endl;
+      }
     }
+
+    this->ready = true;
   }
 
   ////////////////////////////////////////////////////////////////
@@ -186,7 +324,8 @@ class EnvironmentalSensor : public gz::sensors::Sensor
   {
     if (!this->ready) return false;
 
-    const auto worldPosition = worldPose(_entity, _ecm).Pos();
+    this->objectPose = worldPose(_entity, _ecm);
+    const auto worldPosition = this->objectPose.Pos();
     auto lookupCoords =
       getGridFieldCoordinates(_ecm, worldPosition, this->gridField);
 
@@ -195,23 +334,31 @@ class EnvironmentalSensor : public gz::sensors::Sensor
       return false;
     }
 
+    auto vel = _ecm.Component<components::WorldLinearVelocity>(_entity);
+    if (vel != nullptr)
+    {
+      this->velocity = vel->Data();
+    }
     this->position = lookupCoords.value();
     return true;
   }
 
   ////////////////////////////////////////////////////////////////
   public: std::string Field() const
   {
-    return fieldName;
+    return fieldName[0];
   }
 
   private: bool ready {false};
-  private: math::Vector3d position;
-  private: std::string fieldName;
+  private: math::Vector3d position, velocity;
+  private: math::Pose3d objectPose;
+  private: std::size_t numberOfFields{1};
+  private: std::string fieldName[3];
   private: std::string frameId;
-  private: std::optional<gz::math::InMemorySession<double, double>> session;
+  private: std::optional<gz::math::InMemorySession<double, double>> session[3];
   private: std::shared_ptr<gz::sim::v7::components::EnvironmentalData>
     gridField;
+  private: TransformType transformType{TransformType::GLOBAL};
 };
 
 class gz::sim::EnvironmentalSensorSystemPrivate {
@@ -313,6 +460,8 @@ void EnvironmentalSensorSystem::PreUpdate(const gz::sim::UpdateInfo &_info,
           gzerr << "No sensor data loaded\n";
         }
 
+        enableComponent<components::WorldLinearVelocity>(_ecm, _entity);
+
         // Keep track of this sensor
         this->dataPtr->entitySensorMap.insert(std::make_pair(_entity,
             std::move(sensor)));

src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh

@@ -34,6 +34,36 @@ class EnvironmentalSensorSystemPrivate;
 /// field_name refers to the type of data you would like to output.
 /// Alternatively, if you would like to specify a custom field name you may do
 /// so using the <environment_variable> tag.
+///
+/// Additionally, the environment sensor supports scenarios where the data is in
+/// the form of vector fields. For instance in the case of wind or ocean
+/// currents.
+///
+/// Tags:
+///  <output_format> - Either "scalar" or "vector3" depending on type
+///    of output data desired.
+///  <environment_variable> - Only for scalar type. The name of the column of
+///    the CSV file to be used.
+///  <environment_variable_x> - The name of the field to be used as the x value
+///    in global frame for vector3 fields. Note: If this is left out and
+///    vector3 is set, then the x value defaults to zero.
+///  <environment_variable_y> - The name of the field to be used as the y value
+///    in global frame for vector3 fields. Note: If this is left out and
+///    vector3 is set, then the y value defaults to zero.
+///  <environment_variable_z> - The name of the field to be used as the z value
+///    in global frame vector3 fields. Note: If this is left out and
+///    vector3 is set, then the z value defaults to zero.
+///  <transform_type> - When handling vector2 or vector3 types it may be in our
+///    interest to transform them to local coordinate frames. For instance
+///    measurements of ocean currents or wind depend on the orientation and
+///    velocity of the sensor. This field can have 4 values:
+///      * GLOBAL - Don't transform the vectors to a local frame.
+///      * LOCAL - Transform the vector to a local frame.
+///      * ADD_VELOCITY_GLOBAL - Don't transform to local frame but account for
+///          velocity change.
+///      * ADD_VELOCITY_LOCAL - Transform to local frame and account for sensor
+///          velocity. If you're working with wind or ocean currents, this is
+///          probably the option you want.
 class EnvironmentalSensorSystem:
   public gz::sim::System,
   public gz::sim::ISystemConfigure,