From 58361a52e1e0bc26c3c976952c3ef2963756a33b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 25 Jun 2025 09:00:47 +0000 Subject: [PATCH 01/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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/20] 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 c2585cee1bdbc1508ab733027eb8dcc2265b0628 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 22 Aug 2025 05:00:53 +0000 Subject: [PATCH 10/20] Update the bazel build script and add manual trigger Signed-off-by: Arjo Chakravarty --- BUILD.bazel | 20 ++++++++++++++++ examples/worlds/export_occupancy_grid.sdf | 4 ++++ .../free_space_explorer/FreeSpaceExplorer.cc | 24 +++++++++++++++++-- .../free_space_explorer/FreeSpaceExplorer.hh | 1 + 4 files changed, 47 insertions(+), 2 deletions(-) diff --git a/BUILD.bazel b/BUILD.bazel index 9d6d2d9bb9..01bc304ad8 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -801,6 +801,26 @@ gz_sim_system_libraries( ], ) +gz_sim_system_libraries( + srcs = glob( + [ + "src/systems/free_space_explorer/**/*.cc", + "src/systems/free_space_explorer/**/*.hh", + ], + ), + so_lib_name = "gz-sim-free-space_explorer--system.so", + static_lib_name = "gz-sim-free-space-explorer-system-static", + visibility = ["//visibility:public"], + deps = [ + ":gz-sim", + "@gz-common//graphics", + "@gz-math", + "@gz-msgs//:gzmsgs_cc_proto", + "@gz-plugin//:register", + "@gz-transport", + ], +) + gz_sim_system_libraries( srcs = glob( [ diff --git a/examples/worlds/export_occupancy_grid.sdf b/examples/worlds/export_occupancy_grid.sdf index 9bfb6d24c8..67374206da 100644 --- a/examples/worlds/export_occupancy_grid.sdf +++ b/examples/worlds/export_occupancy_grid.sdf @@ -2,6 +2,9 @@ This world demonstrates the FreeSpaceExplorer system plugin. A lidar will explore the world and generate an occupancy grid. The occupancy grid will be published as an image on the /scan_image topic. + + To start the exploration, run the following command in a new terminal: + gz topic -t /start_exploration -m gz.msgs.Boolean -p 'data: true' --> @@ -104,6 +107,7 @@ name="gz::sim::systems::FreeSpaceExplorer"> /scan /scan_image + /start_exploration 100 100 0.25 diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 1096b9b0c2..29cd6f01c7 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -15,10 +15,10 @@ * */ #include "FreeSpaceExplorer.hh" - #include #include #include +#include #include #include #include @@ -63,17 +63,29 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { double resolution; std::string scanTopic; std::string imageTopic; + std::string startTopic; gz::transport::Node::Publisher imagePub; std::string sensorLink; math::Pose3d position; gz::transport::Node node; bool recievedMessageForPose {false}; + bool explorationStarted {false}; std::queue nextPosition; std::unordered_set, PairHash> previouslyVisited; std::recursive_mutex m; + ///////////////////////////////////////////////// + /// \brief Callback for start message + void OnStartMsg(const msgs::Boolean &_scan) + { + if (_scan.data()) + { + this->explorationStarted = true; + } + } + ///////////////////////////////////////////////// /// \brief Perform search over occupancy grid to see if there are any /// reachable unknown cells and return their count. @@ -147,6 +159,11 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { return; } + if (!this->explorationStarted) + { + return; + } + double currAngle = _scan.angle_min(); /// Iterate through laser scan and mark bressenham line of free space @@ -338,12 +355,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->startTopic = _sdf->Get("start_topic", "start").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()); + this->dataPtr->node.Subscribe(this->dataPtr->startTopic, + &FreeSpaceExplorerPrivateData::OnStartMsg, this->dataPtr.get()); this->dataPtr->imagePub = this->dataPtr->node.Advertise(this->dataPtr->imageTopic); gzmsg << "Loaded lidar exploration plugin listening on [" << this->dataPtr->scanTopic << "]\n"; } @@ -382,7 +402,7 @@ void FreeSpaceExplorer::PreUpdate( } this->dataPtr->position = pose.value(); - if (this->dataPtr->nextPosition.empty()) + if (this->dataPtr->nextPosition.empty() || !this->dataPtr->explorationStarted) { return; } diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.hh b/src/systems/free_space_explorer/FreeSpaceExplorer.hh index faf0eaa354..bc54f0aa9b 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.hh +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.hh @@ -45,6 +45,7 @@ namespace systems /// * - Resolution of an individual cell /// * - Link on which the sensor is attached within the model. /// * - Topic to publish occupancy map on + /// * - Topic to listen on before starting exploration. class FreeSpaceExplorer: public gz::sim::System, public gz::sim::ISystemConfigure, From 6b4e6f9fa0e96bace692dbb3babf2701f09e857c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 22 Aug 2025 05:40:03 +0000 Subject: [PATCH 11/20] Remove GUI and fix style Signed-off-by: Arjo Chakravarty --- src/gui/plugins/CMakeLists.txt | 1 - .../plugins/export_occupancy/CMakeLists.txt | 8 - .../export_occupancy/ExportOccupancy.cc | 129 ------------- .../export_occupancy/ExportOccupancy.hh | 64 ------- .../export_occupancy/ExportOccupancy.qml | 181 ------------------ .../export_occupancy/ExportOccupancy.qrc | 5 - .../free_space_explorer/FreeSpaceExplorer.cc | 36 ++-- 7 files changed, 24 insertions(+), 400 deletions(-) delete mode 100644 src/gui/plugins/export_occupancy/CMakeLists.txt delete mode 100644 src/gui/plugins/export_occupancy/ExportOccupancy.cc delete mode 100644 src/gui/plugins/export_occupancy/ExportOccupancy.hh delete mode 100644 src/gui/plugins/export_occupancy/ExportOccupancy.qml delete mode 100644 src/gui/plugins/export_occupancy/ExportOccupancy.qrc diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 0e7dd8e669..e510383df3 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -140,7 +140,6 @@ 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) diff --git a/src/gui/plugins/export_occupancy/CMakeLists.txt b/src/gui/plugins/export_occupancy/CMakeLists.txt deleted file mode 100644 index fc1e4144fc..0000000000 --- a/src/gui/plugins/export_occupancy/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -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 deleted file mode 100644 index 008ad55910..0000000000 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.cc +++ /dev/null @@ -1,129 +0,0 @@ -/* - * 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 -#include -#include -#include - -#include - -using namespace gz; -using namespace sim; - -class gz::sim::ExportOccupancyUiPrivate -{ -}; - -ExportOccupancyUi::ExportOccupancyUi() : dataPtr(std::make_unique()) -{ - gui::App()->Engine()->rootContext()->setContextProperty( - "exportOccupancy", this); -} - -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 &, - EntityComponentManager &_ecm) -{ - -} - -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 deleted file mode 100644 index fa8a17df3e..0000000000 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.hh +++ /dev/null @@ -1,64 +0,0 @@ -/* - * 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; - - /// 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; - }; -} -} -} -#endif diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qml b/src/gui/plugins/export_occupancy/ExportOccupancy.qml deleted file mode 100644 index e6a0b09a89..0000000000 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.qml +++ /dev/null @@ -1,181 +0,0 @@ -/* - * 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" - -Item { - id: root - anchors.fill: parent - - 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.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 - } - } - - // 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.occupancyGridNumberOfVerticalCells - onValueChanged: root.occupancyGridNumberOfVerticalCells = 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) - exportOccupancy.StartExport( - root.lidarSamples, root.lidarRange, root.lidarRangeResolution, root.lidarAngularResolution, - root.distanceFromLidarToGround, root.occupancyGridCellResolution, root.occupancyGridNumberOfHorizontalCells, - root.occupancyGridNumberOfVerticalCells); - } - } - } - } -} diff --git a/src/gui/plugins/export_occupancy/ExportOccupancy.qrc b/src/gui/plugins/export_occupancy/ExportOccupancy.qrc deleted file mode 100644 index 64c227f3d7..0000000000 --- a/src/gui/plugins/export_occupancy/ExportOccupancy.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - ExportOccupancy.qml - - diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 29cd6f01c7..6cfc8e692f 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -353,19 +353,28 @@ void FreeSpaceExplorer::Configure( gz::sim::EventManager &/*_eventMgr*/) { 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->startTopic = _sdf->Get("start_topic", "start").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->scanTopic = + _sdf->Get("lidar_topic", "scan").first; + this->dataPtr->imageTopic = + _sdf->Get("image_topic", "scan_image").first; + this->dataPtr->startTopic = + _sdf->Get("start_topic", "start").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()); this->dataPtr->node.Subscribe(this->dataPtr->startTopic, &FreeSpaceExplorerPrivateData::OnStartMsg, this->dataPtr.get()); - this->dataPtr->imagePub = this->dataPtr->node.Advertise(this->dataPtr->imageTopic); - gzmsg << "Loaded lidar exploration 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"; } ///////////////////////////////////////////////// @@ -378,7 +387,8 @@ void FreeSpaceExplorer::PreUpdate( return; } //TODO(arjo) check link name valisdity - auto l = Link(this->dataPtr->model.LinkByName(_ecm, this->dataPtr->sensorLink)); + auto l = + Link(this->dataPtr->model.LinkByName(_ecm, this->dataPtr->sensorLink)); if (!l.Valid(_ecm)){ gzerr << "Invalid link name " << this->dataPtr->sensorLink << std::endl; return; @@ -392,8 +402,10 @@ void FreeSpaceExplorer::PreUpdate( const std::lock_guard 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; + 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); From 96896871d464cf526a425daf91832c7f76ecf5f1 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 22 Aug 2025 05:49:44 +0000 Subject: [PATCH 12/20] Style Signed-off-by: Arjo Chakravarty --- .../free_space_explorer/FreeSpaceExplorer.cc | 16 +++++++--------- .../free_space_explorer/FreeSpaceExplorer.hh | 3 ++- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 6cfc8e692f..e8ccc1ce9b 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -49,10 +49,7 @@ struct PairHash { 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 + return h1 ^ (h2 << 1); } }; @@ -72,7 +69,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { bool recievedMessageForPose {false}; bool explorationStarted {false}; std::queue nextPosition; - std::unordered_set, PairHash> previouslyVisited; + std::unordered_set, PairHash> previouslyVisited; std::recursive_mutex m; @@ -175,7 +172,8 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { 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); + this->grid->MarkFree(this->position.Pos().X(), + this->position.Pos().Y(), toX, toY); if (obstacleExists) this->grid->MarkOccupied(toX, toY); @@ -219,7 +217,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { imageMsg.set_step(this->grid->Width() * 3); imageMsg.set_data(pixelData.data(), pixelData.size()); this->imagePub.Publish(imageMsg); - //gzmsg << "Scan complete\n"; } ///////////////////////////////////////////////// @@ -264,7 +261,7 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { auto x = pt.first + i; auto y = pt.second + j; - if(visited.count(std::make_pair(x,y)) > 0) + if(visited.count(std::make_pair(x, y)) > 0) { continue; } @@ -305,7 +302,8 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { } /// Scores information gain given laser scan parameters - std::optional ScoreInfoGain(int _x, int _y, const msgs::LaserScan &_scan) + std::optional ScoreInfoGain(int _x, int _y, + const msgs::LaserScan &_scan) { const std::lock_guard lock(this->m); if (!this->grid.has_value()) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.hh b/src/systems/free_space_explorer/FreeSpaceExplorer.hh index bc54f0aa9b..e4e223f354 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.hh +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.hh @@ -43,7 +43,8 @@ namespace systems /// * - 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. + /// * - Link on which the sensor is attached within the + /// model. /// * - Topic to publish occupancy map on /// * - Topic to listen on before starting exploration. class FreeSpaceExplorer: From 8f602a61a57151e43e020a3581b619eb3a040c02 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 22 Aug 2025 09:32:05 +0000 Subject: [PATCH 13/20] Style Signed-off-by: Arjo Chakravarty --- src/systems/free_space_explorer/FreeSpaceExplorer.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index e8ccc1ce9b..9cc15ee87d 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -384,7 +384,6 @@ void FreeSpaceExplorer::PreUpdate( { return; } - //TODO(arjo) check link name valisdity auto l = Link(this->dataPtr->model.LinkByName(_ecm, this->dataPtr->sensorLink)); if (!l.Valid(_ecm)){ From 79bc40084df288ac42a6f8249ebce867285cc776 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 23 Aug 2025 13:50:43 +0000 Subject: [PATCH 14/20] Address Reviewer Feedback AI was used to migrate the logic in OnLaserScanCallback. Generated-by: Gemini-2.5-Pro Signed-off-by: Arjo Chakravarty --- src/gui/plugins/CMakeLists.txt | 2 +- .../free_space_explorer/FreeSpaceExplorer.cc | 234 +++++++++++------- .../free_space_explorer/FreeSpaceExplorer.hh | 12 +- 3 files changed, 157 insertions(+), 91 deletions(-) diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index e510383df3..0def78d3a0 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) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 9cc15ee87d..f40f08891c 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -53,26 +54,56 @@ struct PairHash { } }; +/// \brief Private data pointer that performs actual +/// exploration. struct gz::sim::systems::FreeSpaceExplorerPrivateData { + + /// \brief Occupancy Grid function std::optional grid; + + /// \brief Model of the parent sensor that gets + /// moved around. Model model; + + /// \brief Number of rows and columns the occupancy grid should + /// have. std::size_t numRows, numCols; + + /// \brief Resolution parameter for the occupancy grid double resolution; - std::string scanTopic; - std::string imageTopic; - std::string startTopic; + + /// \brief Image publisher for preview image of occupancy grid gz::transport::Node::Publisher imagePub; + + /// \brief Sensor link std::string sensorLink; + + /// \brief The starting position of the occupancy grid. math::Pose3d position; + + /// \brief GZ-Transport node gz::transport::Node node; - bool recievedMessageForPose {false}; - bool explorationStarted {false}; + /// \brief The signal to enable exploration + std::atomic explorationStarted {false}; + + /// \brief The next position to move the sensor to increase + /// coverage. std::queue nextPosition; - std::unordered_set, PairHash> previouslyVisited; + /// \brief Previously visited locations + std::unordered_set, PairHash> + previouslyVisited; + + /// \brief Mutex to protect data in this structure. std::recursive_mutex m; + /// \brief Laser scan message queue + std::queue laserScanMsgs; + + /// \brief Mutex for the laser scan queue + std::mutex laserScanMutex; + ///////////////////////////////////////////////// /// \brief Callback for start message void OnStartMsg(const msgs::Boolean &_scan) @@ -149,74 +180,8 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { /// 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; - } - - if (!this->explorationStarted) - { - 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 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(); - } - - if (!this->nextPosition.empty()) - { - return; - } - - if (this->CountReachableUnknowns() == 0) - { - 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: No reachable unknown cells.\n"; - return; - } - - auto nextPos = this->GetNextPoint(_scan); - if(nextPos.has_value()) - { - gzmsg << "Setting next position " << nextPos->Pos() <nextPosition.push(nextPos.value()); - } - - 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); + std::lock_guard lock(this->laserScanMutex); + this->laserScanMsgs.push(_scan); } ///////////////////////////////////////////////// @@ -237,7 +202,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" << std::endl; + gzerr << "Proposed point outside of bounds" <dataPtr->model = gz::sim::Model(_entity); - this->dataPtr->scanTopic = + auto scanTopic = _sdf->Get("lidar_topic", "scan").first; - this->dataPtr->imageTopic = + auto imageTopic = _sdf->Get("image_topic", "scan_image").first; - this->dataPtr->startTopic = + auto startTopic = _sdf->Get("start_topic", "start").first; this->dataPtr->sensorLink = _sdf->Get("sensor_link", "link").first; @@ -365,14 +330,14 @@ void FreeSpaceExplorer::Configure( _sdf->Get("height", 10).first; this->dataPtr->resolution = _sdf->Get("resolution", 1.0).first; - this->dataPtr->node.Subscribe(this->dataPtr->scanTopic, + this->dataPtr->node.Subscribe(scanTopic, &FreeSpaceExplorerPrivateData::OnLaserScanMsg, this->dataPtr.get()); - this->dataPtr->node.Subscribe(this->dataPtr->startTopic, + this->dataPtr->node.Subscribe(startTopic, &FreeSpaceExplorerPrivateData::OnStartMsg, this->dataPtr.get()); this->dataPtr->imagePub = - this->dataPtr->node.Advertise(this->dataPtr->imageTopic); + this->dataPtr->node.Advertise(imageTopic); gzmsg << "Loaded lidar exploration plugin listening on [" - << this->dataPtr->scanTopic << "]\n"; + << scanTopic << "] for lidar messages\n"; } ///////////////////////////////////////////////// @@ -384,28 +349,28 @@ void FreeSpaceExplorer::PreUpdate( { return; } - auto l = + auto link = Link(this->dataPtr->model.LinkByName(_ecm, this->dataPtr->sensorLink)); - if (!l.Valid(_ecm)){ + if (!link.Valid(_ecm)){ gzerr << "Invalid link name " << this->dataPtr->sensorLink << std::endl; return; } - auto pose = l.WorldPose(_ecm); + auto pose = link.WorldPose(_ecm); if (!pose.has_value()) { - l.EnableVelocityChecks(_ecm); + link.EnableVelocityChecks(_ecm); return; } const std::lock_guard lock(this->dataPtr->m); if (!this->dataPtr->grid.has_value()) { - auto center_x = + auto centerX = pose->Pos().X() - this->dataPtr->numRows * this->dataPtr->resolution / 2; - auto center_y = + auto centerY = 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); + centerX, centerY); this->dataPtr->grid = {std::move(g)}; this->dataPtr->position = pose.value(); } @@ -421,10 +386,101 @@ void FreeSpaceExplorer::PreUpdate( this->dataPtr->model.SetWorldPoseCmd(_ecm, modelPosCmd); } +///////////////////////////////////////////////// +void FreeSpaceExplorer::PostUpdate( + const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &/*_ecm*/) +{ + if (_info.paused) + { + return; + } + + std::lock_guard lock(this->dataPtr->laserScanMutex); + if (this->dataPtr->laserScanMsgs.empty()) + { + return; + } + + auto scan = this->dataPtr->laserScanMsgs.front(); + this->dataPtr->laserScanMsgs.pop(); + + const std::lock_guard lock2(this->dataPtr->m); + if (!this->dataPtr->grid.has_value()) + { + gzerr<< "Grid not yet inited"; + return; + } + + if (!this->dataPtr->explorationStarted) + { + 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 toX = length * cos(currAngle) + this->dataPtr->position.Pos().X(); + auto toY = length * sin(currAngle) + this->dataPtr->position.Pos().Y(); + + this->dataPtr->grid->MarkFree(this->dataPtr->position.Pos().X(), + this->dataPtr->position.Pos().Y(), toX, toY); + + if (obstacleExists) + this->dataPtr->grid->MarkOccupied(toX, toY); + + currAngle += scan.angle_step(); + } + + if (!this->dataPtr->nextPosition.empty()) + { + return; + } + + if (this->dataPtr->CountReachableUnknowns() == 0) + { + std::vector pixelData; + this->dataPtr->grid->ExportToRGBImage(pixelData); + gz::msgs::Image imageMsg; + imageMsg.set_width(this->dataPtr->grid->Width()); + imageMsg.set_height(this->dataPtr->grid->Height()); + imageMsg.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8); + imageMsg.set_step(this->dataPtr->grid->Width() * 3); + imageMsg.set_data(pixelData.data(), pixelData.size()); + this->dataPtr->imagePub.Publish(imageMsg); + gzmsg << "Scan complete: No reachable unknown cells.\n"; + return; + } + + auto nextPos = this->dataPtr->GetNextPoint(scan); + if(nextPos.has_value()) + { + gzmsg << "Setting next position " << nextPos->Pos() <dataPtr->nextPosition.push(nextPos.value()); + } + + std::vector pixelData; + this->dataPtr->grid->ExportToRGBImage(pixelData); + gz::msgs::Image imageMsg; + imageMsg.set_width(this->dataPtr->grid->Width()); + imageMsg.set_height(this->dataPtr->grid->Height()); + imageMsg.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8); + imageMsg.set_step(this->dataPtr->grid->Width() * 3); + imageMsg.set_data(pixelData.data(), pixelData.size()); + this->dataPtr->imagePub.Publish(imageMsg); +} + + GZ_ADD_PLUGIN( FreeSpaceExplorer, gz::sim::System, FreeSpaceExplorer::ISystemPreUpdate, + FreeSpaceExplorer::ISystemPostUpdate, FreeSpaceExplorer::ISystemConfigure) GZ_ADD_PLUGIN_ALIAS( diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.hh b/src/systems/free_space_explorer/FreeSpaceExplorer.hh index e4e223f354..f4e3fa4962 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.hh +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.hh @@ -47,10 +47,15 @@ namespace systems /// model. /// * - Topic to publish occupancy map on /// * - Topic to listen on before starting exploration. + /// + /// Note: It is assumed the sensor does not implement any pose + /// offset from the link itself. This may of may not be true. + /// and should be fixed in a future release. class FreeSpaceExplorer: public gz::sim::System, public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate { /// \brief Constructor public: FreeSpaceExplorer(); @@ -70,6 +75,11 @@ namespace systems const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override; + // Documentation inherited + public: void PostUpdate( + const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) override; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; From be306ebe1ed1576dd31429be5a6817fe5288836a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 23 Aug 2025 13:54:52 +0000 Subject: [PATCH 15/20] Aaaand this is why I still have a job Signed-off-by: Arjo Chakravarty --- src/systems/free_space_explorer/FreeSpaceExplorer.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index f40f08891c..6bac3129de 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -180,6 +180,10 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { /// Callback for laser scan message void OnLaserScanMsg(const msgs::LaserScan &_scan) { + if (!this->explorationStarted) + { + return; + } std::lock_guard lock(this->laserScanMutex); this->laserScanMsgs.push(_scan); } @@ -412,10 +416,7 @@ void FreeSpaceExplorer::PostUpdate( return; } - if (!this->dataPtr->explorationStarted) - { - return; - } + double currAngle = scan.angle_min(); From 94be81dd2bf3d9f6977c11d9e260deb9a2ede6bb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 23 Aug 2025 14:00:37 +0000 Subject: [PATCH 16/20] Revert unintended changes in gui cmake 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 0def78d3a0..e510383df3 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) add_subdirectory(component_inspector_editor) +add_subdirectory(component_inspector) add_subdirectory(copy_paste) add_subdirectory(entity_context_menu) add_subdirectory(entity_tree) From d6e9faa664cd3140c48dbbe660b8561256c00cbf Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 23 Aug 2025 14:05:26 +0000 Subject: [PATCH 17/20] Remove mutex Since we are accessing the occupancy grid on the main simulation loop only now, we dont need a mutex to protect it. Signed-off-by: Arjo Chakravarty --- src/systems/free_space_explorer/FreeSpaceExplorer.cc | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 6bac3129de..c7df9e3857 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -95,9 +95,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { std::unordered_set, PairHash> previouslyVisited; - /// \brief Mutex to protect data in this structure. - std::recursive_mutex m; - /// \brief Laser scan message queue std::queue laserScanMsgs; @@ -119,7 +116,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { /// 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; @@ -193,7 +189,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { /// explore. std::optional GetNextPoint(const msgs::LaserScan &_scan) { - const std::lock_guard lock(this->m); if (!this->grid.has_value()) { gzerr << "Grid not yet inited" << std::endl; @@ -274,7 +269,6 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { std::optional ScoreInfoGain(int _x, int _y, const msgs::LaserScan &_scan) { - const std::lock_guard lock(this->m); if (!this->grid.has_value()) { gzerr<< "Waiting for occupancy grid to be initialized." << std::endl; @@ -365,7 +359,6 @@ void FreeSpaceExplorer::PreUpdate( return; } - const std::lock_guard lock(this->dataPtr->m); if (!this->dataPtr->grid.has_value()) { auto centerX = @@ -409,7 +402,6 @@ void FreeSpaceExplorer::PostUpdate( auto scan = this->dataPtr->laserScanMsgs.front(); this->dataPtr->laserScanMsgs.pop(); - const std::lock_guard lock2(this->dataPtr->m); if (!this->dataPtr->grid.has_value()) { gzerr<< "Grid not yet inited"; From e8c23ef81103c967e1de1e3a0188939d2487489a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 23 Aug 2025 14:13:17 +0000 Subject: [PATCH 18/20] Revert unessecary 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 e510383df3..0def78d3a0 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 915efabc27aec82bc0369ac16fb8bc9ebfe6d271 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 23 Aug 2025 14:29:22 +0000 Subject: [PATCH 19/20] Move to GZ_UTILS_UNIQUE_IMPL_PTR Signed-off-by: Arjo Chakravarty --- src/systems/free_space_explorer/FreeSpaceExplorer.cc | 12 ++++++------ src/systems/free_space_explorer/FreeSpaceExplorer.hh | 7 ++----- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index c7df9e3857..41e6df06b1 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -56,7 +56,7 @@ struct PairHash { /// \brief Private data pointer that performs actual /// exploration. -struct gz::sim::systems::FreeSpaceExplorerPrivateData { +struct gz::sim::systems::FreeSpaceExplorer::Implementation { /// \brief Occupancy Grid function std::optional grid; @@ -295,10 +295,10 @@ struct gz::sim::systems::FreeSpaceExplorerPrivateData { }; ///////////////////////////////////////////////// -FreeSpaceExplorer::FreeSpaceExplorer() +FreeSpaceExplorer::FreeSpaceExplorer(): + dataPtr( + std::move(gz::utils::MakeUniqueImpl())) { - - this->dataPtr = std::make_unique(); } ///////////////////////////////////////////////// @@ -329,9 +329,9 @@ void FreeSpaceExplorer::Configure( this->dataPtr->resolution = _sdf->Get("resolution", 1.0).first; this->dataPtr->node.Subscribe(scanTopic, - &FreeSpaceExplorerPrivateData::OnLaserScanMsg, this->dataPtr.get()); + &FreeSpaceExplorer::Implementation::OnLaserScanMsg, this->dataPtr.get()); this->dataPtr->node.Subscribe(startTopic, - &FreeSpaceExplorerPrivateData::OnStartMsg, this->dataPtr.get()); + &FreeSpaceExplorer::Implementation::OnStartMsg, this->dataPtr.get()); this->dataPtr->imagePub = this->dataPtr->node.Advertise(imageTopic); gzmsg << "Loaded lidar exploration plugin listening on [" diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.hh b/src/systems/free_space_explorer/FreeSpaceExplorer.hh index f4e3fa4962..c4097ead54 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.hh +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.hh @@ -19,8 +19,7 @@ #include -#include -#include +#include namespace gz { @@ -30,8 +29,6 @@ namespace sim 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. @@ -81,7 +78,7 @@ namespace systems const gz::sim::EntityComponentManager &_ecm) override; /// \brief Private data pointer - private: std::unique_ptr dataPtr; + private: GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } } From 423ce404c2a51cfc49bdea3aaca9423d795acbdb Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Sat, 23 Aug 2025 14:33:53 -0500 Subject: [PATCH 20/20] Fix compiler errors Signed-off-by: Addisu Z. Taddese --- .../free_space_explorer/FreeSpaceExplorer.cc | 48 +++++++++---------- .../free_space_explorer/FreeSpaceExplorer.hh | 6 ++- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.cc b/src/systems/free_space_explorer/FreeSpaceExplorer.cc index 41e6df06b1..4b51de406f 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.cc +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc @@ -56,54 +56,53 @@ struct PairHash { /// \brief Private data pointer that performs actual /// exploration. -struct gz::sim::systems::FreeSpaceExplorer::Implementation { +class gz::sim::systems::FreeSpaceExplorer::Implementation { /// \brief Occupancy Grid function - std::optional grid; + public: std::optional grid; /// \brief Model of the parent sensor that gets /// moved around. - Model model; + public: Model model{}; /// \brief Number of rows and columns the occupancy grid should /// have. - std::size_t numRows, numCols; + public: std::size_t numRows, numCols; /// \brief Resolution parameter for the occupancy grid - double resolution; + public: double resolution; /// \brief Image publisher for preview image of occupancy grid - gz::transport::Node::Publisher imagePub; + public: gz::transport::Node::Publisher imagePub; /// \brief Sensor link - std::string sensorLink; + public: std::string sensorLink; /// \brief The starting position of the occupancy grid. - math::Pose3d position; + public: math::Pose3d position; /// \brief GZ-Transport node - gz::transport::Node node; + public: gz::transport::Node node; /// \brief The signal to enable exploration - std::atomic explorationStarted {false}; + public: std::atomic explorationStarted{false}; /// \brief The next position to move the sensor to increase /// coverage. - std::queue nextPosition; + public: std::queue nextPosition; /// \brief Previously visited locations - std::unordered_set, PairHash> - previouslyVisited; + public: std::unordered_set, PairHash> previouslyVisited; /// \brief Laser scan message queue - std::queue laserScanMsgs; + public: std::queue laserScanMsgs; /// \brief Mutex for the laser scan queue - std::mutex laserScanMutex; + public: std::mutex laserScanMutex; ///////////////////////////////////////////////// /// \brief Callback for start message - void OnStartMsg(const msgs::Boolean &_scan) + public: void OnStartMsg(const msgs::Boolean &_scan) { if (_scan.data()) { @@ -114,7 +113,7 @@ struct gz::sim::systems::FreeSpaceExplorer::Implementation { ///////////////////////////////////////////////// /// \brief Perform search over occupancy grid to see if there are any /// reachable unknown cells and return their count. - int CountReachableUnknowns() + public: int CountReachableUnknowns() { if (!this->grid.has_value()) { @@ -174,7 +173,7 @@ struct gz::sim::systems::FreeSpaceExplorer::Implementation { ///////////////////////////////////////////////// /// Callback for laser scan message - void OnLaserScanMsg(const msgs::LaserScan &_scan) + public: void OnLaserScanMsg(const msgs::LaserScan &_scan) { if (!this->explorationStarted) { @@ -187,7 +186,7 @@ struct gz::sim::systems::FreeSpaceExplorer::Implementation { ///////////////////////////////////////////////// /// Perform search over occupancy grid for next position to /// explore. - std::optional GetNextPoint(const msgs::LaserScan &_scan) + public: std::optional GetNextPoint(const msgs::LaserScan &_scan) { if (!this->grid.has_value()) { @@ -266,8 +265,8 @@ struct gz::sim::systems::FreeSpaceExplorer::Implementation { } /// Scores information gain given laser scan parameters - std::optional ScoreInfoGain(int _x, int _y, - const msgs::LaserScan &_scan) + public: std::optional ScoreInfoGain( + int _x, int _y, const msgs::LaserScan &_scan) { if (!this->grid.has_value()) { @@ -296,8 +295,7 @@ struct gz::sim::systems::FreeSpaceExplorer::Implementation { ///////////////////////////////////////////////// FreeSpaceExplorer::FreeSpaceExplorer(): - dataPtr( - std::move(gz::utils::MakeUniqueImpl())) + dataPtr(gz::utils::MakeUniqueImpl()) { } @@ -323,9 +321,9 @@ void FreeSpaceExplorer::Configure( this->dataPtr->sensorLink = _sdf->Get("sensor_link", "link").first; this->dataPtr->numRows = - _sdf->Get("width", 10).first; + _sdf->Get("width", 10).first; this->dataPtr->numCols = - _sdf->Get("height", 10).first; + _sdf->Get("height", 10).first; this->dataPtr->resolution = _sdf->Get("resolution", 1.0).first; this->dataPtr->node.Subscribe(scanTopic, diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.hh b/src/systems/free_space_explorer/FreeSpaceExplorer.hh index c4097ead54..68f02b03ca 100644 --- a/src/systems/free_space_explorer/FreeSpaceExplorer.hh +++ b/src/systems/free_space_explorer/FreeSpaceExplorer.hh @@ -17,9 +17,11 @@ #ifndef GZ_SIM_SYSTEMS_FREESPACEEXPLORER_HH_ #define GZ_SIM_SYSTEMS_FREESPACEEXPLORER_HH_ -#include - #include +#include + +#include "gz/sim/System.hh" +#include "gz/sim/config.hh" namespace gz {