diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..2987a7f --- /dev/null +++ b/.clang-format @@ -0,0 +1,55 @@ +# Generated from CLion C/C++ Code Style settings +--- +Language: Cpp +BasedOnStyle: LLVM +AccessModifierOffset: -4 +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignOperands: false +AlignTrailingComments: false +AlwaysBreakTemplateDeclarations: Yes +BraceWrapping: + AfterCaseLabel: true + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + AfterExternBlock: false + BeforeCatch: true + BeforeElse: true + BeforeLambdaBody: true + BeforeWhile: true + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBraces: Custom +BreakConstructorInitializers: AfterColon +BreakConstructorInitializersBeforeComma: false +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: false +IncludeCategories: + - Regex: '^<.*' + Priority: 1 + - Regex: '^".*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseBlocks: true +IndentWidth: 4 +InsertNewlineAtEOF: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 2 +NamespaceIndentation: All +PointerAlignment: Left +SpaceInEmptyParentheses: false +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +TabWidth: 4 +... diff --git a/CMakeLists.txt b/CMakeLists.txt index b7f4470..d94b544 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,32 +6,70 @@ SET(CMAKE_AUTOUIC ON) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(Qt5 REQUIRED COMPONENTS Widgets) -find_package(simulation_interfaces REQUIRED) +find_package(simulation_interfaces 1 REQUIRED) find_package(rclcpp_action REQUIRED) find_package(tf2 REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rviz_common REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) set(CMAKE_INCLUDE_CURRENT_DIR ON) set(CMAKE_CXX_STANDARD 17) # Add UI file -set(SOURCES src/main.cpp - src/my_widget.cpp) -add_executable(${PROJECT_NAME} ${SOURCES} +qt5_wrap_cpp(MOC_FILES + include/q_simulation_interfaces/simulation_panel.h +) + +set(SOURCES src/simulation_widget.cpp src/simulation_panel.cpp) + +add_library(${PROJECT_NAME} SHARED ${SOURCES} ${MOC_FILES}) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) -qt_add_resources(${PROJECT_NAME} "resources/lightstyle.qrc") ament_target_dependencies(${PROJECT_NAME} - rclcpp rclcpp_action simulation_interfaces tf2 + rclcpp + rclcpp_action + simulation_interfaces + pluginlib rviz_common interactive_markers visualization_msgs + tf2 tf2_ros tf2_geometry_msgs ) target_link_libraries(${PROJECT_NAME} Qt5::Widgets ) +# Create standalone executable +add_executable(${PROJECT_NAME}_standalone src/standalone_main.cpp src/simulation_widget.cpp ${MOC_FILES}) +target_link_libraries(${PROJECT_NAME}_standalone ${PROJECT_NAME} Qt5::Widgets) +ament_target_dependencies(${PROJECT_NAME}_standalone rclcpp) install(TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(TARGETS ${PROJECT_NAME}_standalone + DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY icons/ + DESTINATION share/${PROJECT_NAME}/icons/classes +) + +ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME}) +pluginlib_export_plugin_description_file(rviz_common q_simulation_plugins.xml) ament_package() diff --git a/README.md b/README.md index 10850c9..001888b 100644 --- a/README.md +++ b/README.md @@ -5,19 +5,26 @@ It utilizes https://github.com/ros-simulation/simulation_interfaces to change th # Prerequisites -I don't know. ROS 2 Humble and libqt5-dev at least. +```shell +sudo apt install ros-$ROS_DISTRO-simulation-interfaces libqt5-dev +``` # Building -```bash -mkdir -p /tmp/ros2_ws/src -cd /tmp/ros2_ws/src -git clone https://github.com/ros-simulation/simulation_interfaces.git -git clone https://github.com/michalpelka/q_simulation_interfaces.git +```shell +mkdir -p ~/ros2_ws/src +cd ~/ros2_ws/src +git clone https://github.com/RobotecAI/q_simulation_interfaces.git cd .. colcon build --symlink-install source install/setup.bash -ros2 run q_simulation_interfaces q_simulation_interfaces ``` +The tool is available in two flavors: +- A standalone GUI application that can be launched as ROS 2 node: +```shell +ros2 run q_simulation_interfaces q_simulation_interfaces_standalone +``` +- A Rviz 2 panel plugin that can be launched from Rviz 2 by clicking on the `Panels` menu and selecting `Add New Panel...` + and then selecting `SimulationInterfacesPanel` from 'q_simulation_interfaces' package. \ No newline at end of file diff --git a/icons/SimulationInterfacesPanel.png b/icons/SimulationInterfacesPanel.png new file mode 100644 index 0000000..355c13e Binary files /dev/null and b/icons/SimulationInterfacesPanel.png differ diff --git a/include/q_simulation_interfaces/simulation_panel.h b/include/q_simulation_interfaces/simulation_panel.h new file mode 100644 index 0000000..0e7e20b --- /dev/null +++ b/include/q_simulation_interfaces/simulation_panel.h @@ -0,0 +1,30 @@ +#pragma once + + +#include +#include +#include +#include + +namespace q_simulation_interfaces +{ + class SimulationWidget; + class SimulationPanel : public rviz_common::Panel + { + Q_OBJECT + public: + explicit SimulationPanel(QWidget* parent = nullptr); + + ~SimulationPanel() override; + void onInitialize() override; + QString getName() const override; + void hideEvent(QHideEvent* event) override; + + private: + SimulationWidget* simulationWidget_; + rviz_common::Display* im_display_{nullptr}; + std::shared_ptr node_ptr_; + }; +} // namespace q_simulation_interfaces +#include +PLUGINLIB_EXPORT_CLASS(q_simulation_interfaces::SimulationPanel, rviz_common::Panel) diff --git a/package.xml b/package.xml index 26a4ef1..2e010d1 100644 --- a/package.xml +++ b/package.xml @@ -9,11 +9,17 @@ ament_cmake rclcpp + pluginlib + rviz_common + visualization_msgs + simulation_interfaces + interactive_markers qt5-qmake qtbase5-dev ament_cmake + diff --git a/q_simulation_plugins.xml b/q_simulation_plugins.xml new file mode 100644 index 0000000..b972974 --- /dev/null +++ b/q_simulation_plugins.xml @@ -0,0 +1,5 @@ + + + Simulation Interface Panel for controlling simulation entities and state + + \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 86e1953..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include -#include -#include -#include "my_widget.hpp" -#include -#include - - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - QApplication app(argc, argv); - - MyWidget widget; - widget.show(); - - int ret = app.exec(); - rclcpp::shutdown(); - return ret; -} - diff --git a/src/my_widget.cpp b/src/my_widget.cpp deleted file mode 100644 index 3e27146..0000000 --- a/src/my_widget.cpp +++ /dev/null @@ -1,360 +0,0 @@ -#include "my_widget.hpp" -#include "ui_sim_widget.h" // auto-generated by uic -#include -#include "service.h" -#include -#include -#include -#include -#include "stringToKeys.h" - - -QString VectorToQstring (const tf2::Vector3& v ) -{ - QString qString = QString::asprintf("%f %f %f", v.x(), v.y(), v.z()); - return qString; -} - -tf2::Vector3 QStringToVector (const QString& line ) -{ - tf2::Vector3 v; - QStringList list = line.split(" "); - if (list.size() == 3) { - v.setX(list[0].toDouble()); - v.setY(list[1].toDouble()); - v.setZ(list[2].toDouble()); - } else { - qWarning() << "Invalid input format. Expected 3 values."; - v = tf2::Vector3(0, 0, 0); // Default to zero vector - } - return v; -} - - -MyWidget::MyWidget(QWidget *parent) - : QWidget(parent), - ui_(new Ui::simWidgetUi) { - ui_->setupUi(this); - node_ = rclcpp::Node::make_shared("qt_gui_node"); - - // propagate reset scope combo - for (const auto& [name, _] : ScopeNameToId) - { - ui_->resetModeCombo->addItem(QString::fromStdString(name)); - } - // propagate state combo - for (const auto& [name, _] : SimStateNameToId) - { - ui_->simStateToSetComboBox->addItem(QString::fromStdString(name)); - } - - connect(ui_->PushButtonRefresh, &QPushButton::clicked, this, &MyWidget::GetSpawnables); - connect(ui_->SpawnButton, &QPushButton::clicked, this, &MyWidget::SpawnButton); - connect(ui_->getAllEntitiesButton, &QPushButton::clicked, this, &MyWidget::GetAllEntities); - connect(ui_->getEntityStateButton, &QPushButton::clicked, this, &MyWidget::GetEntityState); - connect(ui_->setEntityStateButton, &QPushButton::clicked, this, &MyWidget::SetEntityState); - connect(ui_->despawnButton, &QPushButton::clicked, this, &MyWidget::DespawnButton); - connect(ui_->GetSimCapabilites, &QPushButton::clicked, this, &MyWidget::GetSimFeatures); - connect(ui_->resetSimButton, &QPushButton::clicked, this, &MyWidget::ResetSimulation); - connect(ui_->stepSimButtonAction, &QPushButton::clicked, this, &MyWidget::StepSimulation); - connect(ui_->getSimStateBtn, &QPushButton::clicked, this, &MyWidget::GetSimulationState); - connect(ui_->setSimStateButton, &QPushButton::clicked, this, &MyWidget::SetSimulationState); - connect(ui_->stepSimServiceButton, &QPushButton::clicked, this, &MyWidget::StepSimulationService); - connect(ui_->ComboEntities, &QComboBox::currentTextChanged, this, [this](){ - this->GetEntityState(true); - }); -} - -MyWidget::~MyWidget() { - delete ui_; -} - - -void MyWidget::GetSimulationState() -{ - Service service("/get_simulation_state", node_); - auto response = service.call_service_sync(); - if (response) - { - int stateId = response->state.state; - QString stateName; - auto it = SimStateIdToName.find(stateId); - if (it == SimStateIdToName.end()) - { - stateName = QString::asprintf("Unknow state %d", stateId); - } - else - { - stateName = QString::fromStdString(it->second); - } - ui_->simStateLabel->setText(stateName); - } - -} - -void MyWidget::SetSimulationState() -{ - simulation_interfaces::srv::SetSimulationState::Request request; - auto selectedMode = ui_->simStateToSetComboBox->currentText(); - auto it = SimStateNameToId.find(selectedMode.toStdString()); - Q_ASSERT(it != SimStateNameToId.end()); - Service service("/set_simulation_state", node_); - request.state.state = it->second; - auto response = service.call_service_sync(request); - if (response) - { - GetSimulationState(); - } -} - - -void MyWidget::ActionThreadWorker(int steps) { - - using SimulateSteps=simulation_interfaces::action::SimulateSteps; - auto client = rclcpp_action::create_client(node_, "/simulate_steps"); - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - auto goal = std::make_shared(); - goal->steps = steps; - - send_goal_options.feedback_callback = - [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, - const std::shared_ptr feedback) { - // Handle feedback here - float progress = static_cast(feedback->completed_steps) / feedback->remaining_steps; - ui_->simProgressBar->setValue(static_cast(progress * 100)); - }; - auto goal_handle_future = client->async_send_goal(*goal, send_goal_options); - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(node_->get_logger(), "Failed to call service"); - return; - } - auto goal_handle = goal_handle_future.get(); - auto result_future = client->async_get_result(goal_handle); - if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(node_->get_logger(), "Failed to call service"); - return; - } - auto result = result_future.get(); - if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { - RCLCPP_INFO(node_->get_logger(), "Simulation completed successfully"); - } else { - RCLCPP_ERROR(node_->get_logger(), "Simulation failed"); - } - - -} -void MyWidget::StepSimulation() { - - int steps = ui_->stepsSpinBox->value(); - - if (actionThread_.joinable()) - { - actionThread_.join(); - } - actionThread_ = std::thread(&MyWidget::ActionThreadWorker, this, steps); -} - -void MyWidget::ResetSimulation() { - simulation_interfaces::srv::ResetSimulation::Request request; - Service service("/reset_simulation", node_); - //get seletected mode - auto selectedMode = ui_->resetModeCombo->currentText(); - auto it = ScopeNameToId.find(selectedMode.toStdString()); - Q_ASSERT(it != ScopeNameToId.end()); - request.scope = it->second; - auto response = service.call_service_sync(request); - -} - -void MyWidget::GetSimFeatures() -{ - Service service("/get_simulation_features", node_); - auto response = service.call_service_sync(); - ui_->listCapabilities->clear(); - std::set features; - for (auto& feature : response->features.features) - { - features.emplace(feature); - QString capabilityName; - auto it = FeatureToName.find(feature); - if (it == FeatureToName.end()) - { - capabilityName = QString::asprintf("Unknow feature %d", feature); - ui_->listCapabilities->addItem(capabilityName); - } - } - for (auto &[featrueId, featureName] : FeatureToName) - { - const QString labelSupported = u8"✔️"; - const QString labelNotSupported = u8"❌"; - bool isSupported = features.find(featrueId) != features.end(); - QString label = QString(featureName.c_str()) + " : " + (isSupported ? labelSupported : labelNotSupported); - QListWidgetItem *item = new QListWidgetItem(label); - item->setTextAlignment(Qt::AlignLeft); - item->setText(label); - item->setToolTip(QString::fromStdString(FeatureDescription.at(featrueId))); - ui_->listCapabilities->addItem(item); - } - -} - -void MyWidget::StepSimulationService() { - simulation_interfaces::srv::StepSimulation::Request request; - request.steps = ui_->stepsSpinBox->value(); - Service service("/step_simulation", node_); - auto response = service.call_service_sync(request); - if (response) { - // Handle the response if needed - } -} -void MyWidget::DespawnButton() { - simulation_interfaces::srv::DeleteEntity::Request request; - request.entity = ui_->ComboEntities->currentText().toStdString(); - Service service("/delete_entity", node_); - service.call_service_sync(request); -} - -void MyWidget::GetAllEntities() { - Service service("/get_entities", node_); - auto response = service.call_service_sync(); - if (response) - { - ui_->ComboEntities->clear(); - for (const auto &entity: response->entities) { - ui_->ComboEntities->addItem(QString(entity.c_str())); - } - } -} - -void MyWidget::GetEntityState(bool silent) { - simulation_interfaces::srv::GetEntityState::Request request; - request.entity = ui_->ComboEntities->currentText().toStdString(); - - Service service("/get_entity_state", node_); - auto response = service.call_service_sync(request, silent); - if (response) { - ui_->StatePosX->setValue(response->state.pose.position.x); - ui_->StatePosY->setValue(response->state.pose.position.y); - ui_->StatePosZ->setValue(response->state.pose.position.z); - - tf2::Quaternion q = tf2::Quaternion( - response->state.pose.orientation.x, - response->state.pose.orientation.y, - response->state.pose.orientation.z, - response->state.pose.orientation.w); - - const auto axis = q.getAxis(); - const auto angle = q.getAngle(); - ui_->RotVector->setText(VectorToQstring(axis)); - ui_->RotAngle->setValue(angle); - - ui_->StateVelX->setValue(response->state.twist.linear.x); - ui_->StateVelY->setValue(response->state.twist.linear.y); - ui_->StateVelZ->setValue(response->state.twist.linear.z); - ui_->StateVelRotX->setValue(response->state.twist.angular.x); - ui_->StateVelRotY->setValue(response->state.twist.angular.y); - ui_->StateVelRotZ->setValue(response->state.twist.angular.z); - } - - -} - -void MyWidget::SetEntityState() { - simulation_interfaces::srv::SetEntityState::Request request; - request.entity = ui_->ComboEntities->currentText().toStdString(); - request.state.pose.position.x = ui_->StatePosX->value(); - request.state.pose.position.y = ui_->StatePosY->value(); - request.state.pose.position.z = ui_->StatePosZ->value(); - - - const QString vectorStr = ui_->RotVector->text(); - const double angle = ui_->RotAngle->value(); - const auto vector = QStringToVector(vectorStr); - tf2::Quaternion q (vector,angle); - - request.state.pose.orientation.x = q.x(); - request.state.pose.orientation.y = q.y(); - request.state.pose.orientation.z = q.z(); - request.state.pose.orientation.w = q.w(); - - - request.state.twist.linear.x = ui_->StateVelX->value(); - request.state.twist.linear.y = ui_->StateVelY->value(); - request.state.twist.linear.z = ui_->StateVelZ->value(); - request.state.twist.angular.x = ui_->StateVelRotX->value(); - request.state.twist.angular.y = ui_->StateVelRotY->value(); - request.state.twist.angular.z = ui_->StateVelRotZ->value(); - - Service service("/set_entity_state", node_); - auto response = service.call_service_sync(request); -} - -void MyWidget::SpawnButton() { - simulation_interfaces::srv::SpawnEntity::Request request; - request.name = ui_->lineEditName->text().toStdString(); - request.uri = ui_->ComboSpawables->currentText().toStdString(); - request.entity_namespace = ui_->lineEditNamespace->text().toStdString(); - request.allow_renaming = ui_->checkBoxAllowRename->isChecked(); - - request.initial_pose.pose.position.x = ui_->doubleSpinBoxX->value(); - request.initial_pose.pose.position.y = ui_->doubleSpinBoxY->value(); - request.initial_pose.pose.position.z = ui_->doubleSpinBoxZ->value(); - // Create a client for the SpawnEntity service - Service service("/spawn_entity", node_); - auto response = service.call_service_sync(request); - if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) - { - QString message = QString::asprintf("Spawned as %s", response->entity_name.c_str()); - QMessageBox::information(this, "Success", message); - } - -} - -void MyWidget::GetSpawnables() { - // Create a client for the GetSpawnables service - auto client = node_->create_client("/get_spawnables"); - if (!client->wait_for_service(std::chrono::seconds(10))) { - RCLCPP_ERROR(node_->get_logger(), "Service not available after waiting"); - return; - } - auto request = std::make_shared(); - - // Call the service - auto future = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future) != - rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(node_->get_logger(), "Failed to call service"); - return; - } - auto response = future.get(); - if (response->result.result != simulation_interfaces::msg::Result::RESULT_OK) { - RCLCPP_ERROR(node_->get_logger(), "Service call failed: %s", response->result.error_message.c_str()); - return; - } - // Process the response - QString selectedSpawnable = ui_->ComboSpawables->currentText(); - ui_->ComboSpawables->clear(); - - // sort - std::sort(response->spawnables.begin(), response->spawnables.end(), - [](const auto &a, const auto &b) { - return a.uri < b.uri; - }); - for (const auto &spawnable: response->spawnables) { - ui_->ComboSpawables->addItem(QString::fromStdString(spawnable.uri)); - } - if (ui_->ComboSpawables->findText(selectedSpawnable) != -1) - { - ui_->ComboSpawables->setCurrentText(selectedSpawnable); - } - else - { - ui_->ComboSpawables->setCurrentIndex(0); - } -} - - - - - diff --git a/src/my_widget.hpp b/src/my_widget.hpp deleted file mode 100644 index 9bff461..0000000 --- a/src/my_widget.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include -#include - -namespace Ui { -class simWidgetUi; -} - -class MyWidget : public QWidget -{ - Q_OBJECT - -public: - explicit MyWidget(QWidget *parent = nullptr); - ~MyWidget(); - - -private: - void GetSpawnables(); - void SpawnButton(); - void GetAllEntities(); - void GetEntityState(bool silent = false); - void SetEntityState(); - void DespawnButton(); - void GetSimFeatures(); - void ResetSimulation(); - void StepSimulation(); - void GetSimulationState(); - void SetSimulationState(); - void StepSimulationService(); - - - void ActionThreadWorker(int steps); - std::thread actionThread_; - - Ui::simWidgetUi *ui_; - rclcpp::Node::SharedPtr node_; -}; - diff --git a/src/service.h b/src/service.h index dc64894..540cba8 100644 --- a/src/service.h +++ b/src/service.h @@ -1,45 +1,206 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include #include +#include +#include +#include #include -#include "stringToKeys.h" +#include +#include "string_to_keys.h" + +//! Simple expected-like class, where T is the expected type and error is a string. +//! This class is used to handle service responses and errors in a more structured way. +template +class Expected +{ + std::optional value_; + std::string error_; + +public: + //! Constructs the Expected object with a value or an error message. + Expected(const T& value) : value_(value) {} + Expected(T&& value) : value_(std::move(value)) {} + + //! Constructs the Expected object with an error message. + Expected(const std::string& error) : error_(error) {} + + //! Returns the value if it exists, otherwise throws an exception. + bool has_value() const { return value_.has_value(); } + + //! Returns true if the Expected object contains a value, false otherwise. + operator bool() const { return has_value(); } + + //! Returns the value. Does not check if the value exists. + const T& operator*() const + { + assert(value_.has_value()); + return *value_; + } + + //! Returns the value. Does not check if the value exists. + const T* operator->() const + { + assert(value_.has_value()); + return &(*value_); + } -template -class Service { + //! Returns the error message if it exists, otherwise returns an empty string. + const std::string& error() const + { + assert(!value_.has_value()); + return error_; + } +}; + +//! Produces a QT warning message box if the response has an error or if the result is not OK. +//! @param parent - parent widget for the message box +//! @param operation - the operation that was performed, used in the error message +//! @param response - the response from the service call, which can be an Expected +//! @tparam T - the service response type (e.g., simulation_interfaces::srv::GetSimulatorFeatures::Response) +template +void ProduceWarningIfProblem(QWidget* parent, const QString operation, const Expected& response) +{ + if (!response.has_value()) + { + auto w = QString("Failed to %1: %2").arg(operation, QString::fromStdString(response.error())); + QMessageBox::warning(parent, "Error", w); + return; + } + + // only GetSimulatorFeatures::Response does not have a result field, so check if T is not that type + if constexpr (!std::is_same_v) + { + if (response->result.result != simulation_interfaces::msg::Result::RESULT_OK) + { + auto w = QString("Operation %1 failed: %2") + .arg(operation, QString::fromStdString(response->result.error_message)); + QMessageBox::warning(parent, "Error", w); + } + } +} + +//! Interface for checking service results. +class ServiceInterface +{ +public: + //! Pokes the service to check if the result is ready. + //! This method should be called periodically to check if the service call has completed. + virtual void check_service_result() = 0; +}; + +//! Template class for a service client that can call a service asynchronously or synchronously. +//! It uses the ROS 2 rclcpp client to send requests and receive responses. +//! @tparam T - the service type, which must be a ROS 2 service type +template +class Service : public ServiceInterface +{ public: using Request = typename T::Request; using Response = typename T::Response; + using SharedPtr = std::shared_ptr; using Client = rclcpp::Client; using ClientSharedPtr = typename Client::SharedPtr; + using FutureAndRequestId = typename Client::FutureAndRequestId; + + //! Callback type for the service call completion. + //! It gives back to the caller an Expected object, which can either contain the response or an error + //! message. + using CompletionCallback = std::function)>; + + //! Constructor for the Service class. + //! @param service_name - the name of the service to call + //! @param node - the ROS 2 node to use for creating the client + //! @param timeout - the timeout for the service call in milliseconds, default is 1000 ms + Service(const std::string& service_name, rclcpp::Node::SharedPtr node, double timeout = 1000) : + client_(node->create_client(service_name)), node_(node), + timeout_(std::chrono::milliseconds(static_cast(timeout))) + { + } - Service(const std::string &service_name, rclcpp::Node::SharedPtr node) - : client_(node->create_client(service_name)), - node_(node) { - if (!client_->wait_for_service(std::chrono::seconds(10))) { - RCLCPP_ERROR(node->get_logger(), "Service not available after waiting"); + virtual ~Service() = default; + + //! Calls the service asynchronously. + //! @param callback - a callback function to handle the response, can be nullptr + //! @param request - the request to send, default is an empty request + void call_service_async(CompletionCallback callback = nullptr, const Request& request = Request()) + { + RCLCPP_DEBUG(node_->get_logger(), "Calling service %s", client_->get_service_name()); + + if (!client_->service_is_ready()) + { + if (callback) + { + callback(Expected{"Service not available"}); + RCLCPP_ERROR(node_->get_logger(), "Service %s is not available", client_->get_service_name()); + } + return; } + std::shared_ptr req = std::make_shared(request); + service_result_ = client_->async_send_request(req); + callback_ = callback; + service_called_time_ = std::chrono::system_clock::now(); } - std::optional call_service_sync(const Request &request = Request(), bool silent = false) { + //! Checks the service result and calls the callback if the result is ready. + //! This method should be called periodically to check if the service call has completed. + void check_service_result() override + { + if (!service_result_) + { + return; + } + // check duration + if (timeout_.count() > 0 && service_called_time_) + { + auto duration = std::chrono::system_clock::now() - *service_called_time_; + if (duration > timeout_) + { + RCLCPP_ERROR(node_->get_logger(), "Service call timed out"); + if (callback_) + { + callback_(Expected("Service call timed out")); + callback_ = nullptr; + } + service_result_.reset(); + service_called_time_.reset(); + return; + } + } + + if (service_result_->wait_for(std::chrono::seconds(0)) == std::future_status::ready) + { + auto response = service_result_->future.get(); + RCLCPP_DEBUG(node_->get_logger(), "Service %s response) received", client_->get_service_name()); + if (callback_) + { + callback_(Expected(*response)); + callback_ = nullptr; + } + service_result_.reset(); + service_called_time_.reset(); + } + RCLCPP_DEBUG(node_->get_logger(), "Service %s is still processing", client_->get_service_name()); + } - if constexpr (std::is_same() ) + //! Calls the service synchronously and checks the response. + //! @note, this method need to be called against ROS 2 node that can be spun. + //! @param request - the request to send, default is an empty request + Expected call_service_sync(const Request& request = Request(), bool silent = false) + { + if (!client_->wait_for_service(timeout_)) + { + RCLCPP_ERROR(node_->get_logger(), "Service not available after waiting"); + return Expected{"Service not available after waiting"}; + } + if constexpr (std::is_same()) { return call_service_sync_NoCheck(request); - } else { + } + else + { if (silent) { return call_service_sync_NoCheck(request); @@ -47,46 +208,57 @@ class Service { return call_service_sync_Check(request); } } + private: - std::optional call_service_sync_Check(const Request &request){ - std::shared_ptr req = std::make_shared(request); + Expected call_service_sync_Check(const Request& request) + { + std::shared_ptr req = std::make_shared(request); auto future = client_->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, future) != - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node_, future, timeout_) != rclcpp::FutureReturnCode::SUCCESS) + { RCLCPP_ERROR(node_->get_logger(), "Failed to call service"); - return std::nullopt; + return Expected{"Failed to call service"}; } auto response = future.get(); - if (response->result.result != simulation_interfaces::msg::Result::RESULT_OK) { + if (response->result.result != simulation_interfaces::msg::Result::RESULT_OK) + { RCLCPP_ERROR(node_->get_logger(), "Service call failed: %s", response->result.error_message.c_str()); const auto error_code = static_cast(response->result.result); QString errorType; - if (auto it = ErrorIdToName.find(error_code); it != ErrorIdToName.end()) { + if (auto it = ErrorIdToName.find(error_code); it != ErrorIdToName.end()) + { errorType = QString::fromStdString(it->second); - } else { - errorType = "Error : " + QString::number(error_code) ; + } + else + { + errorType = "Error : " + QString::number(error_code); } QMessageBox::warning(nullptr, errorType, QString::fromStdString(response->result.error_message)); - return *response; + return Expected{*response}; } - return *response; + return Expected{*response}; } - std::optional call_service_sync_NoCheck(const Request &request = Request() ) { - std::shared_ptr req = std::make_shared(request); + Expected call_service_sync_NoCheck(const Request& request = Request()) + { + std::shared_ptr req = std::make_shared(request); auto future = client_->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, future) != - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node_, future, timeout_) != rclcpp::FutureReturnCode::SUCCESS) + { RCLCPP_ERROR(node_->get_logger(), "Failed to call service"); - return std::nullopt; + return Expected{"Failed to call service"}; } auto response = future.get(); - return *response; + return Expected{*response}; } private: rclcpp::Node::SharedPtr node_; ClientSharedPtr client_; -}; \ No newline at end of file + std::chrono::milliseconds timeout_; + std::optional service_result_; + std::optional> service_called_time_; + std::function)> callback_; +}; diff --git a/src/sim_widget.ui b/src/sim_widget.ui index c9e252e..22b5229 100644 --- a/src/sim_widget.ui +++ b/src/sim_widget.ui @@ -7,7 +7,7 @@ 0 0 675 - 869 + 901 @@ -82,13 +82,6 @@ - - - - Simulator State - - - @@ -153,20 +146,37 @@ - - - - -999999.000000000000000 + + + + VelRotZ - - 999999999999999.000000000000000 + + + + + + PosZ: - - + + + + + - VelZ: + RotVector + + + + + + + + + + PosX: @@ -177,6 +187,22 @@ + + + + + + + + + + VelY: + + + + + + @@ -187,19 +213,15 @@ - - + + - VelRotY + 0 0 1 - - - - PosX: - - + + @@ -211,57 +233,39 @@ - - + + - RotAngle + VelRotY - - - - 0 0 1 + + + + 2 - - - - - - - - - VelX: + + -360.000000000000000 - - - - - - RotVector + + 360.000000000000000 - - - - - - PosZ: + + 0.001000000000000 + + + QAbstractSpinBox::AdaptiveDecimalStepType - - + + - VelRotZ + VelZ: - - - - - - @@ -269,35 +273,37 @@ - - - - - - - - + + - VelY: + RotAngle[Deg] - - - - - - - 6 + + + + VelX: + + + + - -99.000000000000000 + -999999.000000000000000 - - 0.001000000000000 + + 999999999999999.000000000000000 - - QAbstractSpinBox::AdaptiveDecimalStepType + + + + + + + + + Frame @@ -467,6 +473,43 @@ + + + + 7 + + + + + Frame + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + map + + + + + diff --git a/src/simulation_panel.cpp b/src/simulation_panel.cpp new file mode 100644 index 0000000..e1b64bc --- /dev/null +++ b/src/simulation_panel.cpp @@ -0,0 +1,111 @@ +#include +#include +#include +#include +#include "simulation_widget.h" +namespace q_simulation_interfaces +{ + namespace + { + constexpr char InteractiveMarkerClassId[] = "rviz_default_plugins/InteractiveMarkers"; + constexpr char InteractiveMarkerNamespacePropertyName[] = "Interactive Markers Namespace"; + } // namespace + SimulationPanel::SimulationPanel(QWidget* parent) + { + simulationWidget_ = new SimulationWidget(this); + QVBoxLayout* layout = new QVBoxLayout(this); + layout->addWidget(simulationWidget_); + setLayout(layout); + } + + SimulationPanel::~SimulationPanel() { delete simulationWidget_; } + + //! Recursively iterate through all displays in a display group and apply a function to each display. + void iterateAllDisplays(rviz_common::DisplayGroup* group, std::function func) + { + for (int i = 0; i < group->numChildren(); ++i) + { + auto display = group->getDisplayAt(i); + if (display) + { + if (func(display)) + { + return; // stop iterating if the function returns true + } + } + } + for (int i = 0; i < group->numChildren(); ++i) + { + auto sub_group = group->getGroupAt(i); + if (sub_group) + { + iterateAllDisplays(sub_group, func); + } + } + } + + void SimulationPanel::onInitialize() + { + node_ptr_ = getDisplayContext()->getRosNodeAbstraction().lock(); + if (!node_ptr_) + { + throw std::runtime_error("Failed to get ROS node abstraction"); + } + auto context = this->getDisplayContext(); + simulationWidget_->SetFixedFrame(context->getFixedFrame()); + simulationWidget_->initialize(node_ptr_->get_raw_node()); + + auto display_group = context->getRootDisplayGroup(); + assert(display_group); + + // check if the InteractiveMarkers display already exists + + const auto isInteractiveMarkerDisplay = [this](rviz_common::Display* display) + { + if (display->getClassId() == InteractiveMarkerClassId) + { + auto property = display->findProperty(InteractiveMarkerNamespacePropertyName); + if (property && + property->getValue().toString() == QString::fromStdString(InteractiveMarkerNamespaceValue)) + { + std::cout << "Found InteractiveMarkers display with namespace: " + << property->getValue().toString().toStdString() << std::endl; + im_display_ = display; // Store the found display + return true; // Found the InteractiveMarkers display with the correct namespace + } + } + return false; + }; + + iterateAllDisplays(display_group, isInteractiveMarkerDisplay); + + if (!im_display_) + { + im_display_ = display_group->createDisplay(InteractiveMarkerClassId); + assert(im_display_); + std::cout << "InteractiveMarkers display created" << std::endl; + + im_display_->initialize(context); + + im_display_->setEnabled(true); + im_display_->setShouldBeSaved(false); + im_display_->setName("Simulation Interactive Markers"); + auto property = im_display_->findProperty(InteractiveMarkerNamespacePropertyName); + assert(property); + property->setValue(QString::fromStdString(InteractiveMarkerNamespaceValue)); + display_group->addDisplay(im_display_); + } + }; + + void SimulationPanel::hideEvent(QHideEvent* event) + { + if (im_display_) + { + im_display_ = nullptr; + } + } + + + QString SimulationPanel::getName() const { return "Simulation Panel"; } + +} // namespace q_simulation_interfaces diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp new file mode 100644 index 0000000..d102077 --- /dev/null +++ b/src/simulation_widget.cpp @@ -0,0 +1,699 @@ +#include "simulation_widget.h" +#include +#include +#include +#include +#include +#include +#include +#include "service.h" +#include "string_to_keys.h" +#include "ui_sim_widget.h" +#include "vector_utils.hpp" + +#include +#include +#include +namespace q_simulation_interfaces +{ + namespace + { + geometry_msgs::msg::Quaternion CreateQuaternion(double w, double x, double y, double z) + { + geometry_msgs::msg::Quaternion q; + q.w = w; + q.x = x; + q.y = y; + q.z = z; + return q; + } + + void AddControlToInteractiveMarker(visualization_msgs::msg::InteractiveMarker& interactive_marker, + bool rotationZ) + { + // move axis x + const std::vector axesT = { + CreateQuaternion(1, 1, 0, 0), // x-axis + CreateQuaternion(1, 0, 1, 0), // y-axis + CreateQuaternion(1, 0, 0, 1) // z-axis + }; + std::vector axesR; + if (rotationZ) + { + axesR.push_back(CreateQuaternion(1, 0, 1, 0)); // z-axis + } + + for (const auto& axis : axesT) + { + visualization_msgs::msg::InteractiveMarkerControl control; + control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::FIXED; + control.orientation = axis; + control.name = "move_" + std::to_string(axis.w); + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; + interactive_marker.controls.push_back(control); + } + for (const auto& axis : axesR) + { + visualization_msgs::msg::InteractiveMarkerControl control; + control.orientation_mode = visualization_msgs::msg::InteractiveMarkerControl::FIXED; + control.orientation = axis; + control.name = "rotate_" + std::to_string(axis.w); + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; + interactive_marker.controls.push_back(control); + } + } + + } // namespace + + SimulationWidget::SimulationWidget(QWidget* parent) : QWidget(parent), ui_(new Ui::simWidgetUi) + { + ui_->setupUi(this); + ui_->frameStateLineEdit->setText("world"); + ui_->spawnFrameLineEdit->setText("world"); + + for (const auto& [name, _] : ScopeNameToId) + { + ui_->resetModeCombo->addItem(QString::fromStdString(name)); + } + for (const auto& [_, name] : SimStateIdToName) + { + ui_->simStateToSetComboBox->addItem(QString::fromStdString(name)); + } + + timer_ = new QTimer(this); + connect(timer_, &QTimer::timeout, this, &SimulationWidget::UpdateServices); + timer_->setSingleShot(false); + timer_->start(100); + connect(ui_->PushButtonRefresh, &QPushButton::clicked, this, &SimulationWidget::GetSpawnables); + connect(ui_->SpawnButton, &QPushButton::clicked, this, &SimulationWidget::SpawnButton); + connect(ui_->getAllEntitiesButton, &QPushButton::clicked, this, &SimulationWidget::GetAllEntities); + connect(ui_->getEntityStateButton, &QPushButton::clicked, this, &SimulationWidget::GetEntityState); + connect(ui_->setEntityStateButton, &QPushButton::clicked, this, &SimulationWidget::SetEntityState); + connect(ui_->despawnButton, &QPushButton::clicked, this, &SimulationWidget::DespawnButton); + connect(ui_->GetSimCapabilites, &QPushButton::clicked, this, &SimulationWidget::GetSimFeatures); + connect(ui_->resetSimButton, &QPushButton::clicked, this, &SimulationWidget::ResetSimulation); + connect(ui_->stepSimButtonAction, &QPushButton::clicked, this, &SimulationWidget::StepSimulation); + connect(ui_->getSimStateBtn, &QPushButton::clicked, this, &SimulationWidget::GetSimulationState); + 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 spawn position spin boxes to update marker + connect(ui_->doubleSpinBoxX, QOverload::of(&QDoubleSpinBox::valueChanged), this, + &SimulationWidget::UpdateSpawnPointMarker); + connect(ui_->doubleSpinBoxY, QOverload::of(&QDoubleSpinBox::valueChanged), this, + &SimulationWidget::UpdateSpawnPointMarker); + connect(ui_->doubleSpinBoxZ, QOverload::of(&QDoubleSpinBox::valueChanged), this, + &SimulationWidget::UpdateSpawnPointMarker); + } + + SimulationWidget::~SimulationWidget() + { + if (actionThread_.joinable()) + { + actionThread_.join(); + } + delete ui_; + } + + + void SimulationWidget::initialize(rclcpp::Node::SharedPtr node) + { + node_ = node; + // tf transform listener + tf_buffer_ = std::make_unique(node->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + // Initialize service objects + getSpawnablesService_ = + std::make_shared>("/get_spawnables", node); + serviceInterfaces_.push_back(getSpawnablesService_); + + spawnEntityService_ = std::make_shared>("/spawn_entity", node); + serviceInterfaces_.push_back(spawnEntityService_); + + getEntitiesService_ = std::make_shared>("/get_entities", node); + serviceInterfaces_.push_back(getEntitiesService_); + + getEntityStateService_ = + std::make_shared>("/get_entity_state", node); + serviceInterfaces_.push_back(getEntityStateService_); + + setEntityStateService_ = + std::make_shared>("/set_entity_state", node); + serviceInterfaces_.push_back(setEntityStateService_); + + + deleteEntityService_ = + std::make_shared>("/delete_entity", node); + serviceInterfaces_.push_back(deleteEntityService_); + + + getSimFeaturesService_ = std::make_shared>( + "/get_simulator_features", node); + serviceInterfaces_.push_back(getSimFeaturesService_); + + resetSimulationService_ = + std::make_shared>("/reset_simulation", node); + serviceInterfaces_.push_back(resetSimulationService_); + + getSimulationStateService_ = + std::make_shared>("/get_simulation_state", node); + serviceInterfaces_.push_back(getSimulationStateService_); + + setSimulationStateService_ = + std::make_shared>("/set_simulation_state", node); + serviceInterfaces_.push_back(setSimulationStateService_); + + stepSimulationService_ = + std::make_shared>("/step_simulation", node); + serviceInterfaces_.push_back(stepSimulationService_); + // + interactiveMarkerServer_ = + std::make_shared(InteractiveMarkerNamespaceValue, node); + + // Create spawn point marker + CreateSpawnPointMarker(); + } + + void SimulationWidget::hideEvent(QHideEvent* event) + { + RCLCPP_INFO(node_->get_logger(), "hideEvent"); + if (interactiveMarkerServer_) + { + interactiveMarkerServer_->clear(); + interactiveMarkerServer_->applyChanges(); + } + } + void SimulationWidget::showEvent(QShowEvent* event) + { + RCLCPP_INFO(node_->get_logger(), "showEvent"); + if (interactiveMarkerServer_) + { + CreateSpawnPointMarker(); + UpdateSpawnPointMarker(); // Ensure the marker is updated when shown + } + } + + + void SimulationWidget::GetSimulationState() + { + auto cb = [this](auto response) + { + ProduceWarningIfProblem(this, "Get Simulation State", response); + if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) + { + { + int stateId = response->state.state; + QString stateName; + auto it = SimStateIdToName.find(stateId); + if (it == SimStateIdToName.end()) + { + stateName = QString::asprintf("Unknown state %d", stateId); + } + else + { + stateName = QString::fromStdString(it->second); + } + ui_->simStateLabel->setText(stateName); + } + } + }; + getSimulationStateService_->call_service_async(cb); + } + + void SimulationWidget::SetSimulationState() + { + simulation_interfaces::srv::SetSimulationState::Request request; + auto selectedMode = ui_->simStateToSetComboBox->currentText(); + auto it = SimStateNameToId.find(selectedMode.toStdString()); + Q_ASSERT(it != SimStateNameToId.end()); + request.state.state = it->second; + + auto cb = [this](auto response) + { + ProduceWarningIfProblem(this, "Set Simulation State", response); + if (response) + { + GetSimulationState(); // Refresh state display + } + }; + setSimulationStateService_->call_service_async(cb, request); + } + + void SimulationWidget::ActionThreadWorker(int steps) + { + actionThreadRunning_.store(true); + // create node for action client + auto node = rclcpp::Node::make_shared("qt_gui_action_node"); + using SimulateSteps = simulation_interfaces::action::SimulateSteps; + auto client = rclcpp_action::create_client(node, "/simulate_steps"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + auto goal = std::make_shared(); + goal->steps = steps; + send_goal_options.feedback_callback = + [this, steps](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const std::shared_ptr feedback) + { + float progress = static_cast(feedback->completed_steps) / steps; + actionThreadProgress_.store(progress); + }; + send_goal_options.goal_response_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) + { + if (!goal_handle) + { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by the action server"); + } + else + { + RCLCPP_INFO(node_->get_logger(), "Goal accepted by the action server"); + } + }; + send_goal_options.result_callback = + [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) + { + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) + { + actionThreadProgress_.store(1.0f); + RCLCPP_INFO(node_->get_logger(), "Simulation completed successfully"); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Simulation failed"); + } + }; + auto goal_handle = client->async_send_goal(*goal, send_goal_options); + if (rclcpp::spin_until_future_complete(node, goal_handle) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "Failed to call action"); + actionThreadRunning_.store(false); + return; + } + auto goal_handle_result = goal_handle.get(); + auto result_future = client->async_get_result(goal_handle_result); + if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "Failed to get action result"); + actionThreadRunning_.store(false); + return; + } + actionThreadRunning_.store(false); + } + + void SimulationWidget::StepSimulation() + { + int steps = ui_->stepsSpinBox->value(); + if (actionThreadRunning_.load() == true) + { + QMessageBox::warning(this, "Action in progress", "An action is already running. Please wait."); + return; + } + + if (actionThread_.joinable()) + { + actionThread_.join(); + } + actionThreadRunning_.store(true); + actionThread_ = std::thread(&SimulationWidget::ActionThreadWorker, this, steps); + } + + void SimulationWidget::ResetSimulation() + { + simulation_interfaces::srv::ResetSimulation::Request request; + auto selectedMode = ui_->resetModeCombo->currentText(); + auto it = ScopeNameToId.find(selectedMode.toStdString()); + Q_ASSERT(it != ScopeNameToId.end()); + request.scope = it->second; + + auto cb = [this](auto response) + { + ProduceWarningIfProblem(this, "Reset Simulation", response); + // Refresh entities and state after reset + if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) + { + GetAllEntities(); + GetSimulationState(); + } + }; + resetSimulationService_->call_service_async(cb, request); + } + + void SimulationWidget::GetSimFeatures() + { + auto cb = [this](auto response) + { + ui_->listCapabilities->clear(); + std::set features; + + ProduceWarningIfProblem(this, "Get Simulation Features", response); + if (response) + { + for (auto& feature : response->features.features) + { + features.emplace(feature); + } + + for (auto& [featureId, featureName] : FeatureToName) + { + const QString labelSupported = u8"✔️"; + const QString labelNotSupported = u8"❌"; + bool isSupported = features.find(featureId) != features.end(); + QString label = + QString(featureName.c_str()) + " : " + (isSupported ? labelSupported : labelNotSupported); + QListWidgetItem* item = new QListWidgetItem(label); + item->setTextAlignment(Qt::AlignLeft); + item->setToolTip(QString::fromStdString(FeatureDescription.at(featureId))); + ui_->listCapabilities->addItem(item); + } + } + }; + getSimFeaturesService_->call_service_async(cb); + } + + void SimulationWidget::StepSimulationService() + { + simulation_interfaces::srv::StepSimulation::Request request; + request.steps = ui_->stepsSpinBox->value(); + + auto cb = [this](auto response) + { + ProduceWarningIfProblem(this, "Step Simulation", response); + if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) + { + GetSimulationState(); + } + }; + stepSimulationService_->call_service_async(cb, request); + } + + void SimulationWidget::DespawnButton() + { + simulation_interfaces::srv::DeleteEntity::Request request; + request.entity = ui_->ComboEntities->currentText().toStdString(); + + auto cb = [this](auto response) + { + ProduceWarningIfProblem(this, "Despawn", response); + // Refresh entity list after successful deletion + if (response) + { + GetAllEntities(); + } + }; + deleteEntityService_->call_service_async(cb, request); + } + + void SimulationWidget::GetAllEntities() + { + auto cb = [this](auto response) + { + ProduceWarningIfProblem(this, "GetAllEntities", response); + // Refresh entity list after successful deletion + if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) + { + ui_->ComboEntities->clear(); + + auto entities = response->entities; + std::sort(entities.begin(), entities.end()); + + for (const auto& entity : entities) + { + ui_->ComboEntities->addItem(QString(entity.c_str())); + } + } + }; + getEntitiesService_->call_service_async(cb); + } + + void SimulationWidget::GetEntityState(bool silent) + { + simulation_interfaces::srv::GetEntityState::Request request; + request.entity = ui_->ComboEntities->currentText().toStdString(); + + auto cb = [this, silent, entity = request.entity](auto response) + { + if (!silent) + { + ProduceWarningIfProblem(this, "GetEntityState", response); + } + // Refresh entity list after successful deletion + if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) + { + + // Update UI elements + ui_->StatePosX->setValue(response->state.pose.position.x); + ui_->StatePosY->setValue(response->state.pose.position.y); + ui_->StatePosZ->setValue(response->state.pose.position.z); + + tf2::Quaternion q = + tf2::Quaternion(response->state.pose.orientation.x, response->state.pose.orientation.y, + response->state.pose.orientation.z, response->state.pose.orientation.w); + + const auto axis = q.getAxis(); + const auto angle = qRadiansToDegrees(q.getAngle()); + ui_->RotVector->setText(VectorToQstring(axis)); + ui_->RotAngle->setValue(angle); + if (response->state.header.frame_id.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Empty frame id"); + } + else + { + ui_->frameStateLineEdit->setText(QString::fromStdString(response->state.header.frame_id)); + } + ui_->StateVelX->setValue(response->state.twist.linear.x); + ui_->StateVelY->setValue(response->state.twist.linear.y); + ui_->StateVelZ->setValue(response->state.twist.linear.z); + ui_->StateVelRotX->setValue(response->state.twist.angular.x); + ui_->StateVelRotY->setValue(response->state.twist.angular.y); + ui_->StateVelRotZ->setValue(response->state.twist.angular.z); + + // Update interactive marker if available + if (interactiveMarkerServer_) + { + visualization_msgs::msg::InteractiveMarker interactive_marker; + interactive_marker.header.frame_id = ui_->frameStateLineEdit->text().toStdString(); + interactive_marker.name = "Manipulator"; + interactive_marker.description = "Manipulate entity " + entity; + interactive_marker.pose.position = response->state.pose.position; + interactive_marker.pose.orientation = response->state.pose.orientation; + interactive_marker.scale = 0.2; + + AddControlToInteractiveMarker(interactive_marker, true); + + interactive_markers::InteractiveMarkerServer::FeedbackCallback feedbackCb = + [this, entity](const auto& feedback) + { + const auto& pose = feedback->pose; + + ui_->StatePosX->setValue(pose.position.x); + ui_->StatePosY->setValue(pose.position.y); + ui_->StatePosZ->setValue(pose.position.z); + + tf2::Quaternion q = tf2::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, + pose.orientation.w); + + const auto axis = q.getAxis(); + const auto angle = qRadiansToDegrees(q.getAngle()); + ui_->RotVector->setText(VectorToQstring(axis)); + ui_->RotAngle->setValue(angle); + ui_->frameStateLineEdit->setText(QString::fromStdString(feedback->header.frame_id)); + + simulation_interfaces::srv::SetEntityState::Request request; + request.entity = entity; + request.state.pose = pose; + request.state.header.frame_id = feedback->header.frame_id; + setEntityStateService_->call_service_async(nullptr, request); + }; + interactiveMarkerServer_->insert(interactive_marker, feedbackCb); + interactiveMarkerServer_->applyChanges(); + } + } + }; + getEntityStateService_->call_service_async(cb, request); + } + + void SimulationWidget::SetEntityState() + { + simulation_interfaces::srv::SetEntityState::Request request; + request.entity = ui_->ComboEntities->currentText().toStdString(); + request.state.pose.position.x = ui_->StatePosX->value(); + request.state.pose.position.y = ui_->StatePosY->value(); + request.state.pose.position.z = ui_->StatePosZ->value(); + + const QString vectorStr = ui_->RotVector->text(); + const double angle = qDegreesToRadians(ui_->RotAngle->value()); + const auto vector = QStringToVector(vectorStr); + tf2::Quaternion q(vector, angle); + request.state.header.frame_id = ui_->frameStateLineEdit->text().toStdString(); + request.state.pose.orientation.x = q.x(); + request.state.pose.orientation.y = q.y(); + request.state.pose.orientation.z = q.z(); + request.state.pose.orientation.w = q.w(); + + request.state.twist.linear.x = ui_->StateVelX->value(); + request.state.twist.linear.y = ui_->StateVelY->value(); + request.state.twist.linear.z = ui_->StateVelZ->value(); + request.state.twist.angular.x = ui_->StateVelRotX->value(); + request.state.twist.angular.y = ui_->StateVelRotY->value(); + request.state.twist.angular.z = ui_->StateVelRotZ->value(); + + auto cb = [this](auto response) { ProduceWarningIfProblem(this, "SetEntityState", response); }; + setEntityStateService_->call_service_async(cb, request); + } + + + void SimulationWidget::SpawnButton() + { + simulation_interfaces::srv::SpawnEntity::Request request; + request.name = ui_->lineEditName->text().toStdString(); + request.uri = ui_->ComboSpawables->currentText().toStdString(); + request.entity_namespace = ui_->lineEditNamespace->text().toStdString(); + request.allow_renaming = ui_->checkBoxAllowRename->isChecked(); + request.initial_pose.header.frame_id = ui_->spawnFrameLineEdit->text().toStdString(); + request.initial_pose.pose.position.x = ui_->doubleSpinBoxX->value(); + request.initial_pose.pose.position.y = ui_->doubleSpinBoxY->value(); + request.initial_pose.pose.position.z = ui_->doubleSpinBoxZ->value(); + + auto cb = [this](auto response) + { + ProduceWarningIfProblem(this, "SpawnEntity", response); + if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) + { + QString message = QString::asprintf("Spawned as %s", response->entity_name.c_str()); + QMessageBox::information(this, "Success", message); + // Refresh entity list after successful spawn + GetAllEntities(); + } + }; + spawnEntityService_->call_service_async(cb, request); + } + + void SimulationWidget::GetSpawnables() + { + auto cb = [this](auto response) + { + ui_->ComboSpawables->clear(); + ProduceWarningIfProblem(this, "GetSpawnables", response); + if (response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) + { + QString selectedSpawnable = ui_->ComboSpawables->currentText(); + + auto spawnables = response->spawnables; + std::sort(spawnables.begin(), spawnables.end(), + [](const auto& a, const auto& b) { return a.uri < b.uri; }); + + for (const auto& spawnable : spawnables) + { + ui_->ComboSpawables->addItem(QString::fromStdString(spawnable.uri)); + } + + if (ui_->ComboSpawables->findText(selectedSpawnable) != -1) + { + ui_->ComboSpawables->setCurrentText(selectedSpawnable); + } + else + { + ui_->ComboSpawables->setCurrentIndex(0); + } + } + }; + getSpawnablesService_->call_service_async(cb); + } + + void SimulationWidget::UpdateServices() + { + + if (actionThreadRunning_) + { + ui_->stepSimButtonAction->setEnabled(false); + ui_->stepSimServiceButton->setEnabled(false); + } + else + { + ui_->stepSimButtonAction->setEnabled(true); + ui_->stepSimServiceButton->setEnabled(true); + } + ui_->simProgressBar->setValue(static_cast(this->actionThreadProgress_ * 100)); + for (auto& service : serviceInterfaces_) + { + if (service) + { + service->check_service_result(); + } + } + } + + void SimulationWidget::CreateSpawnPointMarker() + { + if (!interactiveMarkerServer_) + { + return; + } + + visualization_msgs::msg::InteractiveMarker spawnMarker; + spawnMarker.header.frame_id = ui_->spawnFrameLineEdit->text().toStdString(); + spawnMarker.name = "spawn_point"; + spawnMarker.description = "Spawn Point - Drag to set spawn location"; + spawnMarker.scale = 0.3; + + // Set initial position from GUI + spawnMarker.pose.position.x = ui_->doubleSpinBoxX->value(); + spawnMarker.pose.position.y = ui_->doubleSpinBoxY->value(); + spawnMarker.pose.position.z = ui_->doubleSpinBoxZ->value(); + spawnMarker.pose.orientation.w = 1.0; // Default orientation + + AddControlToInteractiveMarker(spawnMarker, false); + + // Add visual marker (cube) + visualization_msgs::msg::InteractiveMarkerControl visualControl; + visualControl.always_visible = true; + visualControl.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE; + + spawnMarker.controls.push_back(visualControl); + + // Set up feedback callback to update GUI when marker is moved + interactive_markers::InteractiveMarkerServer::FeedbackCallback feedbackCb = [this](const auto& feedback) + { + if (feedback && feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE && + ui_) + { + assert(ui_); + const auto& pose = feedback->pose; + ui_->spawnFrameLineEdit->setText(QString::fromStdString(feedback->header.frame_id)); + ui_->doubleSpinBoxX->setValue(pose.position.x); + ui_->doubleSpinBoxY->setValue(pose.position.y); + ui_->doubleSpinBoxZ->setValue(pose.position.z); + } + }; + + interactiveMarkerServer_->insert(spawnMarker, feedbackCb); + } + + void SimulationWidget::UpdateSpawnPointMarker() + { + if (!interactiveMarkerServer_) + { + return; + } + + // Update position from GUI + geometry_msgs::msg::Pose newPose; + std_msgs::msg::Header newHeader; + newHeader.frame_id = ui_->spawnFrameLineEdit->text().toStdString(); + newPose.position.x = ui_->doubleSpinBoxX->value(); + newPose.position.y = ui_->doubleSpinBoxY->value(); + newPose.position.z = ui_->doubleSpinBoxZ->value(); + newPose.orientation.w = 1.0; + + interactiveMarkerServer_->setPose("spawn_point", newPose, newHeader); + interactiveMarkerServer_->applyChanges(); + } + void SimulationWidget::SetFixedFrame(const QString& frame_id) + { + ui_->spawnFrameLineEdit->setText(frame_id); + ui_->frameStateLineEdit->setText(frame_id); + } + +} // namespace q_simulation_interfaces diff --git a/src/simulation_widget.h b/src/simulation_widget.h new file mode 100644 index 0000000..9bcfef2 --- /dev/null +++ b/src/simulation_widget.h @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Ui +{ + class simWidgetUi; +} +namespace q_simulation_interfaces +{ + const char InteractiveMarkerNamespaceValue[] = "simulation_interfaces_panel"; + class SimulationWidget : public QWidget + { + Q_OBJECT + + public: + explicit SimulationWidget(QWidget* parent = nullptr); + ~SimulationWidget() override; + + void SetFixedFrame(const QString& frame_id); + void initialize(rclcpp::Node::SharedPtr node = nullptr); + + private: + // QWidget interface + void hideEvent(QHideEvent* event) override; + void showEvent(QShowEvent* event) override; + + void GetSpawnables(); + void SpawnButton(); + void GetAllEntities(); + void GetEntityState(bool silent = false); + void SetEntityState(); + void DespawnButton(); + void GetSimFeatures(); + void ResetSimulation(); + void StepSimulation(); + void GetSimulationState(); + void SetSimulationState(); + void StepSimulationService(); + + //! The thread with own ROS 2 node that will run the action client + void ActionThreadWorker(int steps); + + //! Called periodically to update the state of the services + void UpdateServices(); + + //! Create and update spawn point interactive marker + void CreateSpawnPointMarker(); + void UpdateSpawnPointMarker(); + + std::thread actionThread_; + std::atomic actionThreadRunning_{false}; //! Flag to control the action thread + std::atomic actionThreadProgress_{0.0f}; //! Progress of the simulation step, used for UI updates + + Ui::simWidgetUi* ui_; + rclcpp::Node::SharedPtr node_; + + // Service member variables + std::shared_ptr> getSpawnablesService_; + std::shared_ptr> spawnEntityService_; + std::shared_ptr> getEntitiesService_; + std::shared_ptr> getEntityStateService_; + std::shared_ptr> setEntityStateService_; + std::shared_ptr> deleteEntityService_; + std::shared_ptr> getSimFeaturesService_; + std::shared_ptr> resetSimulationService_; + std::shared_ptr> getSimulationStateService_; + std::shared_ptr> setSimulationStateService_; + std::shared_ptr> stepSimulationService_; + + // Vector to hold all service interfaces of created services + std::vector> serviceInterfaces_; + QTimer* timer_; //! Timer for periodic updates + + std::shared_ptr interactiveMarkerServer_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + }; +} // namespace q_simulation_interfaces diff --git a/src/standalone_main.cpp b/src/standalone_main.cpp new file mode 100644 index 0000000..123c2c3 --- /dev/null +++ b/src/standalone_main.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include "simulation_widget.h" + +class StandaloneWidget : public QWidget +{ +public: + StandaloneWidget(QWidget* parent = nullptr) : QWidget(parent) + { + // Initialize ROS node + node_ = rclcpp::Node::make_shared("standalone_simulation_widget"); + setWindowTitle("Simulation Interface"); + setMinimumSize(800, 600); + + auto layout = new QVBoxLayout(this); + + // Create a mock display context for the panel + panel_ = new q_simulation_interfaces::SimulationWidget(this); + panel_->initialize(node_); + layout->addWidget(panel_); + node_thread_ = std::thread([this]() { rclcpp::spin(node_); }); + } + + ~StandaloneWidget() + { + if (node_thread_.joinable()) + { + node_thread_.join(); + } + rclcpp::shutdown(); + } + +private: + q_simulation_interfaces::SimulationWidget* panel_; + rclcpp::Node::SharedPtr node_; + std::thread node_thread_; +}; + +int main(int argc, char* argv[]) +{ + + rclcpp::init(argc, argv); + QApplication app(argc, argv); + StandaloneWidget widget; + widget.show(); + + int ret = app.exec(); + rclcpp::shutdown(); + return ret; +} diff --git a/src/stringToKeys.h b/src/stringToKeys.h deleted file mode 100644 index b930c56..0000000 --- a/src/stringToKeys.h +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once - -#include -#include - -const std::map FeatureToName - { - {0, "SPAWNING"}, - {1, "DELETING"}, - {2, "NAMED_POSES"}, - {3, "POSE_BOUNDS"}, - {4, "ENTITY_TAGS"}, - {5, "ENTITY_BOUNDS"}, - {6, "ENTITY_BOUNDS_BOX"}, - {7, "ENTITY_BOUNDS_CONVEX"}, - {8, "ENTITY_CATEGORIES"}, - {9, "SPAWNING_RESOURCE_STRING"}, - {10, "ENTITY_STATE_GETTING"}, - {11, "ENTITY_STATE_SETTING"}, - {12, "ENTITY_INFO_GETTING"}, - {13, "ENTITY_INFO_SETTING"}, - {14, "SPAWNABLES"}, - {20, "SIMULATION_RESET"}, - {21, "SIMULATION_RESET_TIME"}, - {22, "SIMULATION_RESET_STATE"}, - {23, "SIMULATION_RESET_SPAWNED"}, - {24, "SIMULATION_STATE_GETTING"}, - {25, "SIMULATION_STATE_SETTING"}, - {26, "SIMULATION_STATE_PAUSE"}, - {31, "STEP_SIMULATION_SINGLE"}, - {32, "STEP_SIMULATION_MULTIPLE"}, - {33, "STEP_SIMULATION_ACTION"}, - }; -const std::map FeatureDescription - { - {0, "Supports spawn interface (SpawnEntity)."}, - {1, "Supports deleting entities (DeleteEntity)."}, - {2, "Supports predefined named poses (GetNamedPoses)."}, - {3, "Supports pose bounds (GetNamedPoseBounds)."}, - {4, "Supports entity tags in interfaces using EntityFilters, such as GetEntities."}, - {5, "Supports entity bounds (GetEntityBounds)."}, - {6, "Supports entity filtering with bounds with TYPE_BOX."}, - {7, "Supports entity filtering with Bounds TYPE_CONVEX_HULL."}, - {8, "Supports entity categories, such as in use with EntityFilters or SetEntityInfo."}, - {9, "Supports SpawnEntity resource_string field."}, - {10, "Supports GetEntityState interface."}, - {11, "Supports SetEntityState interface."}, - {12, "Supports GetEntityInfo interface."}, - {13, "Supports SetEntityInfo interface."}, - {14, "Supports GetSpawnables interface."}, - {20, "Supports one or more ways to reset the simulation through ResetSimulation."}, - {21, "Supports SCOPE_TIME flag for resetting"}, - {22, "Supports SCOPE_STATE flag for resetting."}, - {23, "Supports SCOPE_SPAWNED flag for resetting."}, - {24, "Supports GetSimulationState interface."}, - {25, "Supports SetSimulationState interface. Check SIMULATION_STATE_PAUSE feature for setting STATE_PAUSED."}, - {26, "Supports the STATE_PAUSED SimulationState in SetSimulationState interface."}, - {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."}, - }; -const std::map ScopeNameToId - { - {"SCOPE_DEFAULT", 0}, - {"SCOPE_TIME", 1}, - {"SCOPE_STATE", 2}, - {"SCOPE_SPAWNED", 4}, - {"SCOPE_ALL", 255}, - }; - -const std::map ErrorIdToName - { - {0, "RESULT_FEATURE_UNSUPPORTED"}, - {2, "RESULT_NOT_FOUND"}, - {3, "RESULT_INCORRECT_STATE"}, - {4, "RESULT_OPERATION_FAILED"}, - }; -const std::map SimStateIdToName - { - {0, "STATE_STOPPED"}, - {1, "STATE_PLAYING"}, - {2, "STATE_PAUSED"}, - {3, "STATE_QUITTING"}, - }; - - -const std::map SimStateNameToId - { - {"STATE_STOPPED", 0}, - {"STATE_PLAYING", 1}, - {"STATE_PAUSED", 2}, - {"STATE_QUITTING", 3}, - }; diff --git a/src/string_to_keys.h b/src/string_to_keys.h new file mode 100644 index 0000000..b22f47c --- /dev/null +++ b/src/string_to_keys.h @@ -0,0 +1,83 @@ +#pragma once + +#include +#include + +const std::map FeatureToName{ + {0, "SPAWNING"}, + {1, "DELETING"}, + {2, "NAMED_POSES"}, + {3, "POSE_BOUNDS"}, + {4, "ENTITY_TAGS"}, + {5, "ENTITY_BOUNDS"}, + {6, "ENTITY_BOUNDS_BOX"}, + {7, "ENTITY_BOUNDS_CONVEX"}, + {8, "ENTITY_CATEGORIES"}, + {9, "SPAWNING_RESOURCE_STRING"}, + {10, "ENTITY_STATE_GETTING"}, + {11, "ENTITY_STATE_SETTING"}, + {12, "ENTITY_INFO_GETTING"}, + {13, "ENTITY_INFO_SETTING"}, + {14, "SPAWNABLES"}, + {20, "SIMULATION_RESET"}, + {21, "SIMULATION_RESET_TIME"}, + {22, "SIMULATION_RESET_STATE"}, + {23, "SIMULATION_RESET_SPAWNED"}, + {24, "SIMULATION_STATE_GETTING"}, + {25, "SIMULATION_STATE_SETTING"}, + {26, "SIMULATION_STATE_PAUSE"}, + {31, "STEP_SIMULATION_SINGLE"}, + {32, "STEP_SIMULATION_MULTIPLE"}, + {33, "STEP_SIMULATION_ACTION"}, +}; +const std::map FeatureDescription{ + {0, "Supports spawn interface (SpawnEntity)."}, + {1, "Supports deleting entities (DeleteEntity)."}, + {2, "Supports predefined named poses (GetNamedPoses)."}, + {3, "Supports pose bounds (GetNamedPoseBounds)."}, + {4, "Supports entity tags in interfaces using EntityFilters, such as GetEntities."}, + {5, "Supports entity bounds (GetEntityBounds)."}, + {6, "Supports entity filtering with bounds with TYPE_BOX."}, + {7, "Supports entity filtering with Bounds TYPE_CONVEX_HULL."}, + {8, "Supports entity categories, such as in use with EntityFilters or SetEntityInfo."}, + {9, "Supports SpawnEntity resource_string field."}, + {10, "Supports GetEntityState interface."}, + {11, "Supports SetEntityState interface."}, + {12, "Supports GetEntityInfo interface."}, + {13, "Supports SetEntityInfo interface."}, + {14, "Supports GetSpawnables interface."}, + {20, "Supports one or more ways to reset the simulation through ResetSimulation."}, + {21, "Supports SCOPE_TIME flag for resetting"}, + {22, "Supports SCOPE_STATE flag for resetting."}, + {23, "Supports SCOPE_SPAWNED flag for resetting."}, + {24, "Supports GetSimulationState interface."}, + {25, "Supports SetSimulationState interface. Check SIMULATION_STATE_PAUSE feature for setting STATE_PAUSED."}, + {26, "Supports the STATE_PAUSED SimulationState in SetSimulationState interface."}, + {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."}, +}; +const std::unordered_map ScopeNameToId{ + {"SCOPE_DEFAULT", 0}, {"SCOPE_TIME", 1}, {"SCOPE_STATE", 2}, {"SCOPE_SPAWNED", 4}, {"SCOPE_ALL", 255}, +}; + +const std::map ErrorIdToName{ + {0, "RESULT_FEATURE_UNSUPPORTED"}, + {2, "RESULT_NOT_FOUND"}, + {3, "RESULT_INCORRECT_STATE"}, + {4, "RESULT_OPERATION_FAILED"}, +}; +const std::map SimStateIdToName{ + {0, "STATE_STOPPED"}, + {1, "STATE_PLAYING"}, + {2, "STATE_PAUSED"}, + {3, "STATE_QUITTING"}, +}; + + +const std::unordered_map SimStateNameToId{ + {"STATE_STOPPED", 0}, + {"STATE_PLAYING", 1}, + {"STATE_PAUSED", 2}, + {"STATE_QUITTING", 3}, +}; diff --git a/src/vector_utils.hpp b/src/vector_utils.hpp new file mode 100644 index 0000000..7264c93 --- /dev/null +++ b/src/vector_utils.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include +#include +#include +#include + +inline QString VectorToQstring(const tf2::Vector3& v) +{ + QString qString = QString::asprintf("%f %f %f", v.x(), v.y(), v.z()); + return qString; +} + +inline tf2::Vector3 QStringToVector(const QString& line) +{ + tf2::Vector3 v; + QStringList list = line.split(" "); + if (list.size() == 3) + { + v.setX(list[0].toDouble()); + v.setY(list[1].toDouble()); + v.setZ(list[2].toDouble()); + } + else + { + qWarning() << "Invalid input format. Expected 3 values."; + v = tf2::Vector3(0, 0, 0); + } + return v; +}