Add support for simulation interfaces in RViz2

.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
+...

CMakeLists.txt

@@ -6,32 +6,70 @@ SET(CMAKE_AUTOUIC ON)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
-find_package(simulation_interfaces REQUIRED)
+find_package(simulation_interfaces 1 REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(interactive_markers REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 set(CMAKE_CXX_STANDARD 17)
 
 # Add UI file
 
-set(SOURCES src/main.cpp
-            src/my_widget.cpp)
-add_executable(${PROJECT_NAME} ${SOURCES}
+qt5_wrap_cpp(MOC_FILES
+        include/q_simulation_interfaces/simulation_panel.h
+)
+
+set(SOURCES src/simulation_widget.cpp src/simulation_panel.cpp)
+
+add_library(${PROJECT_NAME} SHARED ${SOURCES} ${MOC_FILES})
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
 )
-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()
 

README.md

@@ -5,19 +5,26 @@ It utilizes https://github.com/ros-simulation/simulation_interfaces to change th
 
 # Prerequisites
 
-I don't know. ROS 2 Humble and libqt5-dev at least.
+```shell
+sudo apt install ros-$ROS_DISTRO-simulation-interfaces libqt5-dev
+```
 
 # Building
 
-```bash
-mkdir -p /tmp/ros2_ws/src
-cd /tmp/ros2_ws/src
-git clone https://github.com/ros-simulation/simulation_interfaces.git
-git clone https://github.com/michalpelka/q_simulation_interfaces.git
+```shell
+mkdir -p ~/ros2_ws/src
+cd ~/ros2_ws/src
+git clone https://github.com/RobotecAI/q_simulation_interfaces.git
 cd ..
 colcon build --symlink-install
 source install/setup.bash
-ros2 run q_simulation_interfaces q_simulation_interfaces
 ```
 
+The tool is available in two flavors:
 
+- A standalone GUI application that can be launched as ROS 2 node:
+```shell  
+ros2 run q_simulation_interfaces q_simulation_interfaces_standalone
+```
+- A Rviz 2 panel plugin that can be launched from Rviz 2 by clicking on the `Panels` menu and selecting `Add New Panel...`
+  and then selecting `SimulationInterfacesPanel` from 'q_simulation_interfaces' package.

include/q_simulation_interfaces/simulation_panel.h

@@ -0,0 +1,30 @@
+#pragma once
+
+
+#include <QVBoxLayout>
+#include <rviz_common/display.hpp>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/panel.hpp>
+
+namespace q_simulation_interfaces
+{
+    class SimulationWidget;
+    class SimulationPanel : public rviz_common::Panel
+    {
+        Q_OBJECT
+    public:
+        explicit SimulationPanel(QWidget* parent = nullptr);
+
+        ~SimulationPanel() override;
+        void onInitialize() override;
+        QString getName() const override;
+        void hideEvent(QHideEvent* event) override;
+
+    private:
+        SimulationWidget* simulationWidget_;
+        rviz_common::Display* im_display_{nullptr};
+        std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
+    };
+} // namespace q_simulation_interfaces
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(q_simulation_interfaces::SimulationPanel, rviz_common::Panel)

package.xml

@@ -9,11 +9,17 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>pluginlib</depend>
+  <depend>rviz_common</depend>
+  <depend>visualization_msgs</depend>
+  <depend>simulation_interfaces</depend>
+  <depend>interactive_markers</depend>
 
   <exec_depend>qt5-qmake</exec_depend>
   <exec_depend>qtbase5-dev</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/q_simulation_plugins.xml"/>
   </export>
 </package>

q_simulation_plugins.xml

@@ -0,0 +1,5 @@
+<library path="q_simulation_interfaces">
+  <class name="SimulationInterfacesPanel" type="q_simulation_interfaces::SimulationPanel" base_class_type="rviz_common::Panel">
+    <description>Simulation Interface Panel for controlling simulation entities and state</description>
+  </class>
+</library>