Skip to content

Commit

Permalink
Add multichannel lookup for environment sensors. (#1814)
Browse files Browse the repository at this point in the history 
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]>
  • Loading branch information
@arjo129
arjo129 authored Dec 15, 2022
1 parent 53835d8 commit 652e238
Show file tree
Hide file tree
Showing 8 changed files with 496 additions and 88 deletions.
7 changes: 7 additions & 0 deletions src/systems/environmental_sensor_system/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}
)
237 changes: 193 additions & 44 deletions src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
*
*/
#include "EnvironmentalSensorSystem.hh"
#include "TransformTypes.hh"

#include <gz/plugin/Register.hh>

Expand All @@ -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>
Expand Down Expand Up @@ -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
Expand All @@ -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;
}
Expand All @@ -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;
}

Expand All @@ -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;
}

////////////////////////////////////////////////////////////////
Expand All @@ -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);

Expand All @@ -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 {
Expand Down Expand Up @@ -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)));
Expand Down
30 changes: 30 additions & 0 deletions src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
Loading

0 comments on commit 652e238

Please sign in to comment.