From 6a46d928db23177cc077969ffbae7f41d87736bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 9 Jul 2025 16:50:30 +0200 Subject: [PATCH 1/8] Add support for simulation interfaces in RViz2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This commit introduces a new panel for simulation interfaces in RViz2, allowing users to interact with various simulation plugins. The changes include: - Addition of a new `SimulationInterfacesPanel` class. - Integration of the panel into the RViz2 interface. - Updates to the CMake build system to include the new panel. - New icons and configuration files for the panel. - Added standalone app. Signed-off-by: Michał Pełka --- .clang-format | 55 ++ CLAUDE.md | 123 ++++ CMakeLists.txt | 50 +- README.md | 7 +- icons/SimulationInterfacesPanel.png | Bin 0 -> 3265 bytes .../simulation_panel.hpp | 29 + package.xml | 6 + q_simulation_plugins.xml | 5 + src/main.cpp | 20 - src/my_widget.cpp | 360 --------- src/my_widget.hpp | 40 - src/service.h | 189 ++++- src/sim_widget.ui | 208 +++--- src/simulation_panel.cpp | 115 +++ src/simulation_widget.cpp | 682 ++++++++++++++++++ src/simulation_widget.hpp | 97 +++ src/standalone_main.cpp | 51 ++ src/stringToKeys.h | 12 +- src/vector_utils.hpp | 27 + 19 files changed, 1531 insertions(+), 545 deletions(-) create mode 100644 .clang-format create mode 100644 CLAUDE.md create mode 100644 icons/SimulationInterfacesPanel.png create mode 100644 include/q_simulation_interfaces/simulation_panel.hpp create mode 100644 q_simulation_plugins.xml delete mode 100644 src/main.cpp delete mode 100644 src/my_widget.cpp delete mode 100644 src/my_widget.hpp create mode 100644 src/simulation_panel.cpp create mode 100644 src/simulation_widget.cpp create mode 100644 src/simulation_widget.hpp create mode 100644 src/standalone_main.cpp create mode 100644 src/vector_utils.hpp 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/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..f5e9132 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,123 @@ +# q_simulation_interfaces + +A ROS 2 package that provides a Qt-based GUI panel for controlling simulation parameters and entities. This package is designed to work as both an RViz2 plugin and a standalone application. + +## Project Overview + +This is a Qt5/C++ ROS 2 package that creates a GUI interface for the `simulation_interfaces` package. It provides comprehensive simulation control capabilities including entity spawning, state management, and simulation stepping. + +## Architecture + +- **Main Library**: `libq_simulation_interfaces.so` - RViz2 plugin library +- **Standalone Executable**: `q_simulation_interfaces_standalone` - Independent Qt application +- **Core Component**: `SimulationPanel` class - Main GUI controller + +## Key Files Structure + +### Source Files +- `src/simulation_panel.cpp` - Main panel implementation with all GUI logic +- `src/standalone_main.cpp` - Standalone application entry point +- `include/q_simulation_interfaces/simulation_panel.hpp` - Panel header +- `src/service.h` - Template-based ROS 2 service wrapper utility +- `src/stringToKeys.h` - String/key mapping utilities for simulation enums +- `src/vector_utils.hpp` - Vector utility functions for Qt/ROS conversions +- `src/sim_widget.ui` - Qt Designer UI file + +### Configuration Files +- `package.xml` - ROS 2 package manifest with Qt5 dependencies +- `CMakeLists.txt` - Build configuration with Qt5 and ROS 2 integration +- `q_simulation_plugins.xml` - RViz plugin description for pluginlib + +## Dependencies + +### ROS 2 Dependencies +- `rclcpp` - ROS 2 C++ client library +- `rclcpp_action` - ROS 2 action client/server +- `simulation_interfaces` - Main simulation service/message definitions +- `tf2` - Transform library for quaternion operations +- `pluginlib` - Plugin management for RViz integration +- `rviz_common` - RViz panel base classes + +### System Dependencies +- Qt5 (Widgets module) +- `qt5-qmake` and `qtbase5-dev` packages + +## Functionality + +### Entity Management +- **Spawn Entities**: Create new simulation entities from available spawnables +- **Get Entities**: List all current entities in simulation +- **Entity State**: Get/set position, orientation, and velocity of entities +- **Despawn**: Remove entities from simulation + +### Simulation Control +- **Step Simulation**: Advance simulation by specified number of steps (service or action) +- **Reset Simulation**: Reset simulation with different scopes +- **Get/Set Simulation State**: Control simulation running state +- **Get Features**: Query simulator capabilities + +### Service Integration +All functionality is accessed through ROS 2 services defined in `simulation_interfaces`: +- `/get_spawnables`, `/spawn_entity`, `/delete_entity` +- `/get_entities`, `/get_entity_state`, `/set_entity_state` +- `/get_simulation_features`, `/reset_simulation` +- `/get_simulation_state`, `/set_simulation_state` +- `/step_simulation`, `/simulate_steps` (action) + +## Build Instructions + +```bash +# In ROS 2 workspace +colcon build --packages-select q_simulation_interfaces +source install/setup.bash + +# Run as RViz plugin +rviz2 + +# Run standalone +ros2 run q_simulation_interfaces q_simulation_interfaces_standalone +``` + +## Usage Modes + +### RViz2 Plugin +- Integrates as a panel in RViz2 +- Access via Panels menu → Add New Panel → q_simulation_interfaces → SimulationPanel + +### Standalone Application +- Independent Qt application +- Full functionality without RViz2 dependency +- Useful for dedicated simulation control interfaces + +## Code Patterns + +### Service Template Pattern +The `Service` template class in `service.h` provides: +- Unified service calling interface +- Automatic timeout handling +- Error checking and user feedback via QMessageBox +- Special handling for different service types + +### Qt Integration +- Uses Qt5 widgets with Qt Designer UI files +- Signal/slot connections for GUI events +- Qt/ROS data type conversions (quaternions, vectors) +- Thread-safe action client handling + +### Plugin Architecture +- Implements `rviz_common::Panel` interface +- Uses pluginlib for dynamic loading +- Exports plugin via `PLUGINLIB_EXPORT_CLASS` macro + +## Development Notes + +- Uses C++17 standard +- CMake handles Qt5 MOC (Meta-Object Compiler) automatically +- Thread management for long-running actions (simulation stepping) +- Comprehensive error handling with user-friendly dialogs +- Maintains GUI state synchronization with simulation state + +## Git Status +Current branch: `mp/rviz2` (working on RViz2 integration) +Main branch: `master` +Recent commits focus on UI improvements and functionality additions. \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index b7f4470..b0992de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,29 +9,67 @@ find_package(Qt5 REQUIRED COMPONENTS Widgets) find_package(simulation_interfaces 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.hpp +) + +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..6da577d 100644 --- a/README.md +++ b/README.md @@ -5,14 +5,13 @@ 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. +I don't know. ROS 2 Humble and libqt5-dev at least and ros2-$ROS_DISTRO-simulation-interfaces. # Building ```bash -mkdir -p /tmp/ros2_ws/src -cd /tmp/ros2_ws/src -git clone https://github.com/ros-simulation/simulation_interfaces.git +mkdir -p ~/ros2_ws/src +cd ~/ros2_ws/src git clone https://github.com/michalpelka/q_simulation_interfaces.git cd .. colcon build --symlink-install diff --git a/icons/SimulationInterfacesPanel.png b/icons/SimulationInterfacesPanel.png new file mode 100644 index 0000000000000000000000000000000000000000..355c13e377fe37caeb07996e493fa848dea0c71e GIT binary patch literal 3265 zcmV;y3_kOTP)}o_L3Mv|0qmKI; zP1I3?SzO+kY%Xt%N8|F6WOU-pBqni;#z<76aY0QqYDS|X2rf|!%_1l&vO~+-AiX^O z^nJf%j+|p=GGC`Y-dq2=Rp(ayt4b2T2>+#`PRUOxPN_^uv0k)Bw{?S@(N`Yjj)g1h zuFb!h298f1T^$L~<#)c5eA+?lrF}~q(8)p`W(vXnH~R(0EVxm3^N;x*khLxQ^PirD z#y)B`Lffs_+X6C-nLVy)kP&u-8NAvp-80+_?*K3gc z#_Xi_wJ_0p{E(49fX=GBqMP(MfLjwv7T)<3_E`3Pe|!&^_Z43$-wmG6d%UV21(Sj+ z$JF}5OFQR}n7IZ#@x%ydWN%lT{3p0F;@T_bPn;xW9 zML=A6oPSI`cm;dn3CP!-7I_@xTMDGxIw)OKUjHx>Y!%LNt`KN0Z5MVk++K3`y$2mI zR1B$4JO=^z7W`yW!!XhC_{` zz!T5@MnI*yBju@%LP$*TG5>7P#Hq(g9|HLx^1*AnVV7W|c;!8^m`@C*Z8u(n0}?vp9hrPODj_c7>F z;GJMh0%8 z03Ob1Ol#>5u?uu zvg_s6nnMt_E_71hQW$0%A}8JhQ=#dmS3Llu@qg^dfb;!E_i2FV3MSTBegLzo@Gqt5 zV7l-1w6Oxlb{YNd;F%D3IH0%xX5bTG4De4DQjRI-lxiWGW;JEE=!K|As<2o07Q#Bq zy07gkA;eTMQ!EhwT4svbVv!Km=dB0Z(u63tRGg`*5u$NtlSgy15bhoBJ?``3H|5c} zNA`|mLR7dbzppM6qJCO^*~5)OD6^C$%JwJqrT=ysRsGAGNMacDE9VLd?%y^nVnw2u z4V>XeGCN(kasMZHR2T{UPt^AEHHX1A&9=#&2n7=g*A=%x)~4){*J9vqo z{6-D~)Tn1cjE`n_!~!Z*zlL`obDwwD zz=P8AgOv`*o{-b@QySz-H<#xxfmXM5UfYsi2|(U0UzDTZdT4H%S%Ph6cg{bw9k!Hj zKY4H&q`sTxJoXCQnNm9Keot^uanE;u+^GRQ@A`x5uaNh%IkqSZc0SsjapaG%MZ3M{ zq3>abaNze(~dL&2aTf!E+Cm1GV&`8p^KMgf_2)#J3YJ z$Bu@uqA+uC2sqzxyyjR8We>{6K9~&O&OaHKSp=7&ZhTOB3zV+zYEc9g3+roIZ-DNq zR;Z4G+E-=87vlQ#?iQm5|I7Y){`Vn2xbTPKTX4aclbl};4?k|n=qLi>Sw&y)>f%+J z_kS=r>zUd8`#=QYC82=bpF#al%W{hY zZfw7Pr!}_{P69DNN^3cUufO~9?{xqB!Fw6xTJ0bA3v`kT`EL0gc^PGKsQ{wT@?aW zyY;m7BD8 zWZMF0F}LKm6o6xgBg-)q-0Rt<{tTpW)iP}|1dj0kgI{0pJmNV|Zv*v`zJ2)@w_&M<8f`jMGlF43gU;yNAw*By>|9G6>|rSqm41fYG4+#nEAMff3uOf#<)hh z20_Ezh8G(aK~+xG<*KVt>#B9tx}bGp>%`WHAh*e_auc}Ub+2*118Rp_R;xhiu5?qv zLHbHcl~O@>Q)kxQ0@HNUbklSQwS-ziEfBsWd~x_92#5}d3HBpzKt4~*-u0G)Rq2Gso ze+Kn_^%3y5gi$Bxu_)E%;{ zfRk%~NV@PoT%4QZmuG^&v;Jv5b7A_($sr^E4#Rg2tr_G2+E2AhwVt4HCOVUyx8O|2 zk0Y`d!pZE5h2~ZukXNJtC@Clpv8;yfmgocFCm_Tcd@d*hJn+CU2gDh7k%)$pq&qtv z?1S@PU+!Bn6+B`MC$x5Gnb@Y-=Yr3gE*t!Mz`$7p4kq*i{Z;)!U8hv|`S<3Eq8@Nm zcj|Ud0MwcrFSHc{+>`27fNG_xLrR6%H+n@q^)+-QWNA<#Xxxp$Ds*67+&1{pLXd`# zB@G54qaq4;NIgby{KFaIw1C^!ebgP_Y3A_|CJ_y7gWB^Ry$K!T9oJj~pkaiy#eNG^ z$s8vWY=>>v>?=VDR=T;PegRiMw$riHmSw*Ovd-0CSqB6oNh3h&%|H~euXOBj7K7YJ zeqEm4X_U|p)cI+5!;(gJ8gkKB>`5z_?5=G=UI&*KdtyDhydT$lSLTf@TEX2uSG6Les?y;w2$Eq>gd6 zHYj&jn5zau23F#9J~Mfc30qMW=;Eq>23^T?l2N^1sR+A>^&{QQ0Mg(xBMg z5kiP^Q6Z}S%_ee3l!|*oD6(7cjuS%uP~Iyy3!#iwW-9LqAu>d!$oV&TyRZwp5bpKv zP43-7$mw#n%TEZ`LD!G24k5%=v0Z%iB<%kIAT=eiyE_N=00000NkvXXu0mjftA +#include +#include +#include + +namespace q_simulation_interfaces +{ + class SimulationWidget; + class SimulationPanel : public rviz_common::Panel + { + public: + explicit SimulationPanel(QWidget* parent = nullptr); + + ~SimulationPanel() override; + void onInitialize() override; + QString getName() const; + 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..328b28f 100644 --- a/src/service.h +++ b/src/service.h @@ -1,41 +1,176 @@ #pragma once #include -#include -#include -#include -#include -#include #include -#include #include -#include -#include -#include #include #include #include +#include +#include +#include #include "stringToKeys.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 Service { +class Expected { +private: + 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_); } + + //! 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)>; - Service(const std::string &service_name, rclcpp::Node::SharedPtr node) + //! 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) { - if (!client_->wait_for_service(std::chrono::seconds(10))) { - RCLCPP_ERROR(node->get_logger(), "Service not available after waiting"); + node_(node), timeout_(std::chrono::milliseconds(static_cast(timeout))) { + } + + 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_INFO(node_->get_logger(), "Service %s is still processing", client_->get_service_name()); + } + + //! 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); @@ -48,13 +183,13 @@ class Service { } } private: - std::optional call_service_sync_Check(const Request &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) != + 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) { @@ -69,24 +204,28 @@ class Service { } 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() ) { + 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) != + 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..4283b9c 100644 --- a/src/sim_widget.ui +++ b/src/sim_widget.ui @@ -7,7 +7,7 @@ 0 0 675 - 869 + 893 @@ -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,36 @@ - - + + - RotAngle + VelRotY - - - - 0 0 1 + + + + 6 - - - - - - - - - VelX: + + -99.000000000000000 - - - - - - RotVector + + 0.001000000000000 - - - - - - PosZ: + + QAbstractSpinBox::AdaptiveDecimalStepType - - + + - VelRotZ + VelZ: - - - - - - @@ -269,35 +270,37 @@ - - - - - - - - + + - VelY: + RotAngle - - - - - - - 6 + + + + VelX: + + + + - -99.000000000000000 + -999999.000000000000000 - - 0.001000000000000 + + 999999999999999.000000000000000 - - QAbstractSpinBox::AdaptiveDecimalStepType + + + + + + + + + Frame @@ -467,6 +470,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..9e5dce3 --- /dev/null +++ b/src/simulation_panel.cpp @@ -0,0 +1,115 @@ +#include +#include +#include "simulation_widget.hpp" +#include +#include +namespace q_simulation_interfaces +{ + namespace + { + constexpr char InteractiveMarkerClassId[] = "rviz_default_plugins/InteractiveMarkers"; + constexpr char InteractiveMarkerNamespacePropertyName[] = "Interactive Markers 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"); + } + simulationWidget_->intiliaze(node_ptr_->get_raw_node()); + + auto context = this->getDisplayContext(); + 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("Interactive Markers Namespace"); + assert(property); + property->setValue(QString::fromStdString(InteractiveMarkerNamespaceValue)); + display_group->addDisplay(im_display_); + } + }; + + void SimulationPanel::hideEvent(QHideEvent* event) + { + std::cout << "HideEvent" << std::endl; + 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..1751a8d --- /dev/null +++ b/src/simulation_widget.cpp @@ -0,0 +1,682 @@ +#include "simulation_widget.hpp" +#include +#include +#include +#include +#include +#include +#include "service.h" +#include "stringToKeys.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) + { + 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) + { + std::cout << "Simulation widget created" << std::endl; + ui_->setupUi(this); + ui_->frameStateLineEdit->setText("map"); + ui_->spawnFrameLineEdit->setText("map"); + for (const auto& [name, _] : ScopeNameToId) + { + ui_->resetModeCombo->addItem(QString::fromStdString(name)); + } + for (const auto& [name, _] : SimStateNameToId) + { + 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::intiliaze(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(getEntityStateService_); + + + getSimFeaturesService_ = std::make_shared>( + "/get_simulation_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::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](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + const std::shared_ptr feedback) + { + float progress = static_cast(feedback->completed_steps) / feedback->remaining_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 = 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 = 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 = 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); + ui_->simProgressBar->setValue(static_cast(this->actionThreadProgress_ * 100)); + } + else + { + ui_->stepSimButtonAction->setEnabled(true); + ui_->stepSimServiceButton->setEnabled(true); + } + 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->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE) + { + 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); + interactiveMarkerServer_->applyChanges(); + } + + void SimulationWidget::UpdateSpawnPointMarker() + { + if (!interactiveMarkerServer_) + { + return; + } + + // Get current marker + visualization_msgs::msg::InteractiveMarker spawnMarker; + if (!interactiveMarkerServer_->get("spawn_point", spawnMarker)) + { + // Marker doesn't exist, create it + CreateSpawnPointMarker(); + 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(); + } + +} // namespace q_simulation_interfaces diff --git a/src/simulation_widget.hpp b/src/simulation_widget.hpp new file mode 100644 index 0000000..91fd0d2 --- /dev/null +++ b/src/simulation_widget.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include "service.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 intiliaze(rclcpp::Node::SharedPtr node = nullptr); + + 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(); + + //! 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_; + }; +} diff --git a/src/standalone_main.cpp b/src/standalone_main.cpp new file mode 100644 index 0000000..392eb3b --- /dev/null +++ b/src/standalone_main.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include "simulation_widget.hpp" + +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_->intiliaze(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 index b930c56..054d744 100644 --- a/src/stringToKeys.h +++ b/src/stringToKeys.h @@ -3,7 +3,7 @@ #include #include -const std::map FeatureToName +const std::unordered_map FeatureToName { {0, "SPAWNING"}, {1, "DELETING"}, @@ -31,7 +31,7 @@ const std::map FeatureToName {32, "STEP_SIMULATION_MULTIPLE"}, {33, "STEP_SIMULATION_ACTION"}, }; -const std::map FeatureDescription +const std::unordered_map FeatureDescription { {0, "Supports spawn interface (SpawnEntity)."}, {1, "Supports deleting entities (DeleteEntity)."}, @@ -59,7 +59,7 @@ const std::map FeatureDescription {32, "Supports multi-stepping through simulation, either through StepSimulation service or StepSimulation action."}, {33, "Supports SimulateSteps action interface."}, }; -const std::map ScopeNameToId +const std::unordered_map ScopeNameToId { {"SCOPE_DEFAULT", 0}, {"SCOPE_TIME", 1}, @@ -68,14 +68,14 @@ const std::map ScopeNameToId {"SCOPE_ALL", 255}, }; -const std::map ErrorIdToName +const std::unordered_map ErrorIdToName { {0, "RESULT_FEATURE_UNSUPPORTED"}, {2, "RESULT_NOT_FOUND"}, {3, "RESULT_INCORRECT_STATE"}, {4, "RESULT_OPERATION_FAILED"}, }; -const std::map SimStateIdToName +const std::unordered_map SimStateIdToName { {0, "STATE_STOPPED"}, {1, "STATE_PLAYING"}, @@ -84,7 +84,7 @@ const std::map SimStateIdToName }; -const std::map SimStateNameToId +const std::unordered_map SimStateNameToId { {"STATE_STOPPED", 0}, {"STATE_PLAYING", 1}, diff --git a/src/vector_utils.hpp b/src/vector_utils.hpp new file mode 100644 index 0000000..f9b529f --- /dev/null +++ b/src/vector_utils.hpp @@ -0,0 +1,27 @@ +#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; +} From a0db0e5d08daa13333ccfa47825be8dd2660f97c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 6 Aug 2025 12:45:28 +0200 Subject: [PATCH 2/8] Updated to AI review. Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/service.h | 2 +- src/simulation_panel.cpp | 2 +- src/simulation_widget.cpp | 8 ++++---- src/simulation_widget.hpp | 2 +- src/standalone_main.cpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/service.h b/src/service.h index 328b28f..4bcf126 100644 --- a/src/service.h +++ b/src/service.h @@ -160,7 +160,7 @@ class Service : public ServiceInterface { service_result_.reset(); service_called_time_.reset(); } - RCLCPP_INFO(node_->get_logger(), "Service %s is still processing", client_->get_service_name()); + RCLCPP_DEBUG(node_->get_logger(), "Service %s is still processing", client_->get_service_name()); } //! Calls the service synchronously and checks the response. diff --git a/src/simulation_panel.cpp b/src/simulation_panel.cpp index 9e5dce3..959533d 100644 --- a/src/simulation_panel.cpp +++ b/src/simulation_panel.cpp @@ -57,7 +57,7 @@ namespace q_simulation_interfaces { throw std::runtime_error("Failed to get ROS node abstraction"); } - simulationWidget_->intiliaze(node_ptr_->get_raw_node()); + simulationWidget_->initialize(node_ptr_->get_raw_node()); auto context = this->getDisplayContext(); auto display_group = context->getRootDisplayGroup(); diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index 1751a8d..a4430ce 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -38,7 +38,7 @@ namespace q_simulation_interfaces std::vector axesR; if (rotationZ) { - CreateQuaternion(1, 0, 1, 0); // z-axis + axesR.push_back(CreateQuaternion(1, 0, 1, 0)); // z-axis } for (const auto& axis : axesT) @@ -114,7 +114,7 @@ namespace q_simulation_interfaces delete ui_; } - void SimulationWidget::intiliaze(rclcpp::Node::SharedPtr node) + void SimulationWidget::initialize(rclcpp::Node::SharedPtr node) { node_ = node; // tf transform listener @@ -144,7 +144,7 @@ namespace q_simulation_interfaces deleteEntityService_ = std::make_shared>("/delete_entity", node); - serviceInterfaces_.push_back(getEntityStateService_); + serviceInterfaces_.push_back(deleteEntityService_); getSimFeaturesService_ = std::make_shared>( @@ -539,7 +539,7 @@ namespace q_simulation_interfaces auto cb = [this](auto response) { ProduceWarningIfProblem(this, "SpawnEntity", response); - if (!response && response->result.result == simulation_interfaces::msg::Result::RESULT_OK) + 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); diff --git a/src/simulation_widget.hpp b/src/simulation_widget.hpp index 91fd0d2..0ed7f69 100644 --- a/src/simulation_widget.hpp +++ b/src/simulation_widget.hpp @@ -39,7 +39,7 @@ namespace q_simulation_interfaces ~SimulationWidget() override; - void intiliaze(rclcpp::Node::SharedPtr node = nullptr); + void initialize(rclcpp::Node::SharedPtr node = nullptr); private: void GetSpawnables(); diff --git a/src/standalone_main.cpp b/src/standalone_main.cpp index 392eb3b..0321012 100644 --- a/src/standalone_main.cpp +++ b/src/standalone_main.cpp @@ -17,7 +17,7 @@ class StandaloneWidget : public QWidget // Create a mock display context for the panel panel_ = new q_simulation_interfaces::SimulationWidget(this); - panel_->intiliaze(node_); + panel_->initialize(node_); layout->addWidget(panel_); node_thread_ = std::thread([this]() { rclcpp::spin(node_); From 61d516dbeb5e375e197af0d280642cb9e9724b22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 6 Aug 2025 14:38:03 +0200 Subject: [PATCH 3/8] Adjust service name MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- src/simulation_widget.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index a4430ce..d766a79 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -148,7 +148,7 @@ namespace q_simulation_interfaces getSimFeaturesService_ = std::make_shared>( - "/get_simulation_features", node); + "/get_simulator_features", node); serviceInterfaces_.push_back(getSimFeaturesService_); resetSimulationService_ = @@ -589,13 +589,13 @@ namespace q_simulation_interfaces { ui_->stepSimButtonAction->setEnabled(false); ui_->stepSimServiceButton->setEnabled(false); - ui_->simProgressBar->setValue(static_cast(this->actionThreadProgress_ * 100)); } else { ui_->stepSimButtonAction->setEnabled(true); ui_->stepSimServiceButton->setEnabled(true); } + ui_->simProgressBar->setValue(static_cast(this->actionThreadProgress_ * 100)); for (auto& service : serviceInterfaces_) { if (service) From 38bffd6be44b43eff41d4219115eb727f06bf4a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 6 Aug 2025 16:19:40 +0200 Subject: [PATCH 4/8] stability fixes - moving spawn point gizmo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- src/simulation_panel.cpp | 6 ++--- src/simulation_widget.cpp | 54 ++++++++++++++++++++++++++------------- src/simulation_widget.hpp | 6 ++++- 3 files changed, 44 insertions(+), 22 deletions(-) diff --git a/src/simulation_panel.cpp b/src/simulation_panel.cpp index 959533d..887a987 100644 --- a/src/simulation_panel.cpp +++ b/src/simulation_panel.cpp @@ -57,9 +57,10 @@ namespace q_simulation_interfaces { 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 context = this->getDisplayContext(); auto display_group = context->getRootDisplayGroup(); assert(display_group); @@ -93,7 +94,7 @@ namespace q_simulation_interfaces im_display_->setEnabled(true); im_display_->setShouldBeSaved(false); im_display_->setName("Simulation Interactive Markers"); - auto property = im_display_->findProperty("Interactive Markers Namespace"); + auto property = im_display_->findProperty(InteractiveMarkerNamespacePropertyName); assert(property); property->setValue(QString::fromStdString(InteractiveMarkerNamespaceValue)); display_group->addDisplay(im_display_); @@ -102,7 +103,6 @@ namespace q_simulation_interfaces void SimulationPanel::hideEvent(QHideEvent* event) { - std::cout << "HideEvent" << std::endl; if (im_display_) { im_display_ = nullptr; diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index d766a79..061d7a0 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -67,8 +67,8 @@ namespace q_simulation_interfaces { std::cout << "Simulation widget created" << std::endl; ui_->setupUi(this); - ui_->frameStateLineEdit->setText("map"); - ui_->spawnFrameLineEdit->setText("map"); + ui_->frameStateLineEdit->setText("panda_link0"); + ui_->spawnFrameLineEdit->setText("panda_link0"); for (const auto& [name, _] : ScopeNameToId) { ui_->resetModeCombo->addItem(QString::fromStdString(name)); @@ -114,6 +114,7 @@ namespace q_simulation_interfaces delete ui_; } + void SimulationWidget::initialize(rclcpp::Node::SharedPtr node) { node_ = node; @@ -174,6 +175,27 @@ namespace q_simulation_interfaces 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) @@ -636,18 +658,18 @@ namespace q_simulation_interfaces // Set up feedback callback to update GUI when marker is moved interactive_markers::InteractiveMarkerServer::FeedbackCallback feedbackCb = [this](const auto& feedback) { - if (feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE) + if (feedback && feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE && 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); + 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); - interactiveMarkerServer_->applyChanges(); } void SimulationWidget::UpdateSpawnPointMarker() @@ -657,15 +679,6 @@ namespace q_simulation_interfaces return; } - // Get current marker - visualization_msgs::msg::InteractiveMarker spawnMarker; - if (!interactiveMarkerServer_->get("spawn_point", spawnMarker)) - { - // Marker doesn't exist, create it - CreateSpawnPointMarker(); - return; - } - // Update position from GUI geometry_msgs::msg::Pose newPose; std_msgs::msg::Header newHeader; @@ -678,5 +691,10 @@ namespace q_simulation_interfaces 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.hpp b/src/simulation_widget.hpp index 0ed7f69..70d8f86 100644 --- a/src/simulation_widget.hpp +++ b/src/simulation_widget.hpp @@ -38,10 +38,14 @@ namespace q_simulation_interfaces 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(); From 5879e13bb9748fa9dad2098ca7fb1f490a8b97df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 6 Aug 2025 17:36:58 +0200 Subject: [PATCH 5/8] Change to map from unordered map MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- README.md | 2 -- src/simulation_widget.cpp | 3 ++- src/stringToKeys.h | 8 ++++---- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 6da577d..07bd49a 100644 --- a/README.md +++ b/README.md @@ -18,5 +18,3 @@ colcon build --symlink-install source install/setup.bash ros2 run q_simulation_interfaces q_simulation_interfaces ``` - - diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index 061d7a0..b290e83 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -69,11 +69,12 @@ namespace q_simulation_interfaces ui_->setupUi(this); ui_->frameStateLineEdit->setText("panda_link0"); ui_->spawnFrameLineEdit->setText("panda_link0"); + for (const auto& [name, _] : ScopeNameToId) { ui_->resetModeCombo->addItem(QString::fromStdString(name)); } - for (const auto& [name, _] : SimStateNameToId) + for (const auto& [_, name] : SimStateIdToName) { ui_->simStateToSetComboBox->addItem(QString::fromStdString(name)); } diff --git a/src/stringToKeys.h b/src/stringToKeys.h index 054d744..cf25da2 100644 --- a/src/stringToKeys.h +++ b/src/stringToKeys.h @@ -3,7 +3,7 @@ #include #include -const std::unordered_map FeatureToName +const std::map FeatureToName { {0, "SPAWNING"}, {1, "DELETING"}, @@ -31,7 +31,7 @@ const std::unordered_map FeatureToName {32, "STEP_SIMULATION_MULTIPLE"}, {33, "STEP_SIMULATION_ACTION"}, }; -const std::unordered_map FeatureDescription +const std::map FeatureDescription { {0, "Supports spawn interface (SpawnEntity)."}, {1, "Supports deleting entities (DeleteEntity)."}, @@ -68,14 +68,14 @@ const std::unordered_map ScopeNameToId {"SCOPE_ALL", 255}, }; -const std::unordered_map ErrorIdToName +const std::map ErrorIdToName { {0, "RESULT_FEATURE_UNSUPPORTED"}, {2, "RESULT_NOT_FOUND"}, {3, "RESULT_INCORRECT_STATE"}, {4, "RESULT_OPERATION_FAILED"}, }; -const std::unordered_map SimStateIdToName +const std::map SimStateIdToName { {0, "STATE_STOPPED"}, {1, "STATE_PLAYING"}, From 68305082cfa4aad32829baa4385e6e72a0e67fd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 20 Aug 2025 13:14:04 +0200 Subject: [PATCH 6/8] adjust to review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- CLAUDE.md | 123 ------------------ CMakeLists.txt | 6 +- README.md | 18 ++- ...el.png => simulation_interfaces_panel.png} | Bin ...imulation_panel.hpp => simulation_panel.h} | 3 +- src/service.h | 12 +- src/simulation_panel.cpp | 6 +- src/simulation_widget.cpp | 18 +-- ...ulation_widget.hpp => simulation_widget.h} | 2 +- src/standalone_main.cpp | 4 +- src/{stringToKeys.h => string_to_keys.h} | 0 11 files changed, 39 insertions(+), 153 deletions(-) delete mode 100644 CLAUDE.md rename icons/{SimulationInterfacesPanel.png => simulation_interfaces_panel.png} (100%) rename include/q_simulation_interfaces/{simulation_panel.hpp => simulation_panel.h} (93%) rename src/{simulation_widget.hpp => simulation_widget.h} (98%) rename src/{stringToKeys.h => string_to_keys.h} (100%) diff --git a/CLAUDE.md b/CLAUDE.md deleted file mode 100644 index f5e9132..0000000 --- a/CLAUDE.md +++ /dev/null @@ -1,123 +0,0 @@ -# q_simulation_interfaces - -A ROS 2 package that provides a Qt-based GUI panel for controlling simulation parameters and entities. This package is designed to work as both an RViz2 plugin and a standalone application. - -## Project Overview - -This is a Qt5/C++ ROS 2 package that creates a GUI interface for the `simulation_interfaces` package. It provides comprehensive simulation control capabilities including entity spawning, state management, and simulation stepping. - -## Architecture - -- **Main Library**: `libq_simulation_interfaces.so` - RViz2 plugin library -- **Standalone Executable**: `q_simulation_interfaces_standalone` - Independent Qt application -- **Core Component**: `SimulationPanel` class - Main GUI controller - -## Key Files Structure - -### Source Files -- `src/simulation_panel.cpp` - Main panel implementation with all GUI logic -- `src/standalone_main.cpp` - Standalone application entry point -- `include/q_simulation_interfaces/simulation_panel.hpp` - Panel header -- `src/service.h` - Template-based ROS 2 service wrapper utility -- `src/stringToKeys.h` - String/key mapping utilities for simulation enums -- `src/vector_utils.hpp` - Vector utility functions for Qt/ROS conversions -- `src/sim_widget.ui` - Qt Designer UI file - -### Configuration Files -- `package.xml` - ROS 2 package manifest with Qt5 dependencies -- `CMakeLists.txt` - Build configuration with Qt5 and ROS 2 integration -- `q_simulation_plugins.xml` - RViz plugin description for pluginlib - -## Dependencies - -### ROS 2 Dependencies -- `rclcpp` - ROS 2 C++ client library -- `rclcpp_action` - ROS 2 action client/server -- `simulation_interfaces` - Main simulation service/message definitions -- `tf2` - Transform library for quaternion operations -- `pluginlib` - Plugin management for RViz integration -- `rviz_common` - RViz panel base classes - -### System Dependencies -- Qt5 (Widgets module) -- `qt5-qmake` and `qtbase5-dev` packages - -## Functionality - -### Entity Management -- **Spawn Entities**: Create new simulation entities from available spawnables -- **Get Entities**: List all current entities in simulation -- **Entity State**: Get/set position, orientation, and velocity of entities -- **Despawn**: Remove entities from simulation - -### Simulation Control -- **Step Simulation**: Advance simulation by specified number of steps (service or action) -- **Reset Simulation**: Reset simulation with different scopes -- **Get/Set Simulation State**: Control simulation running state -- **Get Features**: Query simulator capabilities - -### Service Integration -All functionality is accessed through ROS 2 services defined in `simulation_interfaces`: -- `/get_spawnables`, `/spawn_entity`, `/delete_entity` -- `/get_entities`, `/get_entity_state`, `/set_entity_state` -- `/get_simulation_features`, `/reset_simulation` -- `/get_simulation_state`, `/set_simulation_state` -- `/step_simulation`, `/simulate_steps` (action) - -## Build Instructions - -```bash -# In ROS 2 workspace -colcon build --packages-select q_simulation_interfaces -source install/setup.bash - -# Run as RViz plugin -rviz2 - -# Run standalone -ros2 run q_simulation_interfaces q_simulation_interfaces_standalone -``` - -## Usage Modes - -### RViz2 Plugin -- Integrates as a panel in RViz2 -- Access via Panels menu → Add New Panel → q_simulation_interfaces → SimulationPanel - -### Standalone Application -- Independent Qt application -- Full functionality without RViz2 dependency -- Useful for dedicated simulation control interfaces - -## Code Patterns - -### Service Template Pattern -The `Service` template class in `service.h` provides: -- Unified service calling interface -- Automatic timeout handling -- Error checking and user feedback via QMessageBox -- Special handling for different service types - -### Qt Integration -- Uses Qt5 widgets with Qt Designer UI files -- Signal/slot connections for GUI events -- Qt/ROS data type conversions (quaternions, vectors) -- Thread-safe action client handling - -### Plugin Architecture -- Implements `rviz_common::Panel` interface -- Uses pluginlib for dynamic loading -- Exports plugin via `PLUGINLIB_EXPORT_CLASS` macro - -## Development Notes - -- Uses C++17 standard -- CMake handles Qt5 MOC (Meta-Object Compiler) automatically -- Thread management for long-running actions (simulation stepping) -- Comprehensive error handling with user-friendly dialogs -- Maintains GUI state synchronization with simulation state - -## Git Status -Current branch: `mp/rviz2` (working on RViz2 integration) -Main branch: `master` -Recent commits focus on UI improvements and functionality additions. \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index b0992de..d94b544 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ 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) @@ -23,7 +23,7 @@ set(CMAKE_CXX_STANDARD 17) # Add UI file qt5_wrap_cpp(MOC_FILES - include/q_simulation_interfaces/simulation_panel.hpp + include/q_simulation_interfaces/simulation_panel.h ) set(SOURCES src/simulation_widget.cpp src/simulation_panel.cpp) @@ -46,7 +46,7 @@ target_link_libraries(${PROJECT_NAME} ) # Create standalone executable -add_executable(${PROJECT_NAME}_standalone src/standalone_main.cpp src/simulation_widget.cpp ${MOC_FILES}) +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) diff --git a/README.md b/README.md index 07bd49a..001888b 100644 --- a/README.md +++ b/README.md @@ -5,16 +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 and ros2-$ROS_DISTRO-simulation-interfaces. +```shell +sudo apt install ros-$ROS_DISTRO-simulation-interfaces libqt5-dev +``` # Building -```bash +```shell mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src -git clone https://github.com/michalpelka/q_simulation_interfaces.git +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/simulation_interfaces_panel.png similarity index 100% rename from icons/SimulationInterfacesPanel.png rename to icons/simulation_interfaces_panel.png diff --git a/include/q_simulation_interfaces/simulation_panel.hpp b/include/q_simulation_interfaces/simulation_panel.h similarity index 93% rename from include/q_simulation_interfaces/simulation_panel.hpp rename to include/q_simulation_interfaces/simulation_panel.h index bc0c69d..1a053ea 100644 --- a/include/q_simulation_interfaces/simulation_panel.hpp +++ b/include/q_simulation_interfaces/simulation_panel.h @@ -11,12 +11,13 @@ 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; + QString getName() const override; void hideEvent(QHideEvent* event) override; private: diff --git a/src/service.h b/src/service.h index 4bcf126..fab6152 100644 --- a/src/service.h +++ b/src/service.h @@ -1,20 +1,18 @@ #pragma once -#include -#include -#include +#include #include #include +#include +#include +#include #include #include -#include -#include -#include "stringToKeys.h" +#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 { -private: std::optional value_; std::string error_; diff --git a/src/simulation_panel.cpp b/src/simulation_panel.cpp index 887a987..b450828 100644 --- a/src/simulation_panel.cpp +++ b/src/simulation_panel.cpp @@ -1,8 +1,8 @@ -#include #include -#include "simulation_widget.hpp" -#include +#include #include +#include +#include "simulation_widget.h" namespace q_simulation_interfaces { namespace diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index b290e83..8fec075 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -1,12 +1,13 @@ -#include "simulation_widget.hpp" +#include "simulation_widget.h" #include #include +#include #include #include #include #include #include "service.h" -#include "stringToKeys.h" +#include "string_to_keys.h" #include "ui_sim_widget.h" #include "vector_utils.hpp" @@ -65,10 +66,9 @@ namespace q_simulation_interfaces SimulationWidget::SimulationWidget(QWidget* parent) : QWidget(parent), ui_(new Ui::simWidgetUi) { - std::cout << "Simulation widget created" << std::endl; ui_->setupUi(this); - ui_->frameStateLineEdit->setText("panda_link0"); - ui_->spawnFrameLineEdit->setText("panda_link0"); + ui_->frameStateLineEdit->setText("world"); + ui_->spawnFrameLineEdit->setText("world"); for (const auto& [name, _] : ScopeNameToId) { @@ -254,10 +254,10 @@ namespace q_simulation_interfaces auto goal = std::make_shared(); goal->steps = steps; send_goal_options.feedback_callback = - [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, + [this, steps](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, const std::shared_ptr feedback) { - float progress = static_cast(feedback->completed_steps) / feedback->remaining_steps; + float progress = static_cast(feedback->completed_steps) / steps; actionThreadProgress_.store(progress); }; send_goal_options.goal_response_callback = @@ -498,7 +498,7 @@ namespace q_simulation_interfaces pose.orientation.w); const auto axis = q.getAxis(); - const auto angle = q.getAngle(); + const auto angle = qRadiansToDegrees(q.getAngle()); ui_->RotVector->setText(VectorToQstring(axis)); ui_->RotAngle->setValue(angle); ui_->frameStateLineEdit->setText(QString::fromStdString(feedback->header.frame_id)); @@ -526,7 +526,7 @@ namespace q_simulation_interfaces request.state.pose.position.z = ui_->StatePosZ->value(); const QString vectorStr = ui_->RotVector->text(); - const double angle = ui_->RotAngle->value(); + 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(); diff --git a/src/simulation_widget.hpp b/src/simulation_widget.h similarity index 98% rename from src/simulation_widget.hpp rename to src/simulation_widget.h index 70d8f86..6ac05b4 100644 --- a/src/simulation_widget.hpp +++ b/src/simulation_widget.h @@ -12,7 +12,7 @@ #include #include - +#include #include #include #include diff --git a/src/standalone_main.cpp b/src/standalone_main.cpp index 0321012..187264e 100644 --- a/src/standalone_main.cpp +++ b/src/standalone_main.cpp @@ -1,7 +1,7 @@ #include -#include #include -#include "simulation_widget.hpp" +#include +#include "simulation_widget.h" class StandaloneWidget : public QWidget { diff --git a/src/stringToKeys.h b/src/string_to_keys.h similarity index 100% rename from src/stringToKeys.h rename to src/string_to_keys.h From 7fb16ad2282250a1176da82ac6c9ad0095299959 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 20 Aug 2025 14:01:18 +0200 Subject: [PATCH 7/8] Clang format MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- ...anel.png => SimulationInterfacesPanel.png} | Bin .../simulation_panel.h | 4 +- src/service.h | 121 ++++++++----- src/simulation_panel.cpp | 18 +- src/simulation_widget.cpp | 20 +-- src/simulation_widget.h | 16 +- src/standalone_main.cpp | 26 +-- src/string_to_keys.h | 162 ++++++++---------- src/vector_utils.hpp | 15 +- 9 files changed, 202 insertions(+), 180 deletions(-) rename icons/{simulation_interfaces_panel.png => SimulationInterfacesPanel.png} (100%) diff --git a/icons/simulation_interfaces_panel.png b/icons/SimulationInterfacesPanel.png similarity index 100% rename from icons/simulation_interfaces_panel.png rename to icons/SimulationInterfacesPanel.png diff --git a/include/q_simulation_interfaces/simulation_panel.h b/include/q_simulation_interfaces/simulation_panel.h index 1a053ea..0e7e20b 100644 --- a/include/q_simulation_interfaces/simulation_panel.h +++ b/include/q_simulation_interfaces/simulation_panel.h @@ -2,9 +2,9 @@ #include -#include #include #include +#include namespace q_simulation_interfaces { @@ -22,7 +22,7 @@ namespace q_simulation_interfaces private: SimulationWidget* simulationWidget_; - rviz_common::Display *im_display_ {nullptr}; + rviz_common::Display* im_display_{nullptr}; std::shared_ptr node_ptr_; }; } // namespace q_simulation_interfaces diff --git a/src/service.h b/src/service.h index fab6152..540cba8 100644 --- a/src/service.h +++ b/src/service.h @@ -11,8 +11,9 @@ //! 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 { +template +class Expected +{ std::optional value_; std::string error_; @@ -31,13 +32,25 @@ class Expected { 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_; } + 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_); } + const T* operator->() const + { + assert(value_.has_value()); + return &(*value_); + } //! Returns the error message if it exists, otherwise returns an empty string. - const std::string& error() const { assert(!value_.has_value()); return error_; } + 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. @@ -45,27 +58,31 @@ class Expected { //! @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()) { +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); - } + 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 { +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. @@ -75,8 +92,9 @@ class ServiceInterface { //! 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 { +template +class Service : public ServiceInterface +{ public: using Request = typename T::Request; using Response = typename T::Response; @@ -88,16 +106,18 @@ class Service : public ServiceInterface { 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. + //! 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, double timeout = 1000) : + client_(node->create_client(service_name)), node_(node), + timeout_(std::chrono::milliseconds(static_cast(timeout))) + { } virtual ~Service() = default; @@ -126,14 +146,18 @@ class Service : public ServiceInterface { //! 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; + void check_service_result() override + { + if (!service_result_) + { + return; } // check duration - if (timeout_.count() > 0 && service_called_time_) { + if (timeout_.count() > 0 && service_called_time_) + { auto duration = std::chrono::system_clock::now() - *service_called_time_; - if (duration > timeout_) { + if (duration > timeout_) + { RCLCPP_ERROR(node_->get_logger(), "Service call timed out"); if (callback_) { @@ -164,15 +188,19 @@ class Service : public ServiceInterface { //! 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_)) { + 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() ) + if constexpr (std::is_same()) { return call_service_sync_NoCheck(request); - } else { + } + else + { if (silent) { return call_service_sync_NoCheck(request); @@ -180,25 +208,31 @@ class Service : public ServiceInterface { return call_service_sync_Check(request); } } + private: - Expected 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, timeout_) != - 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 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)); @@ -207,11 +241,12 @@ class Service : public ServiceInterface { return Expected{*response}; } - Expected 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, timeout_) != - 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 Expected{"Failed to call service"}; } diff --git a/src/simulation_panel.cpp b/src/simulation_panel.cpp index b450828..e1b64bc 100644 --- a/src/simulation_panel.cpp +++ b/src/simulation_panel.cpp @@ -9,7 +9,7 @@ namespace q_simulation_interfaces { constexpr char InteractiveMarkerClassId[] = "rviz_default_plugins/InteractiveMarkers"; constexpr char InteractiveMarkerNamespacePropertyName[] = "Interactive Markers Namespace"; - } + } // namespace SimulationPanel::SimulationPanel(QWidget* parent) { simulationWidget_ = new SimulationWidget(this); @@ -18,14 +18,10 @@ namespace q_simulation_interfaces setLayout(layout); } - SimulationPanel::~SimulationPanel() - { - - delete simulationWidget_; - } + 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) + void iterateAllDisplays(rviz_common::DisplayGroup* group, std::function func) { for (int i = 0; i < group->numChildren(); ++i) { @@ -36,7 +32,6 @@ namespace q_simulation_interfaces { return; // stop iterating if the function returns true } - } } for (int i = 0; i < group->numChildren(); ++i) @@ -47,7 +42,6 @@ namespace q_simulation_interfaces iterateAllDisplays(sub_group, func); } } - } void SimulationPanel::onInitialize() @@ -66,11 +60,13 @@ namespace q_simulation_interfaces // check if the InteractiveMarkers display already exists - const auto isInteractiveMarkerDisplay = [this](rviz_common::Display* display) { + 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)) + if (property && + property->getValue().toString() == QString::fromStdString(InteractiveMarkerNamespaceValue)) { std::cout << "Found InteractiveMarkers display with namespace: " << property->getValue().toString().toStdString() << std::endl; diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index 8fec075..4241da8 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -11,9 +11,9 @@ #include "ui_sim_widget.h" #include "vector_utils.hpp" +#include #include #include -#include namespace q_simulation_interfaces { namespace @@ -28,7 +28,8 @@ namespace q_simulation_interfaces return q; } - void AddControlToInteractiveMarker(visualization_msgs::msg::InteractiveMarker& interactive_marker, bool rotationZ) + void AddControlToInteractiveMarker(visualization_msgs::msg::InteractiveMarker& interactive_marker, + bool rotationZ) { // move axis x const std::vector axesT = { @@ -127,12 +128,10 @@ namespace q_simulation_interfaces std::make_shared>("/get_spawnables", node); serviceInterfaces_.push_back(getSpawnablesService_); - spawnEntityService_ = - std::make_shared>("/spawn_entity", node); + spawnEntityService_ = std::make_shared>("/spawn_entity", node); serviceInterfaces_.push_back(spawnEntityService_); - getEntitiesService_ = - std::make_shared>("/get_entities", node); + getEntitiesService_ = std::make_shared>("/get_entities", node); serviceInterfaces_.push_back(getEntitiesService_); getEntityStateService_ = @@ -184,7 +183,6 @@ namespace q_simulation_interfaces interactiveMarkerServer_->clear(); interactiveMarkerServer_->applyChanges(); } - } void SimulationWidget::showEvent(QShowEvent* event) { @@ -255,7 +253,7 @@ namespace q_simulation_interfaces goal->steps = steps; send_goal_options.feedback_callback = [this, steps](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle, - const std::shared_ptr feedback) + const std::shared_ptr feedback) { float progress = static_cast(feedback->completed_steps) / steps; actionThreadProgress_.store(progress); @@ -318,7 +316,6 @@ namespace q_simulation_interfaces } actionThreadRunning_.store(true); actionThread_ = std::thread(&SimulationWidget::ActionThreadWorker, this, steps); - } void SimulationWidget::ResetSimulation() @@ -507,7 +504,7 @@ namespace q_simulation_interfaces request.entity = entity; request.state.pose = pose; request.state.header.frame_id = feedback->header.frame_id; - setEntityStateService_->call_service_async(nullptr,request); + setEntityStateService_->call_service_async(nullptr, request); }; interactiveMarkerServer_->insert(interactive_marker, feedbackCb); interactiveMarkerServer_->applyChanges(); @@ -659,7 +656,8 @@ namespace q_simulation_interfaces // 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_) + if (feedback && feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE && + ui_) { assert(ui_); const auto& pose = feedback->pose; diff --git a/src/simulation_widget.h b/src/simulation_widget.h index 6ac05b4..9bcfef2 100644 --- a/src/simulation_widget.h +++ b/src/simulation_widget.h @@ -1,6 +1,5 @@ #pragma once -#include "service.h" #include #include #include @@ -8,19 +7,20 @@ #include #include #include -#include #include #include +#include +#include "service.h" -#include -#include +#include #include #include -#include -#include -#include #include +#include +#include +#include #include +#include #include namespace Ui @@ -98,4 +98,4 @@ namespace q_simulation_interfaces 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 index 187264e..123c2c3 100644 --- a/src/standalone_main.cpp +++ b/src/standalone_main.cpp @@ -6,39 +6,39 @@ class StandaloneWidget : public QWidget { public: - StandaloneWidget(QWidget *parent = nullptr) : QWidget(parent) + 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_); - }); + node_thread_ = std::thread([this]() { rclcpp::spin(node_); }); } - + ~StandaloneWidget() { - if (node_thread_.joinable()) { - node_thread_.join(); - } - rclcpp::shutdown(); + if (node_thread_.joinable()) + { + node_thread_.join(); + } + rclcpp::shutdown(); } private: - q_simulation_interfaces::SimulationWidget *panel_; + q_simulation_interfaces::SimulationWidget* panel_; rclcpp::Node::SharedPtr node_; std::thread node_thread_; }; -int main(int argc, char *argv[]) { +int main(int argc, char* argv[]) +{ rclcpp::init(argc, argv); QApplication app(argc, argv); diff --git a/src/string_to_keys.h b/src/string_to_keys.h index cf25da2..b22f47c 100644 --- a/src/string_to_keys.h +++ b/src/string_to_keys.h @@ -1,93 +1,83 @@ #pragma once -#include #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 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::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}, - }; +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 index f9b529f..7264c93 100644 --- a/src/vector_utils.hpp +++ b/src/vector_utils.hpp @@ -1,25 +1,28 @@ #pragma once +#include #include -#include #include -#include +#include -inline QString VectorToQstring (const tf2::Vector3& v ) +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 ) +inline tf2::Vector3 QStringToVector(const QString& line) { tf2::Vector3 v; QStringList list = line.split(" "); - if (list.size() == 3) { + if (list.size() == 3) + { v.setX(list[0].toDouble()); v.setY(list[1].toDouble()); v.setZ(list[2].toDouble()); - } else { + } + else + { qWarning() << "Invalid input format. Expected 3 values."; v = tf2::Vector3(0, 0, 0); } From e381e593a9b0cb9aa8db4d4cc43067e6ca8471bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 21 Aug 2025 11:31:33 +0200 Subject: [PATCH 8/8] Fix angles MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- src/sim_widget.ui | 11 +++++++---- src/simulation_widget.cpp | 2 +- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/sim_widget.ui b/src/sim_widget.ui index 4283b9c..22b5229 100644 --- a/src/sim_widget.ui +++ b/src/sim_widget.ui @@ -7,7 +7,7 @@ 0 0 675 - 893 + 901 @@ -243,10 +243,13 @@ - 6 + 2 - -99.000000000000000 + -360.000000000000000 + + + 360.000000000000000 0.001000000000000 @@ -273,7 +276,7 @@ - RotAngle + RotAngle[Deg] diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index 4241da8..d102077 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -451,7 +451,7 @@ namespace q_simulation_interfaces response->state.pose.orientation.z, response->state.pose.orientation.w); const auto axis = q.getAxis(); - const auto angle = q.getAngle(); + const auto angle = qRadiansToDegrees(q.getAngle()); ui_->RotVector->setText(VectorToQstring(axis)); ui_->RotAngle->setValue(angle); if (response->state.header.frame_id.empty())