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,115 @@ 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("field_type"))
+ {
+ auto dataType = _sdf.Element()->Get<std::string>("field_type");
+ if (dataType == "scalar")
+ {
+ this->numberOfFields = 1;
+ }
+ else if (dataType == "vector2")
+ {
+ this->numberOfFields = 2;
+ }
+ else if (dataType == "vector3")
+ {
+ this->numberOfFields = 3;
+ }
+ else
+ {
+ gzerr << "Invalid data type of " << dataType << " given."
+ << "valid options are scalar, vector2 or vector3. "
+ << "Defaulting to scalar" << std::endl;
+ }
+ }
+
+ // Hansle 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 2:
+ this->pub = this->node.Advertise<gz::msgs::Vector2d>(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 == 2)
+ {
+ if (
+ _sdf.Element()->HasElement("x_variable") &&
+ _sdf.Element()->HasElement("y_variable"))
+ {
+ this->fieldName[0] =
+ _sdf.Element()->Get<std::string>("x_variable");
+ this->fieldName[1] =
+ _sdf.Element()->Get<std::string>("y_variable");
+ }
+ else
+ {
+ gzerr << "Please Specify x_variable AND y_variable" << "\n";
+ }
+ }
+ else if (_sdf.Element() != nullptr &&
+ this->numberOfFields == 3)
+ {
+ if (
+ _sdf.Element()->HasElement("x_variable") &&
+ _sdf.Element()->HasElement("y_variable") &&
+ _sdf.Element()->HasElement("z_variable"))
+ {
+ this->fieldName[0] =
+ _sdf.Element()->Get<std::string>("x_variable");
+ this->fieldName[1] =
+ _sdf.Element()->Get<std::string>("y_variable");
+ this->fieldName[2] =
+ _sdf.Element()->Get<std::string>("z_variable");
+ }
+ else
+ {
+ gzerr <<
+ "Please Specify x_variable AND y_variable AND z_variable" << "\n";
+ }
+ }
  else
  {
- this->fieldName = type.substr(strlen(SENSOR_TYPE_PREFIX));
+ this->fieldName[0] = type.substr(strlen(SENSOR_TYPE_PREFIX));
  }
 
  // Allow setting custom frame_ids
@@ -98,7 +196,7 @@ class EnvironmentalSensor : public gz::sensors::Sensor
  this->frameId = _sdf.Element()->Get<std::string>("frame_id");
  }
 
- gzdbg << "Loaded environmental sensor for " << this->fieldName
+ gzdbg << "Loaded environmental sensor for " << this->fieldName[0]
  << " publishing on " << this->Topic() << std::endl;
 
  return true;
@@ -113,30 +211,87 @@ 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->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.has_value()) return false;
+ if (!this->session[i].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 == 2)
  {
- gzwarn << "Failed to acquire value perhaps out of field?\n";
- return false;
+ gz::msgs::Vector2d 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())
+ {
+ gzwarn << "Failed to acquire value perhaps out of field?\n";
+ return false;
+ }
+
+ math::Vector3d vector(dataPoints[0].value(), dataPoints[1].value(), 0);
+ auto transformed = transformFrame(this->transformType, this->objectPose,
+ this->velocity, vector);
+
+ msg.set_x(transformed.X());
+ msg.set_y(transformed.Y());
+
+ this->pub.Publish(msg);
+ }
+ else if (this->numberOfFields == 3)
+ {
+ 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())
+ {
+ 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,27 +305,34 @@ 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(!data->frame.Has(this->fieldName[i]))
+ {
+ gzwarn << "Environmental sensor could not find field "
+ << this->fieldName[0] << "\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;
+ 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());
+ }
+ this->ready = true;
 
- if(!this->session.has_value())
- {
- gzerr << "Exceeded time stamp." << std::endl;
+ if(!this->session[i].has_value())
+ {
+ gzerr << "Exceeded time stamp." << std::endl;
+ }
  }
  }
 
@@ -186,7 +348,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 +358,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 +484,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)));