From 58361a52e1e0bc26c3c976952c3ef2963756a33b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 25 Jun 2025 09:00:47 +0000 Subject: [PATCH 01/18] Adds a system which enables exporting occupancy grids. This PR adds a system which exports occupancy grids. Currently its a work in progress. The BFS algorithm seems to not identify when there cannot be any progress made. Signed-off-by: Arjo Chakravarty --- src/systems/CMakeLists.txt | 1 + .../free_space_explorer/CMakeLists.txt | 8 + .../free_space_explorer/FreeSpaceExplorer.cc | 300 ++++++++++++++++++ .../free_space_explorer/FreeSpaceExplorer.hh | 75 +++++ 4 files changed, 384 insertions(+) create mode 100644 src/systems/free_space_explorer/CMakeLists.txt create mode 100644 src/systems/free_space_explorer/FreeSpaceExplorer.cc create mode 100644 src/systems/free_space_explorer/FreeSpaceExplorer.hh diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 16eae8326b..2efaabf56b 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -103,6 +103,7 @@ add_subdirectory(environment_preload) add_subdirectory(environmental_sensor_system) add_subdirectory(follow_actor) add_subdirectory(force_torque) +add_subdirectory(free_space_explorer) add_subdirectory(hydrodynamics) add_subdirectory(imu) add_subdirectory(joint_controller) diff --git a/src/systems/free_space_explorer/CMakeLists.txt b/src/systems/free_space_explorer/CMakeLists.txt new file mode 100644 index 0000000000..2d7a514b8a --- /dev/null +++ b/src/systems/free_space_explorer/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(free-space-explorer + SOURCES + FreeSpaceExplorer.cc + PUBLIC_LINK_LIBS + gz-common::gz-common + gz-math::gz-math + gz-plugin::core +) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc new file mode 100644 index 0000000000..1dfb883203 --- /dev/null +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -0,0 +1,300 @@ +#include "FreeSpaceExplorer.hh" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace gz; +using namespace sim; +using namespace systems; + +// Custom hash function for std::pair +struct PairHash { + template + std::size_t operator () (const std::pair& p) const { + auto h1 = std::hash{}(p.first); + auto h2 = std::hash{}(p.second); + + // Simple way to combine hashes. You might want a more robust one + // for very specific use cases, but this is generally sufficient. + return h1 ^ (h2 << 1); // XOR with a left shift to mix bits + } +}; + +struct gz::sim::systems::FreeSpaceExplorerPrivateData { + std::optional grid; + Model model; + std::size_t numRows, numCols; + double resolution; + std::string scanTopic; + std::string sensorLink; + math::Pose3d position; + gz::transport::Node node; + + bool recievedMessageForPose {false}; + std::queue nextPosition; + + std::recursive_mutex m; + + ///////////////////////////////////////////////// + /// Callback for laser scan message + void OnLaserScanMsg(const msgs::LaserScan &_scan) + { + const std::lock_guard lock(this->m); + if (!this->grid.has_value()) + { + gzerr<< "Grid not yet inited"; + return; + } + + double currAngle = _scan.angle_min(); + + /// Iterate through laser scan and mark bressenham line of free space + for (uint32_t index = 0; index < _scan.count(); index++) + { + auto length = _scan.ranges(index); + auto obstacleExists = length <= _scan.range_max(); + length = (length > _scan.range_max()) ? _scan.range_max() : length; + //auto length = 0.5; + auto toX = length * cos(currAngle) + this->position.Pos().X(); + auto toY = length * sin(currAngle) + this->position.Pos().Y(); + + this->grid->MarkFree(this->position.Pos().X(), this->position.Pos().Y(), toX, toY); + + if (obstacleExists) + this->grid->MarkOccupied(toX, toY); + + currAngle += _scan.angle_step(); + } + + //this->recievedMessageForPose = true; + + /// Hack(arjoc) for checking the output of the pixel + std::vector pixelData; + this->grid->ExportToRGBImage(pixelData); + + common::Image fromOccupancy; + fromOccupancy.SetFromData( + pixelData.data(), this->grid->GetWidth(), this->grid->GetHeight(), common::Image::PixelFormatType::RGB_INT8); + fromOccupancy.SavePNG("output.png"); + if (!this->nextPosition.empty()) + { + return; + } + auto nextPos = this->GetNextPoint(_scan); + gzerr << "Setting next position " << nextPos->Pos() <nextPosition.push(nextPos.value()); + } + else { + gzerr << "Scan complete\n"; + } + } + + ///////////////////////////////////////////////// + /// Perform search over occupancy grid + std::optional GetNextPoint(const msgs::LaserScan &_scan) + { + const std::lock_guard lock(this->m); + if (!this->grid.has_value()) + { + gzerr<< "Grid not yet inited"; + return {}; + } + std::queue> q; + std::unordered_set, PairHash> visited; + + int gridX, gridY; + if(!this->grid->WorldToGrid( this->position.Pos().X(), + this->position.Pos().Y(), gridX, gridY)) + { + gzerr << "Proposed point outside of bounds"; + return {}; + } + q.emplace(gridX, gridY); + visited.emplace(gridX, gridY); + auto maxInfoGain = 0.0; + std::pair bestGain = + std::make_pair(gridX, gridY); + + auto numPoints = 0; + + while (!q.empty()) + { + auto pt = q.front(); + q.pop(); + + for(int i = -1; i <=1; i++) + { + for(int j = -1; j <=1; j++) + { + + auto x = pt.first + i; + auto y = pt.second + j; + + if(visited.count(std::make_pair(x,y)) > 0) + { + continue; + } + numPoints++; + + if(this->grid->GetCellState(x, y) == math::CellState::Free) + { + + auto infoGain = this->ScoreInfoGain(x, y, _scan); + if ( infoGain.has_value() && infoGain > maxInfoGain) + { + bestGain = std::make_pair(x, y); + maxInfoGain = infoGain.value(); + } + + visited.emplace(x, y); + q.emplace(x, y); + } + } + } + } + + gzerr << "Searched " << numPoints <grid->GridToWorld(bestGain.first, bestGain.second, newX, newY); + math::Pose3d newPose(this->position); + newPose.Pos().X(newX); + newPose.Pos().Y(newY); + newPose.Pos().Z(this->position.Pos().Z()); + return newPose; + } + + /// Scores information gain given laser scan parameters + std::optional ScoreInfoGain(int _x, int _y, const msgs::LaserScan &_scan) + { + const std::lock_guard lock(this->m); + if (!this->grid.has_value()) + { + gzerr<< "Grid not yet inited"; + return {}; + } + + double currAngle = _scan.angle_min(); + auto length = _scan.range_max(); + auto numCells = _scan.range_max() / this->resolution; + + double infoGain = 0.0; + + /// Iterate through laser scan and evaluate information gain + for (uint32_t index = 0; index < _scan.count(); index++) + { + auto x = static_cast(std::round(numCells * cos(currAngle) + _x)); + auto y = static_cast(std::round(numCells * sin(currAngle) + _y)); + infoGain += this->grid->CalculateIGain(_x, _y, x, y); + + currAngle += _scan.angle_step(); + } + + return infoGain; + } +}; + +///////////////////////////////////////////////// +FreeSpaceExplorer::FreeSpaceExplorer() +{ + + this->dataPtr = std::make_unique(); +} + +///////////////////////////////////////////////// +FreeSpaceExplorer::~FreeSpaceExplorer() +{ + +} + +///////////////////////////////////////////////// +void FreeSpaceExplorer::Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = gz::sim::Model(_entity); + this->dataPtr->scanTopic = _sdf->Get("lidar_topic", "scan").first; + this->dataPtr->sensorLink = _sdf->Get("sensor_link", "link").first; + this->dataPtr->numRows = _sdf->Get("width", 10).first; + this->dataPtr->numCols = _sdf->Get("height", 10).first; + this->dataPtr->resolution = _sdf->Get("resolution", 1.0).first; + this->dataPtr->node.Subscribe(this->dataPtr->scanTopic, + &FreeSpaceExplorerPrivateData::OnLaserScanMsg, this->dataPtr.get()); + gzerr << "Loaded camera plugin listening on [" << this->dataPtr->scanTopic << "]\n"; +} + +///////////////////////////////////////////////// +void FreeSpaceExplorer::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + if (_info.paused) + { + return; + } + //TODO(arjo) check link name valisdity + auto l = Link(this->dataPtr->model.LinkByName(_ecm, this->dataPtr->sensorLink)); + if (!l.Valid(_ecm)){ + gzerr << "Invalid link name " << this->dataPtr->sensorLink < lock(this->dataPtr->m); + if (!this->dataPtr->grid.has_value()) + { + auto center_x = pose->Pos().X() - this->dataPtr->numRows * this->dataPtr->resolution / 2; + auto center_y = pose->Pos().Y() - this->dataPtr->numCols * this->dataPtr->resolution / 2; + math::OccupancyGrid g( + this->dataPtr->resolution, this->dataPtr->numRows, this->dataPtr->numCols, + center_x, center_y); + this->dataPtr->grid = {std::move(g)}; + this->dataPtr->position = pose.value(); + } + this->dataPtr->position = pose.value(); + + if (this->dataPtr->nextPosition.empty()) + { + return; + } + + auto modelPosCmd = this->dataPtr->nextPosition.front(); + this->dataPtr->nextPosition.pop(); + this->dataPtr->model.SetWorldPoseCmd(_ecm, modelPosCmd); + num_steps++; +} + + + +GZ_ADD_PLUGIN( + FreeSpaceExplorer, + gz::sim::System, + FreeSpaceExplorer::ISystemPreUpdate, + FreeSpaceExplorer::ISystemConfigure) + +GZ_ADD_PLUGIN_ALIAS( + FreeSpaceExplorer, + "gz::sim::systems::FreeSpaceExplorer") \ No newline at end of file diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.hh b/src/systems/free_space_explorer/FreeSpaceExplorer.hh new file mode 100644 index 0000000000..0b10b38c1e --- /dev/null +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.hh @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_SYSTEMS_FREESPACEEXPLORER_HH_ +#define GZ_SIM_SYSTEMS_FREESPACEEXPLORER_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + struct FreeSpaceExplorerPrivateData; + + /// \brief This plugin is to be used with a model that has a 2D + /// LiDAR attached to it. It uses this 2d lidar to export an occupancy + /// map of the world. + /// + /// ## System Parameters + /// + /// This system takes in the following parameters: + /// * - Topic to listen to LiDAR on + /// * - Number of columns in the occupancy map + /// * - Number of rows in the occupancy map + /// * - Resolution of an individual cell + /// * - Link on which the sensor is attached within the model. + class FreeSpaceExplorer: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate + { + /// \brief Constructor + public: FreeSpaceExplorer(); + + /// \brief Destructor + public: ~FreeSpaceExplorer() override; + + /// Documentation inherited + public: void Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &/*_eventMgr*/) override; + + /// Documentation inherited + public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif From d2791c29e604f2d61f688bad339f7243064b3eba Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Jun 2025 08:52:11 +0000 Subject: [PATCH 02/18] Mostly style fixes Signed-off-by: Arjo Chakravarty --- examples/worlds/export_occupancy_grid.sdf | 168 ++++++++++++++++++ .../free_space_explorer/FreeSpaceExplorer.cc | 38 +++- 2 files changed, 197 insertions(+), 9 deletions(-) create mode 100644 examples/worlds/export_occupancy_grid.sdf diff --git a/examples/worlds/export_occupancy_grid.sdf b/examples/worlds/export_occupancy_grid.sdf new file mode 100644 index 0000000000..e11373de60 --- /dev/null +++ b/examples/worlds/export_occupancy_grid.sdf @@ -0,0 +1,168 @@ + + + + 0.001 + 1.0 + + + + + + + + + + + ogre2 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + -1 -1 0.5 0 0 0 + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + scan + 10 + + + + 640 + 1 + 0 + 6.28 + + + + 0.08 + 9.0 + 0.01 + + + true + + + true + + + /scan + 100 + 100 + 0.25 + link + + + + + 1 1 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 1dfb883203..da77f9fc98 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -1,7 +1,24 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ #include "FreeSpaceExplorer.hh" #include #include +#include #include #include #include @@ -9,7 +26,17 @@ #include #include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include using namespace gz; using namespace sim; @@ -74,8 +101,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { currAngle += _scan.angle_step(); } - //this->recievedMessageForPose = true; - /// Hack(arjoc) for checking the output of the pixel std::vector pixelData; this->grid->ExportToRGBImage(pixelData); @@ -100,7 +125,8 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } ///////////////////////////////////////////////// - /// Perform search over occupancy grid + /// Perform search over occupancy grid for next position to + /// explore. std::optional GetNextPoint(const msgs::LaserScan &_scan) { const std::lock_guard lock(this->m); @@ -163,8 +189,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } } - gzerr << "Searched " << numPoints <dataPtr->nextPosition.front(); this->dataPtr->nextPosition.pop(); this->dataPtr->model.SetWorldPoseCmd(_ecm, modelPosCmd); - num_steps++; } - - GZ_ADD_PLUGIN( FreeSpaceExplorer, gz::sim::System, From 1c7001f882de877cf323ed6ecbf1a6d4945a5fc7 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 1 Jul 2025 04:40:37 +0000 Subject: [PATCH 03/18] Fix convergance issues Signed-off-by: Arjo Chakravarty --- .../free_space_explorer/FreeSpaceExplorer.cc | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index da77f9fc98..7ce16e11d3 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -67,6 +67,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { bool recievedMessageForPose {false}; std::queue nextPosition; + std::unordered_set, PairHash> previouslyVisited; std::recursive_mutex m; @@ -89,7 +90,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { auto length = _scan.ranges(index); auto obstacleExists = length <= _scan.range_max(); length = (length > _scan.range_max()) ? _scan.range_max() : length; - //auto length = 0.5; auto toX = length * cos(currAngle) + this->position.Pos().X(); auto toY = length * sin(currAngle) + this->position.Pos().Y(); @@ -147,7 +147,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } q.emplace(gridX, gridY); visited.emplace(gridX, gridY); - auto maxInfoGain = 0.0; + auto maxInfoGain = 0; std::pair bestGain = std::make_pair(gridX, gridY); @@ -176,7 +176,8 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { { auto infoGain = this->ScoreInfoGain(x, y, _scan); - if ( infoGain.has_value() && infoGain > maxInfoGain) + if (infoGain.has_value() && infoGain > maxInfoGain + && previouslyVisited.count(std::make_pair(x, y)) == 0) { bestGain = std::make_pair(x, y); maxInfoGain = infoGain.value(); @@ -188,14 +189,17 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } } } - - if (maxInfoGain < 1e-3) + gzerr << "Visited " << visited.size() << "\n"; + gzerr << "Infogan" << maxInfoGain << "\n"; + if (maxInfoGain < 1) { - gzerr << "Could not find areas of information gain\n"; - gzerr << maxInfoGain << "\n"; + gzmsg << "Could not find areas of information gain\n"; + gzmsg << maxInfoGain << "\n"; return std::nullopt; } + previouslyVisited.emplace(bestGain); + double newX, newY; this->grid->GridToWorld(bestGain.first, bestGain.second, newX, newY); math::Pose3d newPose(this->position); @@ -211,7 +215,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { const std::lock_guard lock(this->m); if (!this->grid.has_value()) { - gzerr<< "Grid not yet inited"; + gzerr<< "Waiting for occupancy grid to be intiallized." << std::endl; return {}; } @@ -226,7 +230,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { { auto x = static_cast(std::round(numCells * cos(currAngle) + _x)); auto y = static_cast(std::round(numCells * sin(currAngle) + _y)); - infoGain += this->grid->CalculateIGain(_x, _y, x, y); + infoGain += std::max(this->grid->CalculateIGain(_x, _y, x, y) - 1, 0); currAngle += _scan.angle_step(); } From f64434adcf435d3bda10e7c87c523a1469aebc8b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 1 Jul 2025 05:23:14 +0000 Subject: [PATCH 04/18] style Signed-off-by: Arjo Chakravarty --- .../free_space_explorer/FreeSpaceExplorer.cc | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 7ce16e11d3..4ce6d7be9f 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -83,7 +83,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } double currAngle = _scan.angle_min(); - + /// Iterate through laser scan and mark bressenham line of free space for (uint32_t index = 0; index < _scan.count(); index++) { @@ -102,7 +102,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } /// Hack(arjoc) for checking the output of the pixel - std::vector pixelData; + std::vector pixelData; this->grid->ExportToRGBImage(pixelData); common::Image fromOccupancy; @@ -121,7 +121,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } else { gzerr << "Scan complete\n"; - } + } } ///////////////////////////////////////////////// @@ -162,7 +162,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { { for(int j = -1; j <=1; j++) { - + auto x = pt.first + i; auto y = pt.second + j; @@ -174,7 +174,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { if(this->grid->GetCellState(x, y) == math::CellState::Free) { - + auto infoGain = this->ScoreInfoGain(x, y, _scan); if (infoGain.has_value() && infoGain > maxInfoGain && previouslyVisited.count(std::make_pair(x, y)) == 0) @@ -182,7 +182,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { bestGain = std::make_pair(x, y); maxInfoGain = infoGain.value(); } - + visited.emplace(x, y); q.emplace(x, y); } @@ -215,7 +215,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { const std::lock_guard lock(this->m); if (!this->grid.has_value()) { - gzerr<< "Waiting for occupancy grid to be intiallized." << std::endl; + gzerr<< "Waiting for occupancy grid to be initialized." << std::endl; return {}; } @@ -230,7 +230,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { { auto x = static_cast(std::round(numCells * cos(currAngle) + _x)); auto y = static_cast(std::round(numCells * sin(currAngle) + _y)); - infoGain += std::max(this->grid->CalculateIGain(_x, _y, x, y) - 1, 0); + infoGain +=this->grid->CalculateIGain(_x, _y, x, y); currAngle += _scan.angle_step(); } @@ -242,7 +242,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { ///////////////////////////////////////////////// FreeSpaceExplorer::FreeSpaceExplorer() { - + this->dataPtr = std::make_unique(); } @@ -321,4 +321,4 @@ GZ_ADD_PLUGIN( GZ_ADD_PLUGIN_ALIAS( FreeSpaceExplorer, - "gz::sim::systems::FreeSpaceExplorer") \ No newline at end of file + "gz::sim::systems::FreeSpaceExplorer") From 255aaa4d5f57e510e316d11fc27c6a08487130d6 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 3 Jul 2025 08:22:11 +0000 Subject: [PATCH 05/18] Initial UI wireframe Signed-off-by: Arjo Chakravarty --- src/gui/plugins/CMakeLists.txt | 7 +- .../plugins/export_occupancy/CMakeLists.txt | 8 + .../export_occupancy/ExportOccupancy.cc | 51 +++++ .../export_occupancy/ExportOccupancy.hh | 60 ++++++ .../export_occupancy/ExportOccupancy.qml | 191 ++++++++++++++++++ .../export_occupancy/ExportOccupancy.qrc | 5 + .../free_space_explorer/FreeSpaceExplorer.cc | 21 +- 7 files changed, 330 insertions(+), 13 deletions(-) create mode 100644 src/gui/plugins/export_occupancy/CMakeLists.txt create mode 100644 src/gui/plugins/export_occupancy/ExportOccupancy.cc create mode 100644 src/gui/plugins/export_occupancy/ExportOccupancy.hh create mode 100644 src/gui/plugins/export_occupancy/ExportOccupancy.qml create mode 100644 src/gui/plugins/export_occupancy/ExportOccupancy.qrc diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 27603e74e2..0e7dd8e669 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -133,13 +133,14 @@ add_subdirectory(modules) add_subdirectory(align_tool) add_subdirectory(apply_force_torque) add_subdirectory(banana_for_scale) -add_subdirectory(component_inspector) add_subdirectory(component_inspector_editor) +add_subdirectory(component_inspector) add_subdirectory(copy_paste) add_subdirectory(entity_context_menu) add_subdirectory(entity_tree) add_subdirectory(environment_loader) add_subdirectory(environment_visualization) +add_subdirectory(export_occupancy) add_subdirectory(global_illumination_civct) add_subdirectory(global_illumination_vct) add_subdirectory(joint_position_controller) @@ -149,8 +150,8 @@ add_subdirectory(playback_scrubber) add_subdirectory(plot_3d) add_subdirectory(plotting) add_subdirectory(resource_spawner) -add_subdirectory(select_entities) add_subdirectory(scene_manager) +add_subdirectory(select_entities) add_subdirectory(shapes) add_subdirectory(spawn) add_subdirectory(transform_control) @@ -158,5 +159,5 @@ add_subdirectory(video_recorder) add_subdirectory(view_angle) add_subdirectory(visualization_capabilities) add_subdirectory(visualize_contacts) -add_subdirectory(visualize_lidar) add_subdirectory(visualize_frustum) +add_subdirectory(visualize_lidar) diff --git a/src/gui/plugins/export_occupancy/CMakeLists.txt b/src/gui/plugins/export_occupancy/CMakeLists.txt new file mode 100644 index 0000000000..fc1e4144fc --- /dev/null +++ b/src/gui/plugins/export_occupancy/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(ExportOccupancy + SOURCES ExportOccupancy.cc + QT_HEADERS ExportOccupancy.hh + PRIVATE_LINK_LIBS + gz-common::gz-common + gz-common::io + gz-math::gz-math +) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc new file mode 100644 index 0000000000..32e4c8fe24 --- /dev/null +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "ExportOccupancy.hh" +#include + +using namespace gz; +using namespace sim; + +class gz::sim::ExportOccupancyUiPrivate +{ +}; + +ExportOccupancyUi::ExportOccupancyUi() : dataPtr(std::make_unique()) +{ + +} + +ExportOccupancyUi::~ExportOccupancyUi() +{ + +} + +void ExportOccupancyUi::LoadConfig( + const tinyxml2::XMLElement *_pluginElem) +{ + +} + +void ExportOccupancyUi::Update(const UpdateInfo &, + EntityComponentManager &_ecm) +{ + +} + +// Register this plugin +GZ_ADD_PLUGIN(gz::sim::ExportOccupancyUi, gz::gui::Plugin) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.hh b/src/gui/plugins/export_occupancy/ExportOccupancy.hh new file mode 100644 index 0000000000..12777d6f73 --- /dev/null +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.hh @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_SIM_GUI_EXPORT_OCCUPANCY_HH_ +#define GZ_SIM_GUI_EXPORT_OCCUPANCY_HH_ + +#include + +#include "gz/sim/gui/GuiSystem.hh" +#include "gz/gui/qt.h" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ + class ExportOccupancyUiPrivate; + + /// \brief A GUI plugin for a user to export the occupancy + /// grid of the current world. + class ExportOccupancyUi : public gz::sim::GuiSystem + { + Q_OBJECT + /// \brief Constructor + public: ExportOccupancyUi(); + + /// \brief Destructor + public: ~ExportOccupancyUi() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &, + EntityComponentManager &_ecm) override; + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qml b/src/gui/plugins/export_occupancy/ExportOccupancy.qml new file mode 100644 index 0000000000..54ef9997fd --- /dev/null +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.qml @@ -0,0 +1,191 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Dialogs +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +/* +GridLayout { + columns: 8 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 500 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 +}*/ + +Item { + id: root + anchors.fill: parent + + property int lidarSamples: 0 + property real lidarRange: 0.0 + property real lidarRangeResolution: 0.0 + property real lidarAngularResolution: 0.0 + property real distanceFromLidarToGround: 0.0 + property real occupancyGridCellResolution: 0.0 + property int occupancyGridNumberOfHorizontalCells: 0 + + ScrollView { + anchors.fill: parent + ScrollBar.Horizontal: ScrollBar { + policy: ScrollBar.AlwaysOn // <--- This is the correct placement + } + ScrollBar.Vertical: ScrollBar { + policy: ScrollBar.AlwaysOff // <--- This is the correct placement + } + ColumnLayout { + anchors.fill: parent + anchors.margins: 20 + spacing: 10 + + // Lidar Samples + RowLayout { + Label { text: "Lidar Samples:" } + SpinBox { + id: lidarSamplesInput + Layout.fillWidth: true + from: 0 + to: 100000 // A reasonable upper bound, adjust as needed + value: root.lidarSamples + onValueChanged: root.lidarSamples = value + stepSize: 1 + } + } + + // Lidar Range + RowLayout { + Label { text: "Lidar Range (m):" } + TextField { + id: lidarRangeInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0 } + text: root.lidarRange.toFixed(2) + onEditingFinished: { + root.lidarRange = parseFloat(text) + if (isNaN(root.lidarRange)) root.lidarRange = 0.0 // Handle invalid input + } + } + } + + // Lidar Range Resolution + RowLayout { + Label { text: "Lidar Range Resolution (m):" } + TextField { + id: lidarRangeResolutionInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0 } + text: root.lidarRangeResolution.toFixed(3) + onEditingFinished: { + root.lidarRangeResolution = parseFloat(text) + if (isNaN(root.lidarRangeResolution)) root.lidarRangeResolution = 0.0 + } + } + } + + // Lidar Angular Resolution + RowLayout { + Label { text: "Lidar Angular Resolution (degrees):" } + TextField { + id: lidarAngularResolutionInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0; top: 360.0 } + text: root.lidarAngularResolution.toFixed(2) + onEditingFinished: { + root.lidarAngularResolution = parseFloat(text) + if (isNaN(root.lidarAngularResolution)) root.lidarAngularResolution = 0.0 + } + } + } + + // Distance from Lidar to Ground + RowLayout { + Label { text: "Distance from Lidar to Ground (m):" } + TextField { + id: distanceFromLidarToGroundInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0 } + text: root.distanceFromLidarToGround.toFixed(2) + onEditingFinished: { + root.distanceFromLidarToGround = parseFloat(text) + if (isNaN(root.distanceFromLidarToGround)) root.distanceFromLidarToGround = 0.0 + } + } + } + + // Occupancy Grid Cell Resolution + RowLayout { + Label { text: "Occupancy Grid Cell Resolution (m):" } + TextField { + id: occupancyGridCellResolutionInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0 } + text: root.occupancyGridCellResolution.toFixed(2) + onEditingFinished: { + root.occupancyGridCellResolution = parseFloat(text) + if (isNaN(root.occupancyGridCellResolution)) root.occupancyGridCellResolution = 0.0 + } + } + } + + // Occupancy Grid Number of Horizontal Cells + RowLayout { + Label { text: "Occupancy Grid Number of Horizontal Cells:" } + SpinBox { + id: occupancyGridHorizontalCellsInput + Layout.fillWidth: true + from: 0 + to: 10000 // Adjust upper bound as needed + value: root.occupancyGridNumberOfHorizontalCells + onValueChanged: root.occupancyGridNumberOfHorizontalCells = value + stepSize: 1 + } + } + + RowLayout { + Label { text: "Occupancy Grid Number of Vertical Cells:" } + SpinBox { + id: occupancyGridVerticalCellsInput + Layout.fillWidth: true + from: 0 + to: 10000 // Adjust upper bound as needed + value: root.occupancyGridNumberOfHorizontalCells + onValueChanged: root.occupancyGridNumberOfHorizontalCells = value + stepSize: 1 + } + } + + // Example of how you might use the properties (e.g., a button to print values) + Button { + text: "Add Probe" + onClicked: { + console.log("Lidar Samples:", root.lidarSamples) + console.log("Lidar Range:", root.lidarRange, "m") + console.log("Lidar Range Resolution:", root.lidarRangeResolution, "m") + console.log("Lidar Angular Resolution:", root.lidarAngularResolution, "degrees") + console.log("Distance from Lidar to Ground:", root.distanceFromLidarToGround, "m") + console.log("Occupancy Grid Cell Resolution:", root.occupancyGridCellResolution, "m") + console.log("Occupancy Grid Number of Horizontal Cells:", root.occupancyGridNumberOfHorizontalCells) + } + } + } + } +} diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qrc b/src/gui/plugins/export_occupancy/ExportOccupancy.qrc new file mode 100644 index 0000000000..64c227f3d7 --- /dev/null +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.qrc @@ -0,0 +1,5 @@ + + + ExportOccupancy.qml + + diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 4ce6d7be9f..affb07569d 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -105,22 +105,23 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { std::vector pixelData; this->grid->ExportToRGBImage(pixelData); - common::Image fromOccupancy; - fromOccupancy.SetFromData( - pixelData.data(), this->grid->GetWidth(), this->grid->GetHeight(), common::Image::PixelFormatType::RGB_INT8); - fromOccupancy.SavePNG("output.png"); + if (!this->nextPosition.empty()) { return; } auto nextPos = this->GetNextPoint(_scan); - gzerr << "Setting next position " << nextPos->Pos() <Pos() <nextPosition.push(nextPos.value()); } else { - gzerr << "Scan complete\n"; + common::Image fromOccupancy; + fromOccupancy.SetFromData( + pixelData.data(), this->grid->GetWidth(), this->grid->GetHeight(), common::Image::PixelFormatType::RGB_INT8); + fromOccupancy.SavePNG("output.png"); + gzmsg << "Scan complete\n"; } } @@ -132,7 +133,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { const std::lock_guard lock(this->m); if (!this->grid.has_value()) { - gzerr<< "Grid not yet inited"; + gzerr << "Grid not yet inited" << std::endl; return {}; } std::queue> q; @@ -142,7 +143,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { if(!this->grid->WorldToGrid( this->position.Pos().X(), this->position.Pos().Y(), gridX, gridY)) { - gzerr << "Proposed point outside of bounds"; + gzerr << "Proposed point outside of bounds" << std::endl; return {}; } q.emplace(gridX, gridY); @@ -266,7 +267,7 @@ void FreeSpaceExplorer::Configure( this->dataPtr->resolution = _sdf->Get("resolution", 1.0).first; this->dataPtr->node.Subscribe(this->dataPtr->scanTopic, &FreeSpaceExplorerPrivateData::OnLaserScanMsg, this->dataPtr.get()); - gzerr << "Loaded camera plugin listening on [" << this->dataPtr->scanTopic << "]\n"; + gzmsg << "Loaded camera plugin listening on [" << this->dataPtr->scanTopic << "]\n"; } ///////////////////////////////////////////////// @@ -281,7 +282,7 @@ void FreeSpaceExplorer::PreUpdate( //TODO(arjo) check link name valisdity auto l = Link(this->dataPtr->model.LinkByName(_ecm, this->dataPtr->sensorLink)); if (!l.Valid(_ecm)){ - gzerr << "Invalid link name " << this->dataPtr->sensorLink <dataPtr->sensorLink << std::endl; return; } auto pose = l.WorldPose(_ecm); From c5dc155b7fd82b0a01c7f6f2c32b5d2871b3208f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 21 Jul 2025 11:48:44 +0000 Subject: [PATCH 06/18] Initial implementation of GUI This commit pushes an initial version of the code. Signed-off-by: Arjo Chakravarty --- .../export_occupancy/ExportOccupancy.cc | 80 ++++++++++++++++++- .../export_occupancy/ExportOccupancy.hh | 4 + .../export_occupancy/ExportOccupancy.qml | 38 ++++----- 3 files changed, 97 insertions(+), 25 deletions(-) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc index 32e4c8fe24..008ad55910 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -16,8 +16,13 @@ */ #include "ExportOccupancy.hh" +#include +#include +#include #include +#include + using namespace gz; using namespace sim; @@ -27,7 +32,8 @@ class gz::sim::ExportOccupancyUiPrivate ExportOccupancyUi::ExportOccupancyUi() : dataPtr(std::make_unique()) { - + gui::App()->Engine()->rootContext()->setContextProperty( + "exportOccupancy", this); } ExportOccupancyUi::~ExportOccupancyUi() @@ -38,7 +44,10 @@ ExportOccupancyUi::~ExportOccupancyUi() void ExportOccupancyUi::LoadConfig( const tinyxml2::XMLElement *_pluginElem) { + if (this->title.empty()) + this->title = "Export Occupancy"; + gui::App()->findChild()->installEventFilter(this); } void ExportOccupancyUi::Update(const UpdateInfo &, @@ -47,5 +56,74 @@ void ExportOccupancyUi::Update(const UpdateInfo &, } +void ExportOccupancyUi::StartExport(double _samples, double _range, double _rangeRes, double _angularRes, + double _distanceFromGround, double _gridResolution, std::size_t _numWidth, std::size_t _numHeight) +{ + gz::msgs::EntityFactory factoryReq; + std::stringstream ss; + ss << R"( + + 0 0 )" << _distanceFromGround << R"( 0 0 0 + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + freespace_explorer/scan + 10 + + + + )"<< _samples << R"( + )" << _angularRes << R"( + 0 + 6.28 + + + + 0.08 + )"<< _range << R"( + )"<< _rangeRes < + + + true + + + true + + + freespace_explorer/scan + )" << _numWidth << R"( + )" << _numHeight << R"( + )" << _gridResolution << R"( + link + + + )"; + factoryReq.set_sdf(ss.str()); +} + // Register this plugin GZ_ADD_PLUGIN(gz::sim::ExportOccupancyUi, gz::gui::Plugin) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.hh b/src/gui/plugins/export_occupancy/ExportOccupancy.hh index 12777d6f73..fa8a17df3e 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.hh +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.hh @@ -50,6 +50,10 @@ inline namespace GZ_SIM_VERSION_NAMESPACE public: void Update(const UpdateInfo &, EntityComponentManager &_ecm) override; + /// Triggers the export of our + public: Q_INVOKABLE void StartExport(double _samples, double _range, double _rangeRes, double _angularRes, + double _distanceFromGround, double _gridResolution, std::size_t _numWidth, std::size_t _numHeight); + /// \internal /// \brief Pointer to private data private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qml b/src/gui/plugins/export_occupancy/ExportOccupancy.qml index 54ef9997fd..31306cd308 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.qml +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.qml @@ -21,37 +21,23 @@ import QtQuick.Controls.Material 2.1 import QtQuick.Layouts 1.3 import "qrc:/qml" -/* -GridLayout { - columns: 8 - columnSpacing: 10 - Layout.minimumWidth: 350 - Layout.minimumHeight: 500 - anchors.fill: parent - anchors.leftMargin: 10 - anchors.rightMargin: 10 -}*/ - Item { id: root anchors.fill: parent - property int lidarSamples: 0 - property real lidarRange: 0.0 - property real lidarRangeResolution: 0.0 + property int lidarSamples: 640 + property real lidarRange: 9.0 + property real lidarRangeResolution: 0.01 property real lidarAngularResolution: 0.0 property real distanceFromLidarToGround: 0.0 - property real occupancyGridCellResolution: 0.0 - property int occupancyGridNumberOfHorizontalCells: 0 + property real occupancyGridCellResolution: 0.1 + property int occupancyGridNumberOfHorizontalCells: 100 + property int occupancyGridNumberOfVerticalCells: 10 0 ScrollView { anchors.fill: parent - ScrollBar.Horizontal: ScrollBar { - policy: ScrollBar.AlwaysOn // <--- This is the correct placement - } - ScrollBar.Vertical: ScrollBar { - policy: ScrollBar.AlwaysOff // <--- This is the correct placement - } + ScrollBar.horizontal.policy: ScrollBar.AlwaysOn + ScrollBar.vertical.policy: ScrollBar.AlwaysOff ColumnLayout { anchors.fill: parent anchors.margins: 20 @@ -167,8 +153,8 @@ Item { Layout.fillWidth: true from: 0 to: 10000 // Adjust upper bound as needed - value: root.occupancyGridNumberOfHorizontalCells - onValueChanged: root.occupancyGridNumberOfHorizontalCells = value + value: root.occupancyGridNumberOfVerticalCells + onValueChanged: root.occupancyGridNumberOfVerticalCells = value stepSize: 1 } } @@ -184,6 +170,10 @@ Item { console.log("Distance from Lidar to Ground:", root.distanceFromLidarToGround, "m") console.log("Occupancy Grid Cell Resolution:", root.occupancyGridCellResolution, "m") console.log("Occupancy Grid Number of Horizontal Cells:", root.occupancyGridNumberOfHorizontalCells) + exportOccupancy.StartExport( + root.lidarSamples, root.lidarRange, root.lidarRangeResolution, root.lidarAngularResolution, + root.distanceFromLidarToGround, root.occupancyGridCellResolution, root.occupancyGridNumberOfHorizontalCells, + root.occupancyGridNumberOfVerticalCells); } } } From 5e0956c8e550464c38899e8d13f8a141df488723 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 15 Aug 2025 07:18:02 +0000 Subject: [PATCH 07/18] Fix typo Signed-off-by: Arjo Chakravarty --- .../export_occupancy/ExportOccupancy.qml | 2 +- .../free_space_explorer/FreeSpaceExplorer.cc | 84 +++++++++++++++++-- 2 files changed, 80 insertions(+), 6 deletions(-) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qml b/src/gui/plugins/export_occupancy/ExportOccupancy.qml index 31306cd308..e6a0b09a89 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.qml +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.qml @@ -32,7 +32,7 @@ Item { property real distanceFromLidarToGround: 0.0 property real occupancyGridCellResolution: 0.1 property int occupancyGridNumberOfHorizontalCells: 100 - property int occupancyGridNumberOfVerticalCells: 10 0 + property int occupancyGridNumberOfVerticalCells: 100 ScrollView { anchors.fill: parent diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index affb07569d..9f57062a18 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -71,6 +71,68 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { std::recursive_mutex m; + ///////////////////////////////////////////////// + /// \brief Perform search over occupancy grid to see if there are any + /// reachable unknown cells and return their count. + int CountReachableUnknowns() + { + const std::lock_guard lock(this->m); + if (!this->grid.has_value()) + { + gzerr << "Grid not yet inited" << std::endl; + return 0; + } + std::queue> q; + std::unordered_set, PairHash> visited; + + int gridX, gridY; + if(!this->grid->WorldToGrid( this->position.Pos().X(), + this->position.Pos().Y(), gridX, gridY)) + { + gzerr << "Current position outside of bounds" << std::endl; + return 0; + } + q.emplace(gridX, gridY); + visited.emplace(gridX, gridY); + + int unknownCellCount = 0; + while (!q.empty()) + { + auto pt = q.front(); + q.pop(); + + for(int i = -1; i <=1; i++) + { + for(int j = -1; j <=1; j++) + { + if (i == 0 && j == 0) + continue; + + auto x = pt.first + i; + auto y = pt.second + j; + + auto neighbor = std::make_pair(x, y); + if(visited.count(neighbor) > 0) + { + continue; + } + + auto cellState = this->grid->GetCellState(x, y); + if (cellState == math::CellState::Free) + { + q.emplace(neighbor); + } + else if (cellState == math::CellState::Unknown) + { + unknownCellCount++; + } + visited.emplace(neighbor); + } + } + } + return unknownCellCount; + } + ///////////////////////////////////////////////// /// Callback for laser scan message void OnLaserScanMsg(const msgs::LaserScan &_scan) @@ -101,22 +163,34 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { currAngle += _scan.angle_step(); } - /// Hack(arjoc) for checking the output of the pixel - std::vector pixelData; - this->grid->ExportToRGBImage(pixelData); + if (!this->nextPosition.empty()) + { + return; + } + gzerr << this->CountReachableUnknowns() << "\n"; - if (!this->nextPosition.empty()) + if (this->CountReachableUnknowns() == 0) { + std::vector pixelData; + this->grid->ExportToRGBImage(pixelData); + common::Image fromOccupancy; + fromOccupancy.SetFromData( + pixelData.data(), this->grid->GetWidth(), this->grid->GetHeight(), common::Image::PixelFormatType::RGB_INT8); + fromOccupancy.SavePNG("output.png"); + gzmsg << "Scan complete: No reachable unknown cells.\n"; return; } + auto nextPos = this->GetNextPoint(_scan); - gzmsg << "Setting next position " << nextPos->Pos() <Pos() <nextPosition.push(nextPos.value()); } else { + std::vector pixelData; + this->grid->ExportToRGBImage(pixelData); common::Image fromOccupancy; fromOccupancy.SetFromData( pixelData.data(), this->grid->GetWidth(), this->grid->GetHeight(), common::Image::PixelFormatType::RGB_INT8); From 30a270d204024d2ecc6c33b08e5ec287ecfc09d9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 22 Aug 2025 02:07:43 +0000 Subject: [PATCH 08/18] Bring up to speed to latest gz-math changes Signed-off-by: Arjo Chakravarty --- .../free_space_explorer/FreeSpaceExplorer.cc | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 9f57062a18..169a872a8a 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -117,12 +117,12 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { continue; } - auto cellState = this->grid->GetCellState(x, y); - if (cellState == math::CellState::Free) + auto cellState = this->grid->CellState(x, y); + if (cellState == math::OccupancyCellState::Free) { q.emplace(neighbor); } - else if (cellState == math::CellState::Unknown) + else if (cellState == math::OccupancyCellState::Unknown) { unknownCellCount++; } @@ -176,7 +176,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { this->grid->ExportToRGBImage(pixelData); common::Image fromOccupancy; fromOccupancy.SetFromData( - pixelData.data(), this->grid->GetWidth(), this->grid->GetHeight(), common::Image::PixelFormatType::RGB_INT8); + pixelData.data(), this->grid->Width(), this->grid->Height(), common::Image::PixelFormatType::RGB_INT8); fromOccupancy.SavePNG("output.png"); gzmsg << "Scan complete: No reachable unknown cells.\n"; return; @@ -193,7 +193,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { this->grid->ExportToRGBImage(pixelData); common::Image fromOccupancy; fromOccupancy.SetFromData( - pixelData.data(), this->grid->GetWidth(), this->grid->GetHeight(), common::Image::PixelFormatType::RGB_INT8); + pixelData.data(), this->grid->Width(), this->grid->Height(), common::Image::PixelFormatType::RGB_INT8); fromOccupancy.SavePNG("output.png"); gzmsg << "Scan complete\n"; } @@ -247,7 +247,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } numPoints++; - if(this->grid->GetCellState(x, y) == math::CellState::Free) + if(this->grid->CellState(x, y) == math::OccupancyCellState::Free) { auto infoGain = this->ScoreInfoGain(x, y, _scan); @@ -295,7 +295,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } double currAngle = _scan.angle_min(); - auto length = _scan.range_max(); auto numCells = _scan.range_max() / this->resolution; double infoGain = 0.0; @@ -330,7 +329,7 @@ FreeSpaceExplorer::~FreeSpaceExplorer() void FreeSpaceExplorer::Configure( const gz::sim::Entity &_entity, const std::shared_ptr &_sdf, - gz::sim::EntityComponentManager &_ecm, + gz::sim::EntityComponentManager &/*_ecm*/, gz::sim::EventManager &/*_eventMgr*/) { this->dataPtr->model = gz::sim::Model(_entity); From a6ed74243cd58a5f0ec3a4c128aa9ec694c51cf9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 22 Aug 2025 03:34:41 +0000 Subject: [PATCH 09/18] Make clean example Signed-off-by: Arjo Chakravarty --- examples/worlds/export_occupancy_grid.sdf | 11 +++++ .../free_space_explorer/FreeSpaceExplorer.cc | 45 ++++++++++--------- .../free_space_explorer/FreeSpaceExplorer.hh | 7 ++- 3 files changed, 41 insertions(+), 22 deletions(-) diff --git a/examples/worlds/export_occupancy_grid.sdf b/examples/worlds/export_occupancy_grid.sdf index e11373de60..9bfb6d24c8 100644 --- a/examples/worlds/export_occupancy_grid.sdf +++ b/examples/worlds/export_occupancy_grid.sdf @@ -1,5 +1,15 @@ + + + + /scan_image + + 0.001 1.0 @@ -93,6 +103,7 @@ filename="gz-sim-free-space-explorer-system" name="gz::sim::systems::FreeSpaceExplorer"> /scan + /scan_image 100 100 0.25 diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 169a872a8a..1096b9b0c2 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -61,6 +62,8 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { std::size_t numRows, numCols; double resolution; std::string scanTopic; + std::string imageTopic; + gz::transport::Node::Publisher imagePub; std::string sensorLink; math::Pose3d position; gz::transport::Node node; @@ -168,16 +171,17 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { return; } - gzerr << this->CountReachableUnknowns() << "\n"; - if (this->CountReachableUnknowns() == 0) { std::vector pixelData; this->grid->ExportToRGBImage(pixelData); - common::Image fromOccupancy; - fromOccupancy.SetFromData( - pixelData.data(), this->grid->Width(), this->grid->Height(), common::Image::PixelFormatType::RGB_INT8); - fromOccupancy.SavePNG("output.png"); + gz::msgs::Image imageMsg; + imageMsg.set_width(this->grid->Width()); + imageMsg.set_height(this->grid->Height()); + imageMsg.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8); + imageMsg.set_step(this->grid->Width() * 3); + imageMsg.set_data(pixelData.data(), pixelData.size()); + this->imagePub.Publish(imageMsg); gzmsg << "Scan complete: No reachable unknown cells.\n"; return; } @@ -188,15 +192,17 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { gzmsg << "Setting next position " << nextPos->Pos() <nextPosition.push(nextPos.value()); } - else { - std::vector pixelData; - this->grid->ExportToRGBImage(pixelData); - common::Image fromOccupancy; - fromOccupancy.SetFromData( - pixelData.data(), this->grid->Width(), this->grid->Height(), common::Image::PixelFormatType::RGB_INT8); - fromOccupancy.SavePNG("output.png"); - gzmsg << "Scan complete\n"; - } + + std::vector pixelData; + this->grid->ExportToRGBImage(pixelData); + gz::msgs::Image imageMsg; + imageMsg.set_width(this->grid->Width()); + imageMsg.set_height(this->grid->Height()); + imageMsg.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8); + imageMsg.set_step(this->grid->Width() * 3); + imageMsg.set_data(pixelData.data(), pixelData.size()); + this->imagePub.Publish(imageMsg); + //gzmsg << "Scan complete\n"; } ///////////////////////////////////////////////// @@ -264,12 +270,9 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } } } - gzerr << "Visited " << visited.size() << "\n"; - gzerr << "Infogan" << maxInfoGain << "\n"; + if (maxInfoGain < 1) { - gzmsg << "Could not find areas of information gain\n"; - gzmsg << maxInfoGain << "\n"; return std::nullopt; } @@ -334,13 +337,15 @@ void FreeSpaceExplorer::Configure( { this->dataPtr->model = gz::sim::Model(_entity); this->dataPtr->scanTopic = _sdf->Get("lidar_topic", "scan").first; + this->dataPtr->imageTopic = _sdf->Get("image_topic", "scan_image").first; this->dataPtr->sensorLink = _sdf->Get("sensor_link", "link").first; this->dataPtr->numRows = _sdf->Get("width", 10).first; this->dataPtr->numCols = _sdf->Get("height", 10).first; this->dataPtr->resolution = _sdf->Get("resolution", 1.0).first; this->dataPtr->node.Subscribe(this->dataPtr->scanTopic, &FreeSpaceExplorerPrivateData::OnLaserScanMsg, this->dataPtr.get()); - gzmsg << "Loaded camera plugin listening on [" << this->dataPtr->scanTopic << "]\n"; + this->dataPtr->imagePub = this->dataPtr->node.Advertise(this->dataPtr->imageTopic); + gzmsg << "Loaded lidar exploration plugin listening on [" << this->dataPtr->scanTopic << "]\n"; } ///////////////////////////////////////////////// diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.hh b/src/systems/free_space_explorer/FreeSpaceExplorer.hh index 0b10b38c1e..faf0eaa354 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.hh +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.hh @@ -18,6 +18,8 @@ #define GZ_SIM_SYSTEMS_FREESPACEEXPLORER_HH_ #include + +#include #include namespace gz @@ -30,10 +32,10 @@ namespace systems { struct FreeSpaceExplorerPrivateData; - /// \brief This plugin is to be used with a model that has a 2D + /// \brief This plugin is to be used with a model that has a 2D /// LiDAR attached to it. It uses this 2d lidar to export an occupancy /// map of the world. - /// + /// /// ## System Parameters /// /// This system takes in the following parameters: @@ -42,6 +44,7 @@ namespace systems /// * - Number of rows in the occupancy map /// * - Resolution of an individual cell /// * - Link on which the sensor is attached within the model. + /// * - Topic to publish occupancy map on class FreeSpaceExplorer: public gz::sim::System, public gz::sim::ISystemConfigure, From ac81d597ed9c43858273694eb21f40721da9ec9a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 20 Sep 2025 17:47:40 +0000 Subject: [PATCH 10/18] style Signed-off-by: Arjo Chakravarty --- .../environment_loader/EnvironmentLoader.qml | 2 +- .../export_occupancy/ExportOccupancy.cc | 39 ++- .../export_occupancy/ExportOccupancy.hh | 6 +- .../export_occupancy/ExportOccupancy.qml | 263 +++++++++--------- 4 files changed, 169 insertions(+), 141 deletions(-) diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.qml b/src/gui/plugins/environment_loader/EnvironmentLoader.qml index c727b517f7..637e88b713 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.qml +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.qml @@ -26,7 +26,7 @@ GridLayout { columns: 8 columnSpacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 500 + Layout.minimumHeight: 800 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc index 008ad55910..3576560e51 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -20,6 +20,10 @@ #include #include #include +#include + +#include "gz/sim/Util.hh" +#include "gz/sim/World.hh" #include @@ -28,9 +32,11 @@ using namespace sim; class gz::sim::ExportOccupancyUiPrivate { + public: std::string worldName; }; -ExportOccupancyUi::ExportOccupancyUi() : dataPtr(std::make_unique()) +ExportOccupancyUi::ExportOccupancyUi() : + dataPtr(std::make_unique()) { gui::App()->Engine()->rootContext()->setContextProperty( "exportOccupancy", this); @@ -48,22 +54,35 @@ void ExportOccupancyUi::LoadConfig( this->title = "Export Occupancy"; gui::App()->findChild()->installEventFilter(this); + } void ExportOccupancyUi::Update(const UpdateInfo &, EntityComponentManager &_ecm) { - + auto world = World(worldEntity(_ecm)); + if (!world.Valid(_ecm)) { + gzerr << "Could not get running world" << std::endl; + return; + } + if (!world.Name(_ecm).has_value()) + { + return; + } + this->dataPtr->worldName = world.Name(_ecm).value(); } -void ExportOccupancyUi::StartExport(double _samples, double _range, double _rangeRes, double _angularRes, - double _distanceFromGround, double _gridResolution, std::size_t _numWidth, std::size_t _numHeight) +void ExportOccupancyUi::StartExport(double _samples, double _range, + double _rangeRes, double _angularRes, + double _distanceFromGround, double _gridResolution, + std::size_t _numWidth, std::size_t _numHeight) { gz::msgs::EntityFactory factoryReq; std::stringstream ss; ss << R"( + - 0 0 )" << _distanceFromGround << R"( 0 0 0 + -1.5 -1.5 )" << _distanceFromGround << R"( 0 0 0 0.1 @@ -121,8 +140,18 @@ void ExportOccupancyUi::StartExport(double _samples, double _range, double _rang link + )"; + + gzerr << ss.str() < cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) + {}; + node.Request("/world/" + this->dataPtr->worldName + "/create", + factoryReq, cb); } // Register this plugin diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.hh b/src/gui/plugins/export_occupancy/ExportOccupancy.hh index fa8a17df3e..1ae9a8d687 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.hh +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.hh @@ -51,8 +51,10 @@ inline namespace GZ_SIM_VERSION_NAMESPACE EntityComponentManager &_ecm) override; /// Triggers the export of our - public: Q_INVOKABLE void StartExport(double _samples, double _range, double _rangeRes, double _angularRes, - double _distanceFromGround, double _gridResolution, std::size_t _numWidth, std::size_t _numHeight); + public: Q_INVOKABLE void StartExport(double _samples, double _range, + double _rangeRes, double _angularRes, + double _distanceFromGround, double _gridResolution, + std::size_t _numWidth, std::size_t _numHeight); /// \internal /// \brief Pointer to private data diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qml b/src/gui/plugins/export_occupancy/ExportOccupancy.qml index e6a0b09a89..e0050a52ea 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.qml +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.qml @@ -21,161 +21,158 @@ import QtQuick.Controls.Material 2.1 import QtQuick.Layouts 1.3 import "qrc:/qml" -Item { + +ColumnLayout { id: root anchors.fill: parent + anchors.margins: 20 + Layout.minimumWidth: 350 + Layout.minimumHeight: 800 + spacing: 10 - property int lidarSamples: 640 - property real lidarRange: 9.0 - property real lidarRangeResolution: 0.01 - property real lidarAngularResolution: 0.0 - property real distanceFromLidarToGround: 0.0 + property int lidarSamples: 360 + property real lidarRange: 100.0 + property real lidarRangeResolution: 0.1 + property real lidarAngularResolution: 1.0 + property real distanceFromLidarToGround: 0.3 property real occupancyGridCellResolution: 0.1 property int occupancyGridNumberOfHorizontalCells: 100 property int occupancyGridNumberOfVerticalCells: 100 - - ScrollView { - anchors.fill: parent - ScrollBar.horizontal.policy: ScrollBar.AlwaysOn - ScrollBar.vertical.policy: ScrollBar.AlwaysOff - ColumnLayout { - anchors.fill: parent - anchors.margins: 20 - spacing: 10 - - // Lidar Samples - RowLayout { - Label { text: "Lidar Samples:" } - SpinBox { - id: lidarSamplesInput - Layout.fillWidth: true - from: 0 - to: 100000 // A reasonable upper bound, adjust as needed - value: root.lidarSamples - onValueChanged: root.lidarSamples = value - stepSize: 1 - } + property string state: "configure" + // Lidar Samples + RowLayout { + Label { text: "Lidar Samples:" } + TextField { + id: lidarSamplesInput + Layout.fillWidth: true + validator: IntValidator { bottom: 1 } + text: root.lidarSamples + onEditingFinished: { + root.lidarSamples = parseInt(text) + if (isNaN(root.lidarSamples)) root.lidarSamples = 360.0 // Handle invalid input } + } + } - // Lidar Range - RowLayout { - Label { text: "Lidar Range (m):" } - TextField { - id: lidarRangeInput - Layout.fillWidth: true - validator: DoubleValidator { bottom: 0.0 } - text: root.lidarRange.toFixed(2) - onEditingFinished: { - root.lidarRange = parseFloat(text) - if (isNaN(root.lidarRange)) root.lidarRange = 0.0 // Handle invalid input - } - } + // Lidar Range + RowLayout { + Label { text: "Lidar Range (m):" } + TextField { + id: lidarRangeInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0 } + text: root.lidarRange.toFixed(2) + onEditingFinished: { + root.lidarRange = parseFloat(text) + if (isNaN(root.lidarRange)) root.lidarRange = 0.0 // Handle invalid input } + } + } - // Lidar Range Resolution - RowLayout { - Label { text: "Lidar Range Resolution (m):" } - TextField { - id: lidarRangeResolutionInput - Layout.fillWidth: true - validator: DoubleValidator { bottom: 0.0 } - text: root.lidarRangeResolution.toFixed(3) - onEditingFinished: { - root.lidarRangeResolution = parseFloat(text) - if (isNaN(root.lidarRangeResolution)) root.lidarRangeResolution = 0.0 - } - } + // Lidar Range Resolution + RowLayout { + Label { text: "Lidar Range Resolution (m):" } + TextField { + id: lidarRangeResolutionInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0 } + text: root.lidarRangeResolution.toFixed(3) + onEditingFinished: { + root.lidarRangeResolution = parseFloat(text) + if (isNaN(root.lidarRangeResolution)) root.lidarRangeResolution = 0.0 } + } + } - // Lidar Angular Resolution - RowLayout { - Label { text: "Lidar Angular Resolution (degrees):" } - TextField { - id: lidarAngularResolutionInput - Layout.fillWidth: true - validator: DoubleValidator { bottom: 0.0; top: 360.0 } - text: root.lidarAngularResolution.toFixed(2) - onEditingFinished: { - root.lidarAngularResolution = parseFloat(text) - if (isNaN(root.lidarAngularResolution)) root.lidarAngularResolution = 0.0 - } - } + // Lidar Angular Resolution + RowLayout { + Label { text: "Lidar Angular Resolution (degrees):" } + TextField { + id: lidarAngularResolutionInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0; top: 360.0 } + text: root.lidarAngularResolution.toFixed(2) + onEditingFinished: { + root.lidarAngularResolution = parseFloat(text) + if (isNaN(root.lidarAngularResolution)) root.lidarAngularResolution = 0.0 } + } + } - // Distance from Lidar to Ground - RowLayout { - Label { text: "Distance from Lidar to Ground (m):" } - TextField { - id: distanceFromLidarToGroundInput - Layout.fillWidth: true - validator: DoubleValidator { bottom: 0.0 } - text: root.distanceFromLidarToGround.toFixed(2) - onEditingFinished: { - root.distanceFromLidarToGround = parseFloat(text) - if (isNaN(root.distanceFromLidarToGround)) root.distanceFromLidarToGround = 0.0 - } - } + // Distance from Lidar to Ground + RowLayout { + Label { text: "Distance from Lidar to Ground (m):" } + TextField { + id: distanceFromLidarToGroundInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0 } + text: root.distanceFromLidarToGround.toFixed(2) + onEditingFinished: { + root.distanceFromLidarToGround = parseFloat(text) + if (isNaN(root.distanceFromLidarToGround)) root.distanceFromLidarToGround = 0.0 } + } + } - // Occupancy Grid Cell Resolution - RowLayout { - Label { text: "Occupancy Grid Cell Resolution (m):" } - TextField { - id: occupancyGridCellResolutionInput - Layout.fillWidth: true - validator: DoubleValidator { bottom: 0.0 } - text: root.occupancyGridCellResolution.toFixed(2) - onEditingFinished: { - root.occupancyGridCellResolution = parseFloat(text) - if (isNaN(root.occupancyGridCellResolution)) root.occupancyGridCellResolution = 0.0 - } - } + // Occupancy Grid Cell Resolution + RowLayout { + Label { text: "Cell Resolution (m):" } + TextField { + id: occupancyGridCellResolutionInput + Layout.fillWidth: true + validator: DoubleValidator { bottom: 0.0 } + text: root.occupancyGridCellResolution.toFixed(2) + onEditingFinished: { + root.occupancyGridCellResolution = parseFloat(text) + if (isNaN(root.occupancyGridCellResolution)) root.occupancyGridCellResolution = 0.0 } + } + } - // Occupancy Grid Number of Horizontal Cells - RowLayout { - Label { text: "Occupancy Grid Number of Horizontal Cells:" } - SpinBox { - id: occupancyGridHorizontalCellsInput - Layout.fillWidth: true - from: 0 - to: 10000 // Adjust upper bound as needed - value: root.occupancyGridNumberOfHorizontalCells - onValueChanged: root.occupancyGridNumberOfHorizontalCells = value - stepSize: 1 - } + // Occupancy Grid Number of Horizontal Cells + RowLayout { + Label { text: "Number of Horizontal Cells:" } + TextField { + id: occupancyGridHorizontalCellsInput + Layout.fillWidth: true + validator: IntValidator { bottom: 0 } + text: root.occupancyGridNumberOfHorizontalCells.toFixed(2) + onEditingFinished: { + root.occupancyGridNumberOfHorizontalCells = parseInt(text) + if (isNaN(root. root.occupancyGridNumberOfHorizontalCells))root.occupancyGridNumberOfHorizontalCells = 0 } + } + } - RowLayout { - Label { text: "Occupancy Grid Number of Vertical Cells:" } - SpinBox { - id: occupancyGridVerticalCellsInput - Layout.fillWidth: true - from: 0 - to: 10000 // Adjust upper bound as needed - value: root.occupancyGridNumberOfVerticalCells - onValueChanged: root.occupancyGridNumberOfVerticalCells = value - stepSize: 1 - } + RowLayout { + Label { text: "Number of Vertical Cells:" } + TextField { + id: occupancyGridVerticalCellsInput + Layout.fillWidth: true + validator: IntValidator { bottom: 0 } + text: root.occupancyGridNumberOfVerticalCells.toFixed(2) + onEditingFinished: { + root.occupancyGridNumberOfVerticalCells = parseInt(text) + if (isNaN(root. root.occupancyGridNumberOfVerticalCells)) root. root.occupancyGridNumberOfVerticalCells = 0 } + } + } - // Example of how you might use the properties (e.g., a button to print values) - Button { - text: "Add Probe" - onClicked: { - console.log("Lidar Samples:", root.lidarSamples) - console.log("Lidar Range:", root.lidarRange, "m") - console.log("Lidar Range Resolution:", root.lidarRangeResolution, "m") - console.log("Lidar Angular Resolution:", root.lidarAngularResolution, "degrees") - console.log("Distance from Lidar to Ground:", root.distanceFromLidarToGround, "m") - console.log("Occupancy Grid Cell Resolution:", root.occupancyGridCellResolution, "m") - console.log("Occupancy Grid Number of Horizontal Cells:", root.occupancyGridNumberOfHorizontalCells) - exportOccupancy.StartExport( - root.lidarSamples, root.lidarRange, root.lidarRangeResolution, root.lidarAngularResolution, - root.distanceFromLidarToGround, root.occupancyGridCellResolution, root.occupancyGridNumberOfHorizontalCells, - root.occupancyGridNumberOfVerticalCells); - } - } + // Example of how you might use the properties (e.g., a button to print values) + Button { + text: "Add Probe" + onClicked: { + console.log("Lidar Samples:", root.lidarSamples) + console.log("Lidar Range:", root.lidarRange, "m") + console.log("Lidar Range Resolution:", root.lidarRangeResolution, "m") + console.log("Lidar Angular Resolution:", root.lidarAngularResolution, "degrees") + console.log("Distance from Lidar to Ground:", root.distanceFromLidarToGround, "m") + console.log("Occupancy Grid Cell Resolution:", root.occupancyGridCellResolution, "m") + console.log("Occupancy Grid Number of Horizontal Cells:", root.occupancyGridNumberOfHorizontalCells) + exportOccupancy.StartExport( + root.lidarSamples, root.lidarRange, root.lidarRangeResolution, root.lidarAngularResolution, + root.distanceFromLidarToGround, root.occupancyGridCellResolution, root.occupancyGridNumberOfHorizontalCells, + root.occupancyGridNumberOfVerticalCells); } } } From 84ac97a5a7c744aeb5bb6218ee43a34653f2e311 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 20 Sep 2025 18:10:56 +0000 Subject: [PATCH 11/18] style Signed-off-by: Arjo Chakravarty --- src/gui/plugins/export_occupancy/ExportOccupancy.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc index 3576560e51..7e750e76f2 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -26,6 +26,7 @@ #include "gz/sim/World.hh" #include +#include using namespace gz; using namespace sim; @@ -54,7 +55,6 @@ void ExportOccupancyUi::LoadConfig( this->title = "Export Occupancy"; gui::App()->findChild()->installEventFilter(this); - } void ExportOccupancyUi::Update(const UpdateInfo &, From 68fec4c0d6fe06e04f93c22764139ae6cf79b102 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 20 Sep 2025 18:13:45 +0000 Subject: [PATCH 12/18] Revert stray change Signed-off-by: Arjo Chakravarty --- src/gui/plugins/environment_loader/EnvironmentLoader.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.qml b/src/gui/plugins/environment_loader/EnvironmentLoader.qml index 637e88b713..c727b517f7 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.qml +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.qml @@ -26,7 +26,7 @@ GridLayout { columns: 8 columnSpacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 800 + Layout.minimumHeight: 500 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 From 7f318a7ff9ac1ca80dbf63ce424516ec7f4bb65d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 20 Sep 2025 18:15:14 +0000 Subject: [PATCH 13/18] Revert stray change Signed-off-by: Arjo Chakravarty --- src/gui/plugins/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 0e7dd8e669..5ba2fde4de 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -133,8 +133,8 @@ add_subdirectory(modules) add_subdirectory(align_tool) add_subdirectory(apply_force_torque) add_subdirectory(banana_for_scale) -add_subdirectory(component_inspector_editor) add_subdirectory(component_inspector) +add_subdirectory(component_inspector_editor) add_subdirectory(copy_paste) add_subdirectory(entity_context_menu) add_subdirectory(entity_tree) From 96cf88f7fb512cca258715011c8cf325c3e28970 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 23 Sep 2025 06:37:16 +0000 Subject: [PATCH 14/18] Add support for a "start ecploration button" Signed-off-by: Arjo Chakravarty --- .../export_occupancy/ExportOccupancy.cc | 15 ++++++- .../export_occupancy/ExportOccupancy.hh | 5 ++- .../export_occupancy/ExportOccupancy.qml | 40 ++++++++++++++----- .../free_space_explorer/FreeSpaceExplorer.cc | 9 ++--- 4 files changed, 50 insertions(+), 19 deletions(-) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc index 7e750e76f2..2a71b67712 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -34,6 +34,8 @@ using namespace sim; class gz::sim::ExportOccupancyUiPrivate { public: std::string worldName; + public: gz::transport::Node node; + public: gz::transport::Node::Publisher startPub; }; ExportOccupancyUi::ExportOccupancyUi() : @@ -41,6 +43,9 @@ ExportOccupancyUi::ExportOccupancyUi() : { gui::App()->Engine()->rootContext()->setContextProperty( "exportOccupancy", this); + this->dataPtr->startPub = + this->dataPtr->node.Advertise("/start"); + } ExportOccupancyUi::~ExportOccupancyUi() @@ -82,7 +87,7 @@ void ExportOccupancyUi::StartExport(double _samples, double _range, ss << R"( - -1.5 -1.5 )" << _distanceFromGround << R"( 0 0 0 + 0 0 )" << _distanceFromGround << R"( 0 0 0 0.1 @@ -143,7 +148,6 @@ void ExportOccupancyUi::StartExport(double _samples, double _range, )"; - gzerr << ss.str() <dataPtr->startPub.Publish(start); +} + // Register this plugin GZ_ADD_PLUGIN(gz::sim::ExportOccupancyUi, gz::gui::Plugin) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.hh b/src/gui/plugins/export_occupancy/ExportOccupancy.hh index 1ae9a8d687..a35dfa5c8c 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.hh +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.hh @@ -50,12 +50,15 @@ inline namespace GZ_SIM_VERSION_NAMESPACE public: void Update(const UpdateInfo &, EntityComponentManager &_ecm) override; - /// Triggers the export of our + /// Triggers the export of our occupancy grid public: Q_INVOKABLE void StartExport(double _samples, double _range, double _rangeRes, double _angularRes, double _distanceFromGround, double _gridResolution, std::size_t _numWidth, std::size_t _numHeight); + /// Trigger scan start + public: Q_INVOKABLE void StartExploration(); + /// \internal /// \brief Pointer to private data private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qml b/src/gui/plugins/export_occupancy/ExportOccupancy.qml index e0050a52ea..2ffe4a6072 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.qml +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.qml @@ -41,6 +41,7 @@ ColumnLayout { property string state: "configure" // Lidar Samples RowLayout { + id: sampleRow Label { text: "Lidar Samples:" } TextField { id: lidarSamplesInput @@ -56,6 +57,7 @@ ColumnLayout { // Lidar Range RowLayout { + id: rangeRow Label { text: "Lidar Range (m):" } TextField { id: lidarRangeInput @@ -71,6 +73,7 @@ ColumnLayout { // Lidar Range Resolution RowLayout { + id: rangeResolutionRow Label { text: "Lidar Range Resolution (m):" } TextField { id: lidarRangeResolutionInput @@ -86,6 +89,7 @@ ColumnLayout { // Lidar Angular Resolution RowLayout { + id: angularResolutionRow Label { text: "Lidar Angular Resolution (degrees):" } TextField { id: lidarAngularResolutionInput @@ -101,6 +105,7 @@ ColumnLayout { // Distance from Lidar to Ground RowLayout { + id: heightRow Label { text: "Distance from Lidar to Ground (m):" } TextField { id: distanceFromLidarToGroundInput @@ -116,6 +121,7 @@ ColumnLayout { // Occupancy Grid Cell Resolution RowLayout { + id: resolutionRow Label { text: "Cell Resolution (m):" } TextField { id: occupancyGridCellResolutionInput @@ -131,6 +137,7 @@ ColumnLayout { // Occupancy Grid Number of Horizontal Cells RowLayout { + id: horizontalCellRow Label { text: "Number of Horizontal Cells:" } TextField { id: occupancyGridHorizontalCellsInput @@ -145,6 +152,7 @@ ColumnLayout { } RowLayout { + id: verticalCellRow Label { text: "Number of Vertical Cells:" } TextField { id: occupancyGridVerticalCellsInput @@ -160,19 +168,29 @@ ColumnLayout { // Example of how you might use the properties (e.g., a button to print values) Button { + id: startButton text: "Add Probe" onClicked: { - console.log("Lidar Samples:", root.lidarSamples) - console.log("Lidar Range:", root.lidarRange, "m") - console.log("Lidar Range Resolution:", root.lidarRangeResolution, "m") - console.log("Lidar Angular Resolution:", root.lidarAngularResolution, "degrees") - console.log("Distance from Lidar to Ground:", root.distanceFromLidarToGround, "m") - console.log("Occupancy Grid Cell Resolution:", root.occupancyGridCellResolution, "m") - console.log("Occupancy Grid Number of Horizontal Cells:", root.occupancyGridNumberOfHorizontalCells) - exportOccupancy.StartExport( - root.lidarSamples, root.lidarRange, root.lidarRangeResolution, root.lidarAngularResolution, - root.distanceFromLidarToGround, root.occupancyGridCellResolution, root.occupancyGridNumberOfHorizontalCells, - root.occupancyGridNumberOfVerticalCells); + if (startButton.text === "Add Probe") { + sampleRow.visible = false; + rangeRow.visible = false; + rangeResolutionRow.visible = false; + angularResolutionRow.visible = false; + horizontalCellRow.visible = false; + verticalCellRow.visible = false; + resolutionRow.visible = false; + heightRow.visible = false; + exportOccupancy.StartExport( + root.lidarSamples, root.lidarRange, root.lidarRangeResolution, root.lidarAngularResolution, + root.distanceFromLidarToGround, root.occupancyGridCellResolution, root.occupancyGridNumberOfHorizontalCells, + root.occupancyGridNumberOfVerticalCells); + startButton.text = "Start Scan"; + } + else if (startButton.text === "Start Scan") + { + exportOccupancy.StartExploration(); + startButton.text = "Save Occupancy Map"; + } } } } diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 4b51de406f..efc66ff973 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -402,18 +402,19 @@ void FreeSpaceExplorer::PostUpdate( if (!this->dataPtr->grid.has_value()) { - gzerr<< "Grid not yet inited"; return; } - - double currAngle = scan.angle_min(); /// Iterate through laser scan and mark bressenham line of free space for (uint32_t index = 0; index < scan.count(); index++) { auto length = scan.ranges(index); + if (std::isinf(length)) + { + length = scan.range_max(); + } auto obstacleExists = length <= scan.range_max(); length = (length > scan.range_max()) ? scan.range_max() : length; auto toX = length * cos(currAngle) + this->dataPtr->position.Pos().X(); @@ -432,7 +433,6 @@ void FreeSpaceExplorer::PostUpdate( { return; } - if (this->dataPtr->CountReachableUnknowns() == 0) { std::vector pixelData; @@ -451,7 +451,6 @@ void FreeSpaceExplorer::PostUpdate( auto nextPos = this->dataPtr->GetNextPoint(scan); if(nextPos.has_value()) { - gzmsg << "Setting next position " << nextPos->Pos() <dataPtr->nextPosition.push(nextPos.value()); } From b9a25829c5d1a72b51123daf38451970a35bcae4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 24 Sep 2025 09:45:37 +0000 Subject: [PATCH 15/18] Adds complete Export Occupancy Grid UI. Makes it trivial to export occupancy grids from gazebo worlds. Signed-off-by: Arjo Chakravarty --- .../export_occupancy/ExportOccupancy.cc | 56 ++++++++++++++++++- .../export_occupancy/ExportOccupancy.hh | 45 +++++++++++++++ .../export_occupancy/ExportOccupancy.qml | 43 +++++++++++++- 3 files changed, 140 insertions(+), 4 deletions(-) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc index 2a71b67712..c09c022b02 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -16,7 +16,12 @@ */ #include "ExportOccupancy.hh" + +#include +#include +#include #include +#include #include #include #include @@ -28,6 +33,8 @@ #include #include +#include + using namespace gz; using namespace sim; @@ -36,6 +43,8 @@ class gz::sim::ExportOccupancyUiPrivate public: std::string worldName; public: gz::transport::Node node; public: gz::transport::Node::Publisher startPub; + public: ImageProvider* provider; + public: gz::msgs::Image lastOccupancy; }; ExportOccupancyUi::ExportOccupancyUi() : @@ -45,7 +54,11 @@ ExportOccupancyUi::ExportOccupancyUi() : "exportOccupancy", this); this->dataPtr->startPub = this->dataPtr->node.Advertise("/start"); + this->dataPtr->provider = new ImageProvider(); + this->dataPtr->node.Subscribe("/scan_image", + &ExportOccupancyUi::OnImageMsg, + this); } ExportOccupancyUi::~ExportOccupancyUi() @@ -53,8 +66,28 @@ ExportOccupancyUi::~ExportOccupancyUi() } +void ExportOccupancyUi::RegisterImageProvider(const QString &_uniqueName) +{ + gz::gui::App()->Engine()->addImageProvider(_uniqueName, + this->dataPtr->provider); +} + +void ExportOccupancyUi::OnImageMsg(const gz::msgs::Image &img) +{ + this->dataPtr->lastOccupancy = img; + unsigned int height = img.height(); + unsigned int width = img.width(); + QImage::Format qFormat = QImage::Format_RGB888; + QImage image = QImage(width, height, qFormat); + image = QImage(reinterpret_cast( + img.data().c_str()), width, height, + 3 * width, qFormat); + this->dataPtr->provider->SetImage(image); + emit this->newImage(); +} + void ExportOccupancyUi::LoadConfig( - const tinyxml2::XMLElement *_pluginElem) + const tinyxml2::XMLElement */*_pluginElem*/) { if (this->title.empty()) this->title = "Export Occupancy"; @@ -165,5 +198,26 @@ void ExportOccupancyUi::StartExploration() this->dataPtr->startPub.Publish(start); } +void ExportOccupancyUi::Save() +{ + auto last = this->dataPtr->lastOccupancy; + //auto pt = msgs::Convert(last); + common::Image image; + image.SetFromData( + reinterpret_cast( + const_cast(last.data().data())), + last.width(), last.height(), + common::Image::PixelFormatType::RGB_INT8); + + QString fileName = QFileDialog::getSaveFileName( + nullptr, + tr("Save File"), + QDir::homePath(), // Default directory + tr("PNG Files (*.png);")); + if (fileName.isEmpty()) { + return; + } + image.SavePNG(fileName.toStdString()); +} // Register this plugin GZ_ADD_PLUGIN(gz::sim::ExportOccupancyUi, gz::gui::Plugin) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.hh b/src/gui/plugins/export_occupancy/ExportOccupancy.hh index a35dfa5c8c..edd509f877 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.hh +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.hh @@ -19,6 +19,9 @@ #define GZ_SIM_GUI_EXPORT_OCCUPANCY_HH_ #include +#include + +#include #include "gz/sim/gui/GuiSystem.hh" #include "gz/gui/qt.h" @@ -30,6 +33,36 @@ namespace sim // Inline bracket to help doxygen filtering. inline namespace GZ_SIM_VERSION_NAMESPACE { + class ImageProvider : public QQuickImageProvider + { + public: ImageProvider() + : QQuickImageProvider(QQuickImageProvider::Image) + { + } + + public: QImage requestImage(const QString &, QSize *, + const QSize &) override + { + if (!this->img.isNull()) + { + // Must return a copy + return this->img; + } + + // Placeholder in case we have no image yet + QImage i(400, 400, QImage::Format_RGB888); + i.fill(QColor(128, 128, 128, 100)); + return i; + } + + public: void SetImage(const QImage &_image) + { + this->img = _image; + } + + private: QImage img; + }; + class ExportOccupancyUiPrivate; /// \brief A GUI plugin for a user to export the occupancy @@ -56,9 +89,21 @@ inline namespace GZ_SIM_VERSION_NAMESPACE double _distanceFromGround, double _gridResolution, std::size_t _numWidth, std::size_t _numHeight); + /// Callback for when an occupancy scan is received + private: void OnImageMsg(const gz::msgs::Image &img); + /// Trigger scan start public: Q_INVOKABLE void StartExploration(); + /// Needed to scope image display + public: Q_INVOKABLE void RegisterImageProvider(const QString &_uniqueName); + + /// Save Dialog + public: Q_INVOKABLE void Save(); + + /// \brief Notify that a new image has been received. + signals: void newImage(); + /// \internal /// \brief Pointer to private data private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qml b/src/gui/plugins/export_occupancy/ExportOccupancy.qml index 2ffe4a6072..3e28f35d79 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.qml +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.qml @@ -38,7 +38,21 @@ ColumnLayout { property real occupancyGridCellResolution: 0.1 property int occupancyGridNumberOfHorizontalCells: 100 property int occupancyGridNumberOfVerticalCells: 100 - property string state: "configure" + property string uniqueName: "" + + Connections { + target: exportOccupancy + function onNewImage(){ image.reload(); } + } + + onParentChanged: { + if (undefined === parent) + return; + + uniqueName = parent.card().objectName + "imagedisplay"; + exportOccupancy.RegisterImageProvider(uniqueName); + image.reload(); + } // Lidar Samples RowLayout { id: sampleRow @@ -165,6 +179,21 @@ ColumnLayout { } } } + RowLayout { + id: imageView + visible: false + Image { + id: image + fillMode: Image.PreserveAspectFit + Layout.fillHeight: true + Layout.fillWidth: true + verticalAlignment: Image.AlignTop + function reload() { + // Force image request to C++ + source = "image://" + uniqueName + "/" + Math.random().toString(36).substr(2, 5); + } + } + } // Example of how you might use the properties (e.g., a button to print values) Button { @@ -180,17 +209,25 @@ ColumnLayout { verticalCellRow.visible = false; resolutionRow.visible = false; heightRow.visible = false; + imageView.visible = true; exportOccupancy.StartExport( - root.lidarSamples, root.lidarRange, root.lidarRangeResolution, root.lidarAngularResolution, - root.distanceFromLidarToGround, root.occupancyGridCellResolution, root.occupancyGridNumberOfHorizontalCells, + root.lidarSamples, root.lidarRange, + root.lidarRangeResolution, root.lidarAngularResolution, + root.distanceFromLidarToGround, + root.occupancyGridCellResolution, + root.occupancyGridNumberOfHorizontalCells, root.occupancyGridNumberOfVerticalCells); startButton.text = "Start Scan"; + root.Layout.minimumHeight = 500; } else if (startButton.text === "Start Scan") { exportOccupancy.StartExploration(); startButton.text = "Save Occupancy Map"; } + else { + exportOccupancy.Save(); + } } } } From 0ce7175c1f510f21557916df81477dc14808e79c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 24 Sep 2025 09:56:42 +0000 Subject: [PATCH 16/18] style Signed-off-by: Arjo Chakravarty --- src/gui/plugins/export_occupancy/ExportOccupancy.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc index c09c022b02..e7116c2f6a 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -201,7 +201,6 @@ void ExportOccupancyUi::StartExploration() void ExportOccupancyUi::Save() { auto last = this->dataPtr->lastOccupancy; - //auto pt = msgs::Convert(last); common::Image image; image.SetFromData( reinterpret_cast( @@ -212,9 +211,10 @@ void ExportOccupancyUi::Save() QString fileName = QFileDialog::getSaveFileName( nullptr, tr("Save File"), - QDir::homePath(), // Default directory + QDir::homePath(), tr("PNG Files (*.png);")); - if (fileName.isEmpty()) { + if (fileName.isEmpty()) + { return; } image.SavePNG(fileName.toStdString()); From 41022cc19b00635ba8f9217cac902e003d6623a2 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 25 Sep 2025 02:18:39 +0000 Subject: [PATCH 17/18] Bikeshedding Signed-off-by: Arjo Chakravarty --- src/gui/plugins/export_occupancy/ExportOccupancy.cc | 10 ++++++---- src/gui/plugins/export_occupancy/ExportOccupancy.hh | 10 ++++++---- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc index e7116c2f6a..a3122b2b1b 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -19,7 +19,6 @@ #include #include -#include #include #include #include @@ -30,6 +29,8 @@ #include "gz/sim/Util.hh" #include "gz/sim/World.hh" +#include +#include #include #include @@ -38,7 +39,7 @@ using namespace gz; using namespace sim; -class gz::sim::ExportOccupancyUiPrivate +class gz::sim::ExportOccupancyUi::Implementation { public: std::string worldName; public: gz::transport::Node node; @@ -48,7 +49,7 @@ class gz::sim::ExportOccupancyUiPrivate }; ExportOccupancyUi::ExportOccupancyUi() : - dataPtr(std::make_unique()) + dataPtr(gz::utils::MakeUniqueImpl()) { gui::App()->Engine()->rootContext()->setContextProperty( "exportOccupancy", this); @@ -99,7 +100,8 @@ void ExportOccupancyUi::Update(const UpdateInfo &, EntityComponentManager &_ecm) { auto world = World(worldEntity(_ecm)); - if (!world.Valid(_ecm)) { + if (!world.Valid(_ecm)) + { gzerr << "Could not get running world" << std::endl; return; } diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.hh b/src/gui/plugins/export_occupancy/ExportOccupancy.hh index edd509f877..55d618b432 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.hh +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.hh @@ -18,13 +18,16 @@ #ifndef GZ_SIM_GUI_EXPORT_OCCUPANCY_HH_ #define GZ_SIM_GUI_EXPORT_OCCUPANCY_HH_ +#include #include +#include #include +#include #include +#include #include "gz/sim/gui/GuiSystem.hh" -#include "gz/gui/qt.h" namespace gz { @@ -33,6 +36,7 @@ namespace sim // Inline bracket to help doxygen filtering. inline namespace GZ_SIM_VERSION_NAMESPACE { + /// Helper Image provider class for image view. class ImageProvider : public QQuickImageProvider { public: ImageProvider() @@ -63,8 +67,6 @@ inline namespace GZ_SIM_VERSION_NAMESPACE private: QImage img; }; - class ExportOccupancyUiPrivate; - /// \brief A GUI plugin for a user to export the occupancy /// grid of the current world. class ExportOccupancyUi : public gz::sim::GuiSystem @@ -106,7 +108,7 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \internal /// \brief Pointer to private data - private: std::unique_ptr dataPtr; + private: GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr); }; } } From 696aae2a0047fd9eb99d76f336a1fce6b185acd3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 29 Sep 2025 07:32:30 +0000 Subject: [PATCH 18/18] Fix type issues Signed-off-by: Arjo Chakravarty --- src/gui/plugins/export_occupancy/ExportOccupancy.cc | 2 +- src/gui/plugins/export_occupancy/ExportOccupancy.qml | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.cc b/src/gui/plugins/export_occupancy/ExportOccupancy.cc index a3122b2b1b..2405d0adab 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.cc @@ -122,7 +122,7 @@ void ExportOccupancyUi::StartExport(double _samples, double _range, ss << R"( - 0 0 )" << _distanceFromGround << R"( 0 0 0 + 1.5 1.5 )" << _distanceFromGround << R"( 0 0 0 0.1 diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qml b/src/gui/plugins/export_occupancy/ExportOccupancy.qml index 3e28f35d79..10f4703568 100644 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.qml +++ b/src/gui/plugins/export_occupancy/ExportOccupancy.qml @@ -157,10 +157,10 @@ ColumnLayout { id: occupancyGridHorizontalCellsInput Layout.fillWidth: true validator: IntValidator { bottom: 0 } - text: root.occupancyGridNumberOfHorizontalCells.toFixed(2) + text: root.occupancyGridNumberOfHorizontalCells onEditingFinished: { - root.occupancyGridNumberOfHorizontalCells = parseInt(text) - if (isNaN(root. root.occupancyGridNumberOfHorizontalCells))root.occupancyGridNumberOfHorizontalCells = 0 + root.occupancyGridNumberOfHorizontalCells = parseInt(text); + if (isNaN(root.occupancyGridNumberOfHorizontalCells)) root.occupancyGridNumberOfHorizontalCells = 0; } } } @@ -172,10 +172,10 @@ ColumnLayout { id: occupancyGridVerticalCellsInput Layout.fillWidth: true validator: IntValidator { bottom: 0 } - text: root.occupancyGridNumberOfVerticalCells.toFixed(2) + text: root.occupancyGridNumberOfVerticalCells onEditingFinished: { - root.occupancyGridNumberOfVerticalCells = parseInt(text) - if (isNaN(root. root.occupancyGridNumberOfVerticalCells)) root. root.occupancyGridNumberOfVerticalCells = 0 + root.occupancyGridNumberOfVerticalCells = parseInt(text); + if (isNaN(root.occupancyGridNumberOfVerticalCells)) root.occupancyGridNumberOfVerticalCells = 0; } } }