diff --git a/BUILD.bazel b/BUILD.bazel
index 97ebf7d774..a9d3c897ea 100644
--- a/BUILD.bazel
+++ b/BUILD.bazel
@@ -821,6 +821,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
new file mode 100644
index 0000000000..67374206da
--- /dev/null
+++ b/examples/worlds/export_occupancy_grid.sdf
@@ -0,0 +1,183 @@
+
+
+
+
+
+ /scan_image
+
+
+
+ 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
+ /scan_image
+ /start_exploration
+ 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/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt
index 27603e74e2..0def78d3a0 100644
--- a/src/gui/plugins/CMakeLists.txt
+++ b/src/gui/plugins/CMakeLists.txt
@@ -149,8 +149,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 +158,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/systems/CMakeLists.txt b/src/systems/CMakeLists.txt
index 597cbd9574..84255b30ff 100644
--- a/src/systems/CMakeLists.txt
+++ b/src/systems/CMakeLists.txt
@@ -104,6 +104,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..4b51de406f
--- /dev/null
+++ b/src/systems/free_space_explorer/FreeSpaceExplorer.cc
@@ -0,0 +1,479 @@
+/*
+ * 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
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#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);
+ return h1 ^ (h2 << 1);
+ }
+};
+
+/// \brief Private data pointer that performs actual
+/// exploration.
+class gz::sim::systems::FreeSpaceExplorer::Implementation {
+
+ /// \brief Occupancy Grid function
+ public: std::optional grid;
+
+ /// \brief Model of the parent sensor that gets
+ /// moved around.
+ public: Model model{};
+
+ /// \brief Number of rows and columns the occupancy grid should
+ /// have.
+ public: std::size_t numRows, numCols;
+
+ /// \brief Resolution parameter for the occupancy grid
+ public: double resolution;
+
+ /// \brief Image publisher for preview image of occupancy grid
+ public: gz::transport::Node::Publisher imagePub;
+
+ /// \brief Sensor link
+ public: std::string sensorLink;
+
+ /// \brief The starting position of the occupancy grid.
+ public: math::Pose3d position;
+
+ /// \brief GZ-Transport node
+ public: gz::transport::Node node;
+
+ /// \brief The signal to enable exploration
+ public: std::atomic explorationStarted{false};
+
+ /// \brief The next position to move the sensor to increase
+ /// coverage.
+ public: std::queue nextPosition;
+
+ /// \brief Previously visited locations
+ public: std::unordered_set, PairHash> previouslyVisited;
+
+ /// \brief Laser scan message queue
+ public: std::queue laserScanMsgs;
+
+ /// \brief Mutex for the laser scan queue
+ public: std::mutex laserScanMutex;
+
+ /////////////////////////////////////////////////
+ /// \brief Callback for start message
+ public: 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.
+ public: int CountReachableUnknowns()
+ {
+ 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->CellState(x, y);
+ if (cellState == math::OccupancyCellState::Free)
+ {
+ q.emplace(neighbor);
+ }
+ else if (cellState == math::OccupancyCellState::Unknown)
+ {
+ unknownCellCount++;
+ }
+ visited.emplace(neighbor);
+ }
+ }
+ }
+ return unknownCellCount;
+ }
+
+ /////////////////////////////////////////////////
+ /// Callback for laser scan message
+ public: void OnLaserScanMsg(const msgs::LaserScan &_scan)
+ {
+ if (!this->explorationStarted)
+ {
+ return;
+ }
+ std::lock_guard lock(this->laserScanMutex);
+ this->laserScanMsgs.push(_scan);
+ }
+
+ /////////////////////////////////////////////////
+ /// Perform search over occupancy grid for next position to
+ /// explore.
+ public: std::optional GetNextPoint(const msgs::LaserScan &_scan)
+ {
+ if (!this->grid.has_value())
+ {
+ gzerr << "Grid not yet inited" << std::endl;
+ 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" < 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->CellState(x, y) == math::OccupancyCellState::Free)
+ {
+
+ auto infoGain = this->ScoreInfoGain(x, y, _scan);
+ if (infoGain.has_value() && infoGain > maxInfoGain
+ && previouslyVisited.count(std::make_pair(x, y)) == 0)
+ {
+ bestGain = std::make_pair(x, y);
+ maxInfoGain = infoGain.value();
+ }
+
+ visited.emplace(x, y);
+ q.emplace(x, y);
+ }
+ }
+ }
+ }
+
+ if (maxInfoGain < 1)
+ {
+ return std::nullopt;
+ }
+
+ previouslyVisited.emplace(bestGain);
+
+ double newX, newY;
+ this->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
+ public: std::optional ScoreInfoGain(
+ int _x, int _y, const msgs::LaserScan &_scan)
+ {
+ if (!this->grid.has_value())
+ {
+ gzerr<< "Waiting for occupancy grid to be initialized." << std::endl;
+ return {};
+ }
+
+ double currAngle = _scan.angle_min();
+ 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():
+ dataPtr(gz::utils::MakeUniqueImpl())
+{
+}
+
+/////////////////////////////////////////////////
+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);
+ auto scanTopic =
+ _sdf->Get("lidar_topic", "scan").first;
+ auto imageTopic =
+ _sdf->Get("image_topic", "scan_image").first;
+ auto 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(scanTopic,
+ &FreeSpaceExplorer::Implementation::OnLaserScanMsg, this->dataPtr.get());
+ this->dataPtr->node.Subscribe(startTopic,
+ &FreeSpaceExplorer::Implementation::OnStartMsg, this->dataPtr.get());
+ this->dataPtr->imagePub =
+ this->dataPtr->node.Advertise(imageTopic);
+ gzmsg << "Loaded lidar exploration plugin listening on ["
+ << scanTopic << "] for lidar messages\n";
+}
+
+/////////////////////////////////////////////////
+void FreeSpaceExplorer::PreUpdate(
+ const gz::sim::UpdateInfo &_info,
+ gz::sim::EntityComponentManager &_ecm)
+{
+ if (_info.paused)
+ {
+ return;
+ }
+ auto link =
+ Link(this->dataPtr->model.LinkByName(_ecm, this->dataPtr->sensorLink));
+ if (!link.Valid(_ecm)){
+ gzerr << "Invalid link name " << this->dataPtr->sensorLink << std::endl;
+ return;
+ }
+ auto pose = link.WorldPose(_ecm);
+ if (!pose.has_value()) {
+ link.EnableVelocityChecks(_ecm);
+ return;
+ }
+
+ if (!this->dataPtr->grid.has_value())
+ {
+ auto centerX =
+ pose->Pos().X() - this->dataPtr->numRows * this->dataPtr->resolution / 2;
+ auto centerY =
+ pose->Pos().Y() - this->dataPtr->numCols * this->dataPtr->resolution / 2;
+ math::OccupancyGrid g(
+ this->dataPtr->resolution, this->dataPtr->numRows, this->dataPtr->numCols,
+ centerX, centerY);
+ this->dataPtr->grid = {std::move(g)};
+ this->dataPtr->position = pose.value();
+ }
+ this->dataPtr->position = pose.value();
+
+ if (this->dataPtr->nextPosition.empty() || !this->dataPtr->explorationStarted)
+ {
+ return;
+ }
+
+ auto modelPosCmd = this->dataPtr->nextPosition.front();
+ this->dataPtr->nextPosition.pop();
+ 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();
+
+ if (!this->dataPtr->grid.has_value())
+ {
+ gzerr<< "Grid not yet inited";
+ return;
+ }
+
+
+
+ double currAngle = scan.angle_min();
+
+ /// Iterate through laser scan and mark bressenham line of free space
+ for (uint32_t index = 0; index < scan.count(); index++)
+ {
+ auto length = scan.ranges(index);
+ 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(
+ FreeSpaceExplorer,
+ "gz::sim::systems::FreeSpaceExplorer")
diff --git a/src/systems/free_space_explorer/FreeSpaceExplorer.hh b/src/systems/free_space_explorer/FreeSpaceExplorer.hh
new file mode 100644
index 0000000000..68f02b03ca
--- /dev/null
+++ b/src/systems/free_space_explorer/FreeSpaceExplorer.hh
@@ -0,0 +1,89 @@
+/*
+ * 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
+
+#include "gz/sim/System.hh"
+#include "gz/sim/config.hh"
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems
+{
+ /// \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.
+ /// * - 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::ISystemPostUpdate
+ {
+ /// \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;
+
+ // Documentation inherited
+ public: void PostUpdate(
+ const gz::sim::UpdateInfo &_info,
+ const gz::sim::EntityComponentManager &_ecm) override;
+
+ /// \brief Private data pointer
+ private: GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr)
+ };
+}
+}
+}
+}
+#endif