Add multichannel lookup for environment sensors. (#1814)

Often when sensing we may be interested in vector quantities. This PR extends the environmental_sensor to support vector quantities. Some additional work was required handle transformations. In particular if the data is something like Ocean Currents or Wind then the data has to be transformed to the robot's local coordinate frame and the robot's velocity also has to be accounted for.

Signed-off-by: Arjo Chakravarty <[email protected]>

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,