diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 031dc21ee9..3aa0ca4e7a 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -71,7 +71,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate public: bool visualize{false}; /// \brief Sample resolutions - public: math::Vector3d samples; + public: math::Vector3 samples; /// \brief Is the file loaded public: bool fileLoaded{false}; @@ -106,8 +106,13 @@ class gz::sim::systems::EnvironmentPreloadPrivate // Only visualize if a file exists return; } - auto converted = msgs::Convert(_resChanged); - if (this->samples == converted) + math::Vector3 converted{ + static_cast(ceil(_resChanged.x())), + static_cast(ceil(_resChanged.y())), + static_cast(ceil(_resChanged.z()))}; + if (this->samples.X() == converted.X() && + this->samples.Y() == converted.Y() && + this->samples.Z() == converted.Z()) { // If the sample has not changed return. // This is because resampling is expensive. diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc index 75b37b6557..338b92aede 100644 --- a/src/systems/environment_preload/VisualizationTool.cc +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -61,7 +61,7 @@ void EnvironmentVisualizationTool::Step( const UpdateInfo &_info, const EntityComponentManager &_ecm, const std::shared_ptr &_data, - double _xSamples, double _ySamples, double _zSamples) + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) { if (this->finishedTime) { @@ -114,7 +114,7 @@ void EnvironmentVisualizationTool::Step( ///////////////////////////////////////////////// void EnvironmentVisualizationTool::Visualize( const std::shared_ptr &_data, - double _xSamples, double _ySamples, double _zSamples) + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) { for (auto key : _data->frame.Keys()) { @@ -126,13 +126,13 @@ void EnvironmentVisualizationTool::Visualize( auto dy = step.Y() / _ySamples; auto dz = step.Z() / _zSamples; std::size_t idx = 0; - for (std::size_t x_steps = 0; x_steps < ceil(_xSamples); x_steps++) + for (std::size_t x_steps = 0; x_steps < _xSamples; x_steps++) { auto x = lower_bound.X() + x_steps * dx; - for (std::size_t y_steps = 0; y_steps < ceil(_ySamples); y_steps++) + for (std::size_t y_steps = 0; y_steps < _ySamples; y_steps++) { auto y = lower_bound.Y() + y_steps * dy; - for (std::size_t z_steps = 0; z_steps < ceil(_zSamples); z_steps++) + for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++) { auto z = lower_bound.Z() + z_steps * dz; auto res = frame.LookUp(session, math::Vector3d(x, y, z)); diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh index bb2e2fce1b..06505c042b 100644 --- a/src/systems/environment_preload/VisualizationTool.hh +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -69,7 +69,7 @@ class EnvironmentVisualizationTool /// \brief Create publisher structures whenever a new environment is made /// available. - /// \param[in] _data Data to be visuallized + /// \param[in] _data Data to be visualized /// \param[in] _info simulation info for current time step private: void CreatePointCloudTopics( const std::shared_ptr &_data, @@ -81,7 +81,7 @@ class EnvironmentVisualizationTool /// \brief Step the visualizations /// \param[in] _info The simulation info including timestep /// \param[in] _ecm The Entity-Component-Manager - /// \param[in] _data The data to be visuallized + /// \param[in] _data The data to be visualized /// \param[in] _xSample Samples along x /// \param[in] _ySample Samples along y /// \param[in] _zSample Samples along z @@ -89,16 +89,16 @@ class EnvironmentVisualizationTool const UpdateInfo &_info, const EntityComponentManager &_ecm, const std::shared_ptr &_data, - double _xSamples, double _ySamples, double _zSamples); + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); /// \brief Publishes a sample of the data - /// \param[in] _data The data to be visuallized + /// \param[in] _data The data to be visualized /// \param[in] _xSample Samples along x /// \param[in] _ySample Samples along y /// \param[in] _zSample Samples along z private: void Visualize( const std::shared_ptr &_data, - double _xSamples, double _ySamples, double _zSamples); + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); /// \brief Get the point cloud data. private: void Publish(); @@ -106,7 +106,7 @@ class EnvironmentVisualizationTool /// \brief Resize the point cloud structure (used to reallocate /// memory when resolution changes) /// \param[in] _ecm The Entity-Component-Manager - /// \param[in] _data The data to be visuallized + /// \param[in] _data The data to be visualized /// \param[in] _xSample Samples along x /// \param[in] _ySample Samples along y /// \param[in] _zSample Samples along z