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}, };