diff --git a/include/q_simulation_interfaces/service_discovery.h b/include/q_simulation_interfaces/service_discovery.h
index 9259b90..d0f9e0b 100644
--- a/include/q_simulation_interfaces/service_discovery.h
+++ b/include/q_simulation_interfaces/service_discovery.h
@@ -43,6 +43,10 @@ namespace q_simulation_interfaces
SERVICE_STEP_SIMULATION,
SERVICE_GET_SIM_STATE,
SERVICE_SET_SIM_STATE,
+ SERVICE_GET_CURRENT_WORLD,
+ SERVICE_GET_AVAILABLE_WORLDS,
+ SERVICE_LOAD_WORLD,
+ SERVICE_UNLOAD_WORLD,
ACTION_SIMULATE_STEPS,
SUPPORTED_SERVICE_IDL_COUNT
};
@@ -75,6 +79,12 @@ namespace q_simulation_interfaces
false},
{ServiceType::SERVICE_SET_SIM_STATE, "simulation_interfaces/srv/SetSimulationState", "Set Sim State",
false},
+ {ServiceType::SERVICE_GET_CURRENT_WORLD, "simulation_interfaces/srv/GetCurrentWorld", "Get Current World",
+ false},
+ {ServiceType::SERVICE_GET_AVAILABLE_WORLDS, "simulation_interfaces/srv/GetAvailableWorlds",
+ "Get Available Worlds", false},
+ {ServiceType::SERVICE_LOAD_WORLD, "simulation_interfaces/srv/LoadWorld", "Load World", false},
+ {ServiceType::SERVICE_UNLOAD_WORLD, "simulation_interfaces/srv/UnloadWorld", "Unload World", false},
{ServiceType::ACTION_SIMULATE_STEPS, "simulation_interfaces/action/SimulateSteps", "Simulate Steps",
true}}};
diff --git a/src/sim_widget.ui b/src/sim_widget.ui
index 6adf3aa..0b8334e 100644
--- a/src/sim_widget.ui
+++ b/src/sim_widget.ui
@@ -11,7 +11,7 @@
- -
+
-
0
@@ -519,6 +519,67 @@
+
+
+ Worlds
+
+
+ -
+
+
+ Get Available Worlds
+
+
+
+ -
+
+
+ Worlds: Offline only
+
+
+ true
+
+
+
+ -
+
+
+ -
+
+
+ Load World
+
+
+
+ -
+
+
+ Unload World
+
+
+
+ -
+
+
+ Current world:
+
+
+
+ -
+
+
+ Qt::Vertical
+
+
+
+ 20
+ 225
+
+
+
+
+
+
true
diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp
index 533c64c..07e8495 100644
--- a/src/simulation_widget.cpp
+++ b/src/simulation_widget.cpp
@@ -114,6 +114,9 @@ namespace q_simulation_interfaces
connect(ui_->setSimStateButton, &QPushButton::clicked, this, &SimulationWidget::SetSimulationState);
connect(ui_->stepSimServiceButton, &QPushButton::clicked, this, &SimulationWidget::StepSimulationService);
connect(ui_->ComboEntities, &QComboBox::currentTextChanged, this, [this]() { this->GetEntityState(true); });
+ connect(ui_->getAvailableWorldsButton, &QPushButton::clicked, this, &SimulationWidget::GetAvailableWorlds);
+ connect(ui_->loadWorldButton, &QPushButton::clicked, this, &SimulationWidget::LoadWorld);
+ connect(ui_->unloadWorldButton, &QPushButton::clicked, this, &SimulationWidget::UnloadWorld);
// Connect spawn position spin boxes to update marker
connect(ui_->doubleSpinBoxX, QOverload::of(&QDoubleSpinBox::valueChanged), this,
@@ -234,6 +237,126 @@ namespace q_simulation_interfaces
setSimulationStateService_->call_service_async(cb, request);
}
+
+ void SimulationWidget::LoadWorld()
+ {
+ if (!loadWorldService_)
+ {
+ QMessageBox::warning(this, "Service Not Available",
+ "Load World service is not available. Please select a valid service.");
+ return;
+ }
+
+ simulation_interfaces::srv::LoadWorld::Request request;
+ auto selectedWorld = ui_->availableWorldsCombo->currentText();
+ if (useUriForWorlds_)
+ {
+ request.uri = selectedWorld.toStdString();
+ }
+ else
+ {
+ request.resource_string = selectedWorld.toStdString();
+ }
+
+ auto cb = [this](auto response)
+ {
+ ProduceWarningIfProblem(this, "Load World", response);
+ GetCurrentWorld();
+ };
+ loadWorldService_->call_service_async(cb, request);
+ }
+
+ void SimulationWidget::UnloadWorld()
+ {
+ if (!unloadWorldService_)
+ {
+ QMessageBox::warning(this, "Service Not Available",
+ "Unload World service is not available. Please select a valid service.");
+ return;
+ }
+
+ simulation_interfaces::srv::UnloadWorld::Request request;
+
+ auto cb = [this](auto response)
+ {
+ ProduceWarningIfProblem(this, "Unload World", response);
+ GetCurrentWorld();
+ };
+ unloadWorldService_->call_service_async(cb, request);
+ }
+
+ void SimulationWidget::GetAvailableWorlds()
+ {
+ if (!getAvailableWorldsService_)
+ {
+ QMessageBox::warning(this, "Service Not Available",
+ "Get Available Worlds service is not available. Please select a valid service.");
+ return;
+ }
+
+ simulation_interfaces::srv::GetAvailableWorlds::Request request;
+ request.offline_only = ui_->worldsUseOfflineCheck->isChecked();
+
+ auto cb = [this](auto response)
+ {
+ ProduceWarningIfProblem(this, "Get Available Worlds", response);
+ if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK)
+ {
+ ui_->availableWorldsCombo->clear();
+
+ auto worlds = response->worlds;
+
+ if (!worlds.empty())
+ {
+ // Determine if we are using URI or resource string based on the first world
+ useUriForWorlds_ = !worlds[0].world_resource.uri.empty();
+ }
+
+ for (const auto& world : worlds)
+ {
+
+ const QString worldStr = useUriForWorlds_
+ ? QString::fromStdString(world.world_resource.uri)
+ : QString::fromStdString(world.world_resource.resource_string);
+ ui_->availableWorldsCombo->addItem(worldStr);
+ }
+ }
+ };
+ getAvailableWorldsService_->call_service_async(cb, request);
+ }
+
+ void SimulationWidget::GetCurrentWorld()
+ {
+ if (!getCurrentWorldService_)
+ {
+ // Getting current world is called periodically, so just update the label without showing a message box
+ ui_->currentWorldLabel->setText("Current world: unknown due to unavailable service");
+ return;
+ }
+ simulation_interfaces::srv::GetCurrentWorld::Request request;
+ auto cb = [this](auto response)
+ {
+ // Check for NO_WORLD_LOADED error, which is not an error in this context
+ if (response &&
+ response->result.result == simulation_interfaces::srv::GetCurrentWorld::Response::NO_WORLD_LOADED)
+ {
+ ui_->currentWorldLabel->setText("Current world: No world loaded");
+ return;
+ }
+
+ ProduceWarningIfProblem(this, "Get Current World", response);
+ if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK)
+ {
+ useUriForWorlds_ = !response->world.world_resource.uri.empty();
+ const QString worldStr = useUriForWorlds_
+ ? QString::fromStdString(response->world.world_resource.uri)
+ : QString::fromStdString(response->world.world_resource.resource_string);
+ ui_->currentWorldLabel->setText("Current world: " + worldStr);
+ }
+ };
+ getCurrentWorldService_->call_service_async(cb, request);
+ }
+
void SimulationWidget::ActionThreadWorker(int steps)
{
actionThreadRunning_.store(true);
@@ -877,6 +1000,47 @@ namespace q_simulation_interfaces
serviceInterfaces_.insert(setSimulationStateService_);
}
break;
+ case ServiceType::SERVICE_GET_CURRENT_WORLD:
+ serviceInterfaces_.erase(getCurrentWorldService_);
+ getCurrentWorldService_ = shouldReset
+ ? nullptr
+ : std::make_shared>(selectedServiceName, node_);
+ if (getCurrentWorldService_)
+ {
+ serviceInterfaces_.insert(getCurrentWorldService_);
+ }
+ GetCurrentWorld();
+ break;
+ case ServiceType::SERVICE_GET_AVAILABLE_WORLDS:
+ serviceInterfaces_.erase(getAvailableWorldsService_);
+ getAvailableWorldsService_ = shouldReset
+ ? nullptr
+ : std::make_shared>(selectedServiceName, node_);
+ if (getAvailableWorldsService_)
+ {
+ serviceInterfaces_.insert(getAvailableWorldsService_);
+ }
+ break;
+ case ServiceType::SERVICE_LOAD_WORLD:
+ serviceInterfaces_.erase(loadWorldService_);
+ loadWorldService_ = shouldReset
+ ? nullptr
+ : std::make_shared>(selectedServiceName, node_);
+ if (loadWorldService_)
+ {
+ serviceInterfaces_.insert(loadWorldService_);
+ }
+ break;
+ case ServiceType::SERVICE_UNLOAD_WORLD:
+ serviceInterfaces_.erase(unloadWorldService_);
+ unloadWorldService_ = shouldReset
+ ? nullptr
+ : std::make_shared>(selectedServiceName, node_);
+ if (unloadWorldService_)
+ {
+ serviceInterfaces_.insert(unloadWorldService_);
+ }
+ break;
case ServiceType::ACTION_SIMULATE_STEPS:
simulateStepsAction_ = selectedServiceName;
break;
diff --git a/src/simulation_widget.h b/src/simulation_widget.h
index 99e83e1..8ff9dd0 100644
--- a/src/simulation_widget.h
+++ b/src/simulation_widget.h
@@ -30,15 +30,19 @@
#include
#include
#include
+#include
+#include
#include
#include
#include
#include
+#include
#include
#include
#include
#include
#include
+#include
namespace Ui
{
@@ -65,7 +69,6 @@ namespace q_simulation_interfaces
void hideEvent(QHideEvent* event) override;
void showEvent(QShowEvent* event) override;
- void onShowServicesTab();
void GetSpawnables();
void SpawnButton();
void GetAllEntities();
@@ -78,6 +81,10 @@ namespace q_simulation_interfaces
void GetSimulationState();
void SetSimulationState();
void StepSimulationService();
+ void LoadWorld();
+ void UnloadWorld();
+ void GetAvailableWorlds();
+ void GetCurrentWorld();
//! The thread with own ROS 2 node that will run the action client
void ActionThreadWorker(int steps);
@@ -112,10 +119,17 @@ namespace q_simulation_interfaces
std::shared_ptr> getSimulationStateService_;
std::shared_ptr> setSimulationStateService_;
std::shared_ptr> stepSimulationService_;
+ std::shared_ptr> getAvailableWorldsService_;
+ std::shared_ptr> getCurrentWorldService_;
+ std::shared_ptr> loadWorldService_;
+ std::shared_ptr> unloadWorldService_;
// Action names
std::string simulateStepsAction_ = "";
+ // Whether to use URI or resource string for worlds
+ bool useUriForWorlds_ = true;
+
// Vector to hold all service interfaces of created services
std::set> serviceInterfaces_;
QTimer* timer_; //! Timer for periodic updates
diff --git a/src/string_to_keys.h b/src/string_to_keys.h
index 4788b71..b21cdae 100644
--- a/src/string_to_keys.h
+++ b/src/string_to_keys.h
@@ -44,7 +44,14 @@ const std::map FeatureToName{
{31, "STEP_SIMULATION_SINGLE"},
{32, "STEP_SIMULATION_MULTIPLE"},
{33, "STEP_SIMULATION_ACTION"},
+ {40, "WORLD_LOADING"},
+ {41, "WORLD_RESOURCE_STRING"},
+ {42, "WORLD_TAGS"},
+ {43, "WORLD_UNLOADING"},
+ {44, "WORLD_INFO_GETTING"},
+ {45, "AVAILABLE_WORLDS"},
};
+
const std::map FeatureDescription{
{0, "Supports spawn interface (SpawnEntity)."},
{1, "Supports deleting entities (DeleteEntity)."},
@@ -71,7 +78,14 @@ const std::map FeatureDescription{
{31, "Supports single stepping through simulation with StepSimulation interface."},
{32, "Supports multi-stepping through simulation, either through StepSimulation service or StepSimulation action."},
{33, "Supports SimulateSteps action interface."},
+ {40, "Supports loading worlds through LoadWorld interface."},
+ {41, "Supports resource_string field in LoadWorld interface."},
+ {42, "Supports tags field in LoadWorld interface."},
+ {43, "Supports unloading worlds through UnloadWorld interface."},
+ {44, "Supports GetWorldInfo interface."},
+ {45, "Supports GetAvailableWorlds interface."},
};
+
const std::unordered_map ScopeNameToId{
{"SCOPE_DEFAULT", 0}, {"SCOPE_TIME", 1}, {"SCOPE_STATE", 2}, {"SCOPE_SPAWNED", 4}, {"SCOPE_ALL", 255},
};