Skip to content

Commit

Permalink
Fix enviroment system loading mechanism (#1842)
Browse files Browse the repository at this point in the history 
* Fix enviroment system loading mechanism

Currently, there is an issue with the way the Environment loader plugin loads data. In particular it directly writes to the ECM. While this makes sense intuitively, it does not work in practice as the GUI runs on a client process while systems that use it run on the server. This PR fixes this issue by introducing a topic through which the GUI may load Environment Data on the server.

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* small changes

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Working on porting the visuals

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>

* Actually send message for loading from ui to environment preload plugin.

Visuallization still goes 💥

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Rewrite EnvironmentVisualization Widget to be simpler.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* fix crashes.

Vis still not working

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Get a different 💥

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Works some times.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fixed synchronization issues.

Now left with one more crash that needs debugging when "play" is hit.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* No more 💥s 🎉

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Sprinkled with healthy dose of Doxygen

Also refactored the visualization tool out.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* More style fixes

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fix Typo with unit map

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address PR feedback

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Style fixes

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fix incorrect use of path.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fix example loading issues.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Update src/systems/environment_preload/VisualizationTool.cc

Co-authored-by: Mabel Zhang <mabel@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>

* Adds a warning regarding loading plugins.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Automatically loads plugin if missing

This commit automatically loads the environment preload plugin if it is
missing.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address some feedback I missed

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address some feedback

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fixes issue  described by @iche033.

However fix depends on gazebosim/gz-math#551

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Fixed failing tests

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

---------

Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Arjo Chakravarty <arjo129@gmail.com>
Co-authored-by: Michael Carroll <mjcarroll@intrinsic.ai>
Co-authored-by: Mabel Zhang <mabel@openrobotics.org>
Co-authored-by: Ian Chen <ichen@openrobotics.org>
  • Loading branch information
@arjo129 @mjcarroll
@mabelzhang @iche033
4 people authored Oct 3, 2023
1 parent 35c068e commit f6efeef
Show file tree
Hide file tree
Showing 9 changed files with 732 additions and 293 deletions.
4 changes: 4 additions & 0 deletions examples/worlds/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,4 +2,8 @@ file(GLOB files "*.sdf")
install(FILES ${files}
DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds)

file(GLOB csv_files "*.csv")
install(FILES ${csv_files}
DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds)

add_subdirectory(thumbnails)
11 changes: 10 additions & 1 deletion examples/worlds/environmental_sensor.sdf
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,12 @@
<?xml version="1.0" ?>
<!-- This example show cases how to load and unload environmental data. -->
<!--
This example show cases how to load and unload environmental data.
Before opening this file in a seperate terminal run:
gz topic -e -t /sensors/humidity
Then open this file by running:
gz sim environmental_sensor.sdf
Play the simulation and you should see a data stream of increasing numbers eventually stop at 90.
-->
<sdf version="1.6">
<world name="environmental_sensor_example">
<plugin
Expand All @@ -20,6 +27,7 @@
name="gz::sim::systems::EnvironmentalSystem">
</plugin>

<!-- The system specifies where to preload -->
<plugin
filename="gz-sim-environment-preload-system"
name="gz::sim::systems::EnvironmentPreload">
Expand Down Expand Up @@ -104,6 +112,7 @@
<sensor name="custom_sensor" type="custom" gz:type="environmental_sensor/humidity">
<always_on>1</always_on>
<update_rate>30</update_rate>
<topic>sensors/humidity</topic>
</sensor>
</link>
</model>
Expand Down
119 changes: 85 additions & 34 deletions src/gui/plugins/environment_loader/EnvironmentLoader.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,11 @@

#include <gz/gui/Application.hh>
#include <gz/gui/MainWindow.hh>
#include <gz/sim/components/Environment.hh>
#include <gz/sim/Util.hh>
#include <gz/sim/components/Name.hh>

#include <gz/plugin/Register.hh>
#include <gz/msgs/entity_plugin_v.pb.h>

#include <atomic>
#include <mutex>
Expand All @@ -33,6 +34,11 @@
#include <gz/common/CSVStreams.hh>
#include <gz/common/DataFrame.hh>

#include <gz/transport/Node.hh>

#include <gz/msgs/data_load_options.pb.h>
#include <gz/msgs/Utility.hh>

using namespace gz;
using namespace sim;

Expand All @@ -42,6 +48,11 @@ namespace sim
{
inline namespace GZ_SIM_VERSION_NAMESPACE
{
const char* preload_plugin_name{
"gz::sim::systems::EnvironmentPreload"};
const char* preload_plugin_filename{
"gz-sim-environment-preload-system"};
using Units = msgs::DataLoadPathOptions_DataAngularUnits;
/// \brief Private data class for EnvironmentLoader
class EnvironmentLoaderPrivate
{
Expand All @@ -65,7 +76,7 @@ class EnvironmentLoaderPrivate
public: int zIndex{-1};

/// \brief Index of data dimension to be used as units.
public: QString unit;
public: QString unit{"radians"};

public: using ReferenceT = math::SphericalCoordinates::CoordinateType;

Expand All @@ -76,12 +87,12 @@ class EnvironmentLoaderPrivate
{QString("ecef"), math::SphericalCoordinates::ECEF}};

/// \brief Map of supported spatial units.
public: const QMap<QString, components::EnvironmentalData::ReferenceUnits>
public: const QMap<QString, Units>
unitMap{
{QString("degree"),
components::EnvironmentalData::ReferenceUnits::DEGREES},
Units::DataLoadPathOptions_DataAngularUnits_DEGREES},
{QString("radians"),
components::EnvironmentalData::ReferenceUnits::RADIANS}
Units::DataLoadPathOptions_DataAngularUnits_RADIANS}
};

/// \brief Spatial reference.
Expand All @@ -92,6 +103,12 @@ class EnvironmentLoaderPrivate

/// \brief Whether to attempt an environmental data load.
public: std::atomic<bool> needsLoad{false};

/// \brief Gz transport node
public: transport::Node node;

/// \brief publisher
public: std::optional<transport::Node::Publisher> pub{std::nullopt};
};
}
}
Expand Down Expand Up @@ -123,46 +140,80 @@ void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *)
void EnvironmentLoader::Update(const UpdateInfo &,
EntityComponentManager &_ecm)
{
if (this->dataPtr->needsLoad)
auto world = worldEntity(_ecm);

if (!this->dataPtr->pub.has_value())
{
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
this->dataPtr->needsLoad = false;

/// TODO(arjo): Handle the case where we are loading a file in windows.
/// Should SDFormat files support going from windows <=> unix paths?
std::ifstream dataFile(this->dataPtr->dataPath.toStdString());
gzmsg << "Loading environmental data from "
<< this->dataPtr->dataPath.toStdString()
<< std::endl;
try
auto topic = transport::TopicUtils::AsValidTopic(
scopedName(world, _ecm) + "/" + "environment");
this->dataPtr->pub =
{this->dataPtr->node.Advertise<msgs::DataLoadPathOptions>(topic)};
}

static bool warned = false;
if (!this->dataPtr->pub->HasConnections() && !warned)
{
warned = true;
gzwarn << "Could not find a subscriber for the environment. "
<< "Attempting to load environmental preload plugin."
<< std::endl;

auto nameComp = _ecm.Component<components::Name>(world);
if (nullptr == nameComp) {
gzerr << "Failed to get world name" << std::endl;
return;
}
auto worldName = nameComp->Data();
msgs::EntityPlugin_V req;
req.mutable_entity()->set_id(world);
auto plugin = req.add_plugins();
plugin->set_name(preload_plugin_name);
plugin->set_filename(preload_plugin_filename);
plugin->set_innerxml("");
msgs::Boolean res;
bool result;
const unsigned int timeout = 5000;
const auto service = transport::TopicUtils::AsValidTopic(
"/world/" + worldName + "/entity/system/add");
if (service.empty())
{
gzerr << "Unable to request " << service << std::endl;
return;
}

if (this->dataPtr->node.Request(service, req, timeout, res, result))
{
using ComponentDataT = components::EnvironmentalData;
auto data = ComponentDataT::MakeShared(
common::IO<ComponentDataT::FrameT>::ReadFrom(
common::CSVIStreamIterator(dataFile),
common::CSVIStreamIterator(),
this->dataPtr->timeIndex, {
static_cast<size_t>(this->dataPtr->xIndex),
static_cast<size_t>(this->dataPtr->yIndex),
static_cast<size_t>(this->dataPtr->zIndex)}),
this->dataPtr->referenceMap[this->dataPtr->reference],
this->dataPtr->unitMap[this->dataPtr->unit]);

using ComponentT = components::Environment;
_ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)});
gzdbg << "Added plugin successfully" << std::endl;
}
catch (const std::invalid_argument &exc)
else
{
gzerr << "Failed to load environmental data" << std::endl
<< exc.what() << std::endl;
gzerr << "Failed to load plugin" << std::endl;
}
}
}

/////////////////////////////////////////////////
void EnvironmentLoader::ScheduleLoad()
{
this->dataPtr->needsLoad = this->IsConfigured();
if(this->IsConfigured() && this->dataPtr->pub.has_value())
{
msgs::DataLoadPathOptions data;
data.set_path(this->dataPtr->dataPath.toStdString());
data.set_time(
this->dataPtr->dimensionList[this->dataPtr->timeIndex].toStdString());
data.set_x(
this->dataPtr->dimensionList[this->dataPtr->xIndex].toStdString());
data.set_y(
this->dataPtr->dimensionList[this->dataPtr->yIndex].toStdString());
data.set_z(
this->dataPtr->dimensionList[this->dataPtr->zIndex].toStdString());
auto referenceFrame = this->dataPtr->referenceMap[this->dataPtr->reference];

data.set_coordinate_type(msgs::ConvertCoord(referenceFrame));
data.set_units(this->dataPtr->unitMap[this->dataPtr->unit]);

this->dataPtr->pub->Publish(data);
}
}

/////////////////////////////////////////////////
Expand Down
Loading

0 comments on commit f6efeef

Please sign in to comment.