From 022fcb2c1cf569dc1f1ce2c24cc61fd4672b3cdc Mon Sep 17 00:00:00 2001 From: Unknown Date: Tue, 26 Jun 2018 18:27:30 +0200 Subject: [PATCH] Initial commit --- rviz_interface/interface_tests/CMakeLists.txt | 228 +++++++++ .../interface_tests/CMakeLists.txt~ | 227 +++++++++ .../interface_tests/msg/InterfaceConfigT.msg | 8 + .../interface_tests/msg/StateSpace.msg~ | 32 ++ .../interface_tests/msg/StateSpaceT.msg | 35 ++ rviz_interface/interface_tests/package.xml | 81 ++++ rviz_interface/interface_tests/package.xml~ | 81 ++++ .../interface_tests/panel_plugin.xml | 8 + .../interface_tests/panel_plugin.xml~ | 8 + .../interface_tests/plugin_description.xml~ | 9 + .../interface_tests/src/interface_test.cpp | 432 ++++++++++++++++++ .../interface_tests/src/listener_test.cpp | 121 +++++ .../interface_tests/src/marker_test.cpp | 292 ++++++++++++ .../interface_tests/src/panel_test.cpp | 172 +++++++ .../interface_tests/src/panel_test.hpp | 101 ++++ rviz_interface/package.xml~ | 69 +++ rviz_interface/rviz_interface/CMakeLists.txt | 212 +++++++++ rviz_interface/rviz_interface/CMakeLists.txt~ | 212 +++++++++ .../rviz_interface/msg/InterfaceConfig.msg | 8 + .../rviz_interface/msg/InterfaceConfig.msg~ | 35 ++ .../rviz_interface/msg/StateSpace.msg | 35 ++ .../rviz_interface/msg/StateSpace.msg~ | 32 ++ rviz_interface/rviz_interface/package.xml | 81 ++++ rviz_interface/rviz_interface/package.xml~ | 72 +++ .../rviz_interface/panel_plugin.xml | 9 + .../rviz_interface/panel_plugin.xml~ | 8 + .../rviz_interface/src/InteractiveObject.cpp | 193 ++++++++ .../rviz_interface/src/InteractiveObject.hpp | 42 ++ .../rviz_interface/src/InterfacePanel.cpp | 109 +++++ .../rviz_interface/src/InterfacePanel.hpp | 88 ++++ .../rviz_interface/src/RvizInterface.cpp | 46 ++ .../rviz_interface/src/RvizInterface.hpp | 30 ++ 32 files changed, 3116 insertions(+) create mode 100644 rviz_interface/interface_tests/CMakeLists.txt create mode 100644 rviz_interface/interface_tests/CMakeLists.txt~ create mode 100644 rviz_interface/interface_tests/msg/InterfaceConfigT.msg create mode 100644 rviz_interface/interface_tests/msg/StateSpace.msg~ create mode 100644 rviz_interface/interface_tests/msg/StateSpaceT.msg create mode 100644 rviz_interface/interface_tests/package.xml create mode 100644 rviz_interface/interface_tests/package.xml~ create mode 100644 rviz_interface/interface_tests/panel_plugin.xml create mode 100644 rviz_interface/interface_tests/panel_plugin.xml~ create mode 100644 rviz_interface/interface_tests/plugin_description.xml~ create mode 100644 rviz_interface/interface_tests/src/interface_test.cpp create mode 100644 rviz_interface/interface_tests/src/listener_test.cpp create mode 100644 rviz_interface/interface_tests/src/marker_test.cpp create mode 100644 rviz_interface/interface_tests/src/panel_test.cpp create mode 100644 rviz_interface/interface_tests/src/panel_test.hpp create mode 100644 rviz_interface/package.xml~ create mode 100644 rviz_interface/rviz_interface/CMakeLists.txt create mode 100644 rviz_interface/rviz_interface/CMakeLists.txt~ create mode 100644 rviz_interface/rviz_interface/msg/InterfaceConfig.msg create mode 100644 rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ create mode 100644 rviz_interface/rviz_interface/msg/StateSpace.msg create mode 100644 rviz_interface/rviz_interface/msg/StateSpace.msg~ create mode 100644 rviz_interface/rviz_interface/package.xml create mode 100644 rviz_interface/rviz_interface/package.xml~ create mode 100644 rviz_interface/rviz_interface/panel_plugin.xml create mode 100644 rviz_interface/rviz_interface/panel_plugin.xml~ create mode 100644 rviz_interface/rviz_interface/src/InteractiveObject.cpp create mode 100644 rviz_interface/rviz_interface/src/InteractiveObject.hpp create mode 100644 rviz_interface/rviz_interface/src/InterfacePanel.cpp create mode 100644 rviz_interface/rviz_interface/src/InterfacePanel.hpp create mode 100644 rviz_interface/rviz_interface/src/RvizInterface.cpp create mode 100644 rviz_interface/rviz_interface/src/RvizInterface.hpp diff --git a/rviz_interface/interface_tests/CMakeLists.txt b/rviz_interface/interface_tests/CMakeLists.txt new file mode 100644 index 0000000..205a0ac --- /dev/null +++ b/rviz_interface/interface_tests/CMakeLists.txt @@ -0,0 +1,228 @@ +cmake_minimum_required(VERSION 2.8.3) +project(interface_tests) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS interactive_markers roscpp visualization_msgs tf rospy std_msgs message_generation genmsg) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + StateSpaceT.msg + InterfaceConfigT.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + CATKIN_DEPENDS interactive_markers roscpp visualization_msgs tf message_runtime +) + +## QT ## +## This setting causes Qt's "MOC" generation to happen automatically. +set(CMAKE_AUTOMOC ON) + +## This plugin includes Qt widgets, so we must include Qt. +## We'll use the version that rviz used so they are compatible. +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + ## pull in all required include dirs, define QT_LIBRARIES, etc. + include(${QT_USE_FILE}) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies + set(QT_LIBRARIES Qt5::Widgets) +endif() + +## I prefer the Qt signals and slots to avoid defining "emit", "slots", +## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. +add_definitions(-DQT_NO_KEYWORDS) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +## Here we specify the list of source files for the panel. +## The generated MOC files are included automatically as headers. +set(SRC_FILES + src/panel_test.cpp +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/beginner_tutorials.cpp +# ) +## An rviz plugin is just a shared library, so here we declare the +## library to be called ``${PROJECT_NAME}`` (which is +## "rviz_plugin_tutorials", or whatever your version of this project +## is called) and specify the list of source files we collected above +## in ``${SRC_FILES}``. +add_library(${PROJECT_NAME} ${SRC_FILES}) +#add_library(rviz_interface_plugin src/panel_test.cpp) + +## Link the myviz executable with whatever Qt libraries have been defined by +## the ``find_package(Qt4 ...)`` line above, or by the +## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries +## catkin has included. +## +## Although this puts "rviz_plugin_tutorials" (or whatever you have +## called the project) as the name of the library, cmake knows it is a +## library and names the actual file something like +## "librviz_plugin_tutorials.so", or whatever is appropriate for your +## particular OS. +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(marker_test src/marker_test.cpp) +target_link_libraries(marker_test ${catkin_LIBRARIES}) + +add_executable(interface_test src/interface_test.cpp) +target_link_libraries(interface_test ${catkin_LIBRARIES}) + +add_executable(listener_test src/listener_test.cpp) +target_link_libraries(listener_test ${catkin_LIBRARIES}) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(marker_test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(interface_test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination + +#install(PROGRAMS + #scripts/basic_controls.py + #scripts/cube.py + #scripts/menu.py + #scripts/simple_marker.py + #DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +## Mark executables and/or libraries for installation +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +#install(TARGETS +# marker_test +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) +#install(TARGETS +# interface_test +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) +install(FILES + panel_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_beginner_tutorials.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/rviz_interface/interface_tests/CMakeLists.txt~ b/rviz_interface/interface_tests/CMakeLists.txt~ new file mode 100644 index 0000000..86e5d4a --- /dev/null +++ b/rviz_interface/interface_tests/CMakeLists.txt~ @@ -0,0 +1,227 @@ +cmake_minimum_required(VERSION 2.8.3) +project(interface_tests) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS interactive_markers roscpp visualization_msgs tf rospy std_msgs message_generation genmsg) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + StateSpaceT.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + CATKIN_DEPENDS interactive_markers roscpp visualization_msgs tf message_runtime +) + +## QT ## +## This setting causes Qt's "MOC" generation to happen automatically. +set(CMAKE_AUTOMOC ON) + +## This plugin includes Qt widgets, so we must include Qt. +## We'll use the version that rviz used so they are compatible. +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + ## pull in all required include dirs, define QT_LIBRARIES, etc. + include(${QT_USE_FILE}) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies + set(QT_LIBRARIES Qt5::Widgets) +endif() + +## I prefer the Qt signals and slots to avoid defining "emit", "slots", +## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. +add_definitions(-DQT_NO_KEYWORDS) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +## Here we specify the list of source files for the panel. +## The generated MOC files are included automatically as headers. +set(SRC_FILES + src/panel_test.cpp +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/beginner_tutorials.cpp +# ) +## An rviz plugin is just a shared library, so here we declare the +## library to be called ``${PROJECT_NAME}`` (which is +## "rviz_plugin_tutorials", or whatever your version of this project +## is called) and specify the list of source files we collected above +## in ``${SRC_FILES}``. +add_library(${PROJECT_NAME} ${SRC_FILES}) +#add_library(rviz_interface_plugin src/panel_test.cpp) + +## Link the myviz executable with whatever Qt libraries have been defined by +## the ``find_package(Qt4 ...)`` line above, or by the +## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries +## catkin has included. +## +## Although this puts "rviz_plugin_tutorials" (or whatever you have +## called the project) as the name of the library, cmake knows it is a +## library and names the actual file something like +## "librviz_plugin_tutorials.so", or whatever is appropriate for your +## particular OS. +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(marker_test src/marker_test.cpp) +target_link_libraries(marker_test ${catkin_LIBRARIES}) + +add_executable(interface_test src/interface_test.cpp) +target_link_libraries(interface_test ${catkin_LIBRARIES}) + +add_executable(listener_test src/listener_test.cpp) +target_link_libraries(listener_test ${catkin_LIBRARIES}) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(marker_test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(interface_test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination + +#install(PROGRAMS + #scripts/basic_controls.py + #scripts/cube.py + #scripts/menu.py + #scripts/simple_marker.py + #DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +## Mark executables and/or libraries for installation +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +#install(TARGETS +# marker_test +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) +#install(TARGETS +# interface_test +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) +install(FILES + panel_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_beginner_tutorials.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/rviz_interface/interface_tests/msg/InterfaceConfigT.msg b/rviz_interface/interface_tests/msg/InterfaceConfigT.msg new file mode 100644 index 0000000..d91f75e --- /dev/null +++ b/rviz_interface/interface_tests/msg/InterfaceConfigT.msg @@ -0,0 +1,8 @@ +# Configuration of the Rviz Interface. + +# General info. +#Header header + +#Maximum distance error allowed arround the objective +float32 max_error + diff --git a/rviz_interface/interface_tests/msg/StateSpace.msg~ b/rviz_interface/interface_tests/msg/StateSpace.msg~ new file mode 100644 index 0000000..893409e --- /dev/null +++ b/rviz_interface/interface_tests/msg/StateSpace.msg~ @@ -0,0 +1,32 @@ +# Represents a state space. + +# General info. +#Header header + +# Identifying string for this state space. (ID ?) +string name + +# State Space type: controls how the state space is displayed / computed. +# Default: ORDINARY +# ORDINARY : General display / compute of the state space without any assumption. +# STATE_2D : Expect a 2D position and orientation (Dimension = 3). +# STATE_3D : Expect a 3D position and orientation (quaternion) (Dimension = 7). +# STATE_BOOL : Expect a single boolean information (ie bouton on/off) (Dimension = 1). +uint8 ORDINARY=0 +uint8 STATE_2D=1 +uint8 STATE_3D=2 +uint8 STATE_BOOL=3 + +uint8 state_type + +#State Space dimension : must be equal to the sum of all the data dimensions. +#uint8 dimension + +#State Space data : contains the state space data. +uint8[] discrete_data +float32[] real_data + +# Short description (< 40 characters) of what this state space represent, +# e.g. "Button A pressed". +# Default: ? +string description diff --git a/rviz_interface/interface_tests/msg/StateSpaceT.msg b/rviz_interface/interface_tests/msg/StateSpaceT.msg new file mode 100644 index 0000000..b848f1b --- /dev/null +++ b/rviz_interface/interface_tests/msg/StateSpaceT.msg @@ -0,0 +1,35 @@ +# Represents a state space. + +# General info. +#Header header + +# Identifying string for this state space. (ID ?) +string name + +# State Space type: controls how the state space is displayed / computed. +# Default: ORDINARY +# ORDINARY : General display / compute of the state space without any assumption. +# STATE_2D : Expect a 2D position and orientation (Dimension = 3). +# STATE_3D : Expect a 3D position and orientation (quaternion) (Dimension = 7). +# STATE_BOOL : Expect a single boolean information (ie bouton on/off) (Dimension = 1). +uint8 ORDINARY=0 +uint8 STATE_2D=1 +uint8 STATE_3D=2 +uint8 STATE_BOOL=3 + +uint8 state_type + +#State Space dimension : must be equal to the sum of all the data dimensions. +#uint8 dimension + +#Maximum distance error allowed arround the objective +float32 max_error + +#State Space data : contains the state space data. +uint8[] discrete_data +float32[] real_data + +# Short description (< 40 characters) of what this state space represent, +# e.g. "Button A pressed". +# Default: ? +string description diff --git a/rviz_interface/interface_tests/package.xml b/rviz_interface/interface_tests/package.xml new file mode 100644 index 0000000..b3316d0 --- /dev/null +++ b/rviz_interface/interface_tests/package.xml @@ -0,0 +1,81 @@ + + + interface_tests + 0.0.0 + The interface_tests package + + + + + blue + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + roscpp + interactive_markers + visualization_msgs + tf + + roscpp + interactive_markers + visualization_msgs + tf + + message_generation + message_runtime + + catkin + + + qtbase5-dev + rviz + + libqt5-core + libqt5-gui + libqt5-widgets + rviz + + + + + + + diff --git a/rviz_interface/interface_tests/package.xml~ b/rviz_interface/interface_tests/package.xml~ new file mode 100644 index 0000000..b3316d0 --- /dev/null +++ b/rviz_interface/interface_tests/package.xml~ @@ -0,0 +1,81 @@ + + + interface_tests + 0.0.0 + The interface_tests package + + + + + blue + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + roscpp + interactive_markers + visualization_msgs + tf + + roscpp + interactive_markers + visualization_msgs + tf + + message_generation + message_runtime + + catkin + + + qtbase5-dev + rviz + + libqt5-core + libqt5-gui + libqt5-widgets + rviz + + + + + + + diff --git a/rviz_interface/interface_tests/panel_plugin.xml b/rviz_interface/interface_tests/panel_plugin.xml new file mode 100644 index 0000000..fde277e --- /dev/null +++ b/rviz_interface/interface_tests/panel_plugin.xml @@ -0,0 +1,8 @@ + + + + A panel test. + + + diff --git a/rviz_interface/interface_tests/panel_plugin.xml~ b/rviz_interface/interface_tests/panel_plugin.xml~ new file mode 100644 index 0000000..a35e87e --- /dev/null +++ b/rviz_interface/interface_tests/panel_plugin.xml~ @@ -0,0 +1,8 @@ + + + + A panel test. + + + diff --git a/rviz_interface/interface_tests/plugin_description.xml~ b/rviz_interface/interface_tests/plugin_description.xml~ new file mode 100644 index 0000000..fee1e8d --- /dev/null +++ b/rviz_interface/interface_tests/plugin_description.xml~ @@ -0,0 +1,9 @@ + + + + A panel widget allowing simple diff-drive style robot base control. + + + diff --git a/rviz_interface/interface_tests/src/interface_test.cpp b/rviz_interface/interface_tests/src/interface_test.cpp new file mode 100644 index 0000000..68c78c9 --- /dev/null +++ b/rviz_interface/interface_tests/src/interface_test.cpp @@ -0,0 +1,432 @@ +#include + +#include +#include + +#include + +using namespace visualization_msgs; + +// class InteractiveObject +// { +// protected: +// unsigned int _objectID; //Identifiant de l'objet +// static unsigned int nextObjectID; +// std::string _name; +// unsigned int _type; +// //std::vector _int_markers; +// visualization_msgs::InteractiveMarker _int_marker; + +// interactive_markers::InteractiveMarkerServer& _server; +// interface_tests::StateSpaceT _state; + +// public: +// InteractiveObject(const std::string& name, unsigned int type, interactive_markers::InteractiveMarkerServer& server): _name(name), _type(type), _server(server) +// { +// _objectID = nextObjectID++; +// _state.state_type=type; +// //_server = server; +// //_int_marker = createInteractiveMarker(_name+_objectID,box_marker); +// } + +// //std::vector& markers(){ return _int_markers;} +// visualization_msgs::InteractiveMarker& marker(){ return _int_marker;} +// interface_tests::StateSpaceT& state(){ return _state;} +// }; + +// unsigned int InteractiveObject::nextObjectID = 1; + +class InterfaceTest +{ +private : + ros::NodeHandle _n; + ros::Publisher _objective_pub; + //ros::Subscriber sub_; + interactive_markers::InteractiveMarkerServer server; + + //std::vector _int_markers; + // std::vector _objects; + +public: + + InterfaceTest(): server("interface_tests") + { + //Topic you want to publish + _objective_pub = _n.advertise("state_objective", 10); + + //Topic you want to subscribe + //sub_ = n_.subscribe("/subscribed_topic", 1, &SubscribeAndPublish::callback, this); + + + // create a grey box marker + visualization_msgs::Marker box_marker; + box_marker.type = visualization_msgs::Marker::CUBE; + box_marker.scale.x = 0.45; + box_marker.scale.y = 0.45; + box_marker.scale.z = 0.45; + box_marker.color.r = 0.5; + box_marker.color.g = 0.5; + box_marker.color.b = 0.5; + box_marker.color.a = 1.0; + + tf::Vector3 position; + position = tf::Vector3(0, 0, 0); + + visualization_msgs::InteractiveMarker int_box = createInteractiveMarker("6DOF 1",box_marker); + addButtoncontrol(int_box); + add6DOFcontrol(int_box); + + + visualization_msgs::Marker sphere_marker; + sphere_marker.type = visualization_msgs::Marker::SPHERE; + sphere_marker.scale.x = 0.45; + sphere_marker.scale.y = 0.45; + sphere_marker.scale.z = 0.45; + sphere_marker.color.r = 0.5; + sphere_marker.color.g = 0.5; + sphere_marker.color.b = 0.5; + sphere_marker.color.a = 1.0; + + position = tf::Vector3(-3, 3, 0); + + visualization_msgs::InteractiveMarker int_sphere = createInteractiveMarker("3DOF",sphere_marker,position); + addButtoncontrol(int_sphere); + add3DOFcontrol(int_sphere); + + visualization_msgs::Marker cyl_marker; + cyl_marker.type = visualization_msgs::Marker::CYLINDER; + cyl_marker.scale.x = 0.45; + cyl_marker.scale.y = 0.45; + cyl_marker.scale.z = 0.45; + cyl_marker.color.r = 0.5; + cyl_marker.color.g = 0.5; + cyl_marker.color.b = 0.5; + cyl_marker.color.a = 1.0; + + position = tf::Vector3(3, 3, 0); + + visualization_msgs::InteractiveMarker int_cyl = createInteractiveMarker("Button",cyl_marker, position); + addButtoncontrol(int_cyl); + } + + //Name must be unique + visualization_msgs::InteractiveMarker createInteractiveMarker(const std::string& name, visualization_msgs::Marker& marker, const tf::Vector3& position = tf::Vector3(0,0,0)) + { + //// Création d'un marker interactif //// + visualization_msgs::InteractiveMarker int_marker; + int_marker.header.frame_id = "base_link"; + int_marker.header.stamp=ros::Time::now(); + int_marker.name = name; + int_marker.description = name; + tf::pointTFToMsg(position, int_marker.pose.position); + + // create a non-interactive control which contains the box + visualization_msgs::InteractiveMarkerControl container; + container.always_visible = true; //Toujours visible hors du mode interaction + container.markers.push_back( marker ); + + // add the control to the interactive marker + int_marker.controls.push_back( container ); + + // _int_markers.push_back(int_marker); + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + //server.insert(_int_marker, &InterfaceTest::processFeedback); + server.insert(int_marker, boost::bind(&InterfaceTest::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test + + // 'commit' changes and send to all clients + server.applyChanges(); + + return int_marker; + } + + void addButtoncontrol(visualization_msgs::InteractiveMarker& int_marker) + { + //// Ajout d'interactions //// + visualization_msgs::InteractiveMarkerControl marker_control; + + int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON; + int_marker.controls[0].name= "Button"; + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + //server.insert(_int_marker, &InterfaceTest::processFeedback); + server.insert(int_marker, boost::bind(&InterfaceTest::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test + + // 'commit' changes and send to all clients + server.applyChanges(); + } + + //6DOF + Boutton + //Name must be unique + void add6DOFcontrol(visualization_msgs::InteractiveMarker& int_marker) + { + //// Ajout d'interactions //// + // create a control which will move the box + // this control does not contain any markers, + // which will cause RViz to insert two arrows + visualization_msgs::InteractiveMarkerControl marker_control; + + //_int_marker.controls[0].interaction_mode = InteractiveMarkerControl::MOVE_ROTATE_3D; //Deplacement par "grab" de la souris (CTRL + Souris = rotation) + //int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON; //Ajout fonctionalité Boutton (Validation ?) + int_marker.controls[0].name= "3D"; + + marker_control.orientation_mode = InteractiveMarkerControl::FIXED; + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 1; + marker_control.orientation.y = 0; + marker_control.orientation.z = 0; + //Ajout des controles associées + marker_control.name = "rotate_x"; + marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(marker_control); + marker_control.name = "move_x"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(marker_control); + + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 0; + marker_control.orientation.y = 1; + marker_control.orientation.z = 0; + //Ajout des controles associées + marker_control.name = "rotate_y"; + marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(marker_control); + marker_control.name = "move_y"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(marker_control); + + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 0; + marker_control.orientation.y = 0; + marker_control.orientation.z = 1; + //Ajout des controles associées + marker_control.name = "rotate_z"; + marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(marker_control); + marker_control.name = "move_z"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(marker_control); + + // _int_markers.push_back(int_marker); + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + //server.insert(_int_marker, &InterfaceTest::processFeedback); + server.insert(int_marker, boost::bind(&InterfaceTest::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test + + // 'commit' changes and send to all clients + server.applyChanges(); + } + + //3DOF + Boutton + //Name must be unique + void add3DOFcontrol(visualization_msgs::InteractiveMarker& int_marker) + { + //// Ajout d'interactions //// + // create a control which will move the box + // this control does not contain any markers, + // which will cause RViz to insert two arrows + visualization_msgs::InteractiveMarkerControl marker_control; + + //_int_marker.controls[0].interaction_mode = InteractiveMarkerControl::MOVE_ROTATE_3D; //Deplacement par "grab" de la souris (CTRL + Souris = rotation) + // int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON; //Ajout fonctionalité Boutton (Validation ?) + int_marker.controls[0].name= "2D"; + + marker_control.orientation_mode = InteractiveMarkerControl::FIXED; + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 1; + marker_control.orientation.y = 0; + marker_control.orientation.z = 0; + //Ajout des controles associées + marker_control.name = "move_x"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(marker_control); + + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 0; + marker_control.orientation.y = 1; + marker_control.orientation.z = 0; + //Ajout des controles associées + marker_control.name = "rotate_y"; + marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(marker_control); + + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 0; + marker_control.orientation.y = 0; + marker_control.orientation.z = 1; + //Ajout des controles associées + marker_control.name = "move_z"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(marker_control); + + // _int_markers.push_back(int_marker); + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + //server.insert(_int_marker, &InterfaceTest::processFeedback); + server.insert(int_marker, boost::bind(&InterfaceTest::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test + + // 'commit' changes and send to all clients + server.applyChanges(); + } + + //Faire deffedback different pour chaque type ? + void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) + { + //// SEND OBJECTIVE //// + if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK) + { + interface_tests::StateSpaceT obj; + obj.name = feedback->marker_name +" objective - "+ feedback->control_name; + + if(feedback->control_name == "Button") + { + obj.state_type = 3; //interface_tests::STATE_BOOL; + + obj.discrete_data.push_back(true); + } + else if(feedback->control_name == "2D") + { + obj.state_type = 1; //interface_tests::STATE_2D; + + obj.real_data.push_back(feedback->pose.position.x); + obj.real_data.push_back(feedback->pose.position.y); + // obj.real_data.push_back(feedback->pose.position.z); + // obj.real_data.push_back(feedback->pose.orientation.w); + // obj.real_data.push_back(feedback->pose.orientation.x); + // obj.real_data.push_back(feedback->pose.orientation.y); + obj.real_data.push_back(feedback->pose.orientation.z); + } + else if(feedback->control_name == "3D") + { + obj.state_type = 2; //interface_tests::STATE_3D; + + obj.real_data.push_back(feedback->pose.position.x); + obj.real_data.push_back(feedback->pose.position.y); + obj.real_data.push_back(feedback->pose.position.z); + obj.real_data.push_back(feedback->pose.orientation.w); + obj.real_data.push_back(feedback->pose.orientation.x); + obj.real_data.push_back(feedback->pose.orientation.y); + obj.real_data.push_back(feedback->pose.orientation.z); + } + else + { + obj.state_type = 0; // interface_tests::ORDINARY; + + obj.real_data.push_back(feedback->pose.position.x); + obj.real_data.push_back(feedback->pose.position.y); + obj.real_data.push_back(feedback->pose.position.z); + obj.real_data.push_back(feedback->pose.orientation.w); + obj.real_data.push_back(feedback->pose.orientation.x); + obj.real_data.push_back(feedback->pose.orientation.y); + obj.real_data.push_back(feedback->pose.orientation.z); + } + //obj.discrete_data.push_back(1); + //obj.discrete_data.push_back(false); + + //obj.dimension = obj.real_data.size() + obj.discrete_data.size(); + + obj.description="Test du message StateSpaceT !"; + + _objective_pub.publish(obj); + } + + //// PRINT INFO //// + // ROS_INFO_STREAM( feedback->marker_name << " is now at " + // << feedback->pose.position.x << ", " << feedback->pose.position.y + // << ", " << feedback->pose.position.z ); + + // std::ostringstream s; + // s << "Feedback from marker '" << feedback->marker_name << "' "<< " / control '" << feedback->control_name << "'"; + + // std::ostringstream mouse_point_ss; + // if( feedback->mouse_point_valid ) + // { + // mouse_point_ss << " at " << feedback->mouse_point.x + // << ", " << feedback->mouse_point.y + // << ", " << feedback->mouse_point.z + // << " in frame " << feedback->header.frame_id; + // } + + // switch ( feedback->event_type ) + // { + // case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: + // ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." ); + // break; + + // case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE: + // ROS_INFO_STREAM( s.str() << ": pose changed" + // << "\nposition = " + // << feedback->pose.position.x + // << ", " << feedback->pose.position.y + // << ", " << feedback->pose.position.z + // << "\norientation = " + // << feedback->pose.orientation.w + // << ", " << feedback->pose.orientation.x + // << ", " << feedback->pose.orientation.y + // << ", " << feedback->pose.orientation.z + // << "\nframe: " << feedback->header.frame_id + // << " time: " << feedback->header.stamp.sec << "sec, " + // << feedback->header.stamp.nsec << " nsec" ); + // break; + + // case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN: + // ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." ); + // break; + + // case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: + // ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." ); + // break; + // } + } + + // Marker makeBox( InteractiveMarker &msg ) + // { + // Marker marker; + + // marker.type = Marker::CUBE; + // marker.scale.x = msg.scale * 0.45; + // marker.scale.y = msg.scale * 0.45; + // marker.scale.z = msg.scale * 0.45; + // marker.color.r = 0.5; + // marker.color.g = 0.5; + // marker.color.b = 0.5; + // marker.color.a = 1.0; + + // return marker; + // } + + // InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg ) + // { + // InteractiveMarkerControl control; + // control.always_visible = true; + // control.markers.push_back( makeBox(msg) ); + // msg.controls.push_back( control ); + + // return msg.controls.back(); + // } + + //const std::vector& markers() const { return _int_markers;} + // std::vector& markers() { return _int_markers;} +}; + +int main(int argc, char **argv) +{ + //Initiate ROS + ros::init(argc, argv, "interface_tests"); + + InterfaceTest Interface; + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/rviz_interface/interface_tests/src/listener_test.cpp b/rviz_interface/interface_tests/src/listener_test.cpp new file mode 100644 index 0000000..f1840e7 --- /dev/null +++ b/rviz_interface/interface_tests/src/listener_test.cpp @@ -0,0 +1,121 @@ +#include "ros/ros.h" + +#include + +/** + * This tutorial demonstrates simple receipt of messages over the ROS system. + */ +void objectiveCallback(const interface_tests::StateSpaceT& msg) +{ + std::ostringstream s; + s << "Current Objective : " << std::endl << " Name : "<< msg.name << std::endl; + //s << " Dimension :" << (int) msg.dimension << std::endl; + + std::vector discrete_data = msg.discrete_data; + std::vector real_data = msg.real_data; + + switch(msg.state_type) + { + case(interface_tests::StateSpaceT::STATE_BOOL): + if(discrete_data.size() !=1) + ROS_WARN("Receiving STATE_BOOL objective with invalid dimension"); + else + { + s << " Switch State !" << std::endl; + } + break; + + case(interface_tests::StateSpaceT::STATE_2D): + if(real_data.size() != 3) + ROS_WARN("Receiving STATE_2D objective with invalid dimension"); + else + { + s << " Position : " << std::endl; + s << " x : "<< real_data[0] << std::endl; + s << " y : "<< real_data[1] << std::endl; + s << " Orientation : " << std::endl; + s << " theta : "<< real_data[2] << std::endl; + } + break; + + case(interface_tests::StateSpaceT::STATE_3D): + if(real_data.size() != 7) + ROS_WARN("Receiving STATE_3D objective with invalid dimension"); + else + { + s << " Position : " << std::endl; + s << " x : "<< real_data[0] << std::endl; + s << " y : "<< real_data[1] << std::endl; + s << " z : "<< real_data[2] << std::endl; + s << " Orientation : " << std::endl; + s << " w : "<< real_data[3] << std::endl; + s << " x : "<< real_data[4] << std::endl; + s << " y : "<< real_data[5] << std::endl; + s << " z : "<< real_data[6] << std::endl; + } + break; + + case(interface_tests::StateSpaceT::ORDINARY): + s << " Discrete data : "<< discrete_data.size() << std::endl; + for(unsigned int i =0; i + +#include +//#include + +#include + +using namespace visualization_msgs; + +Marker makeBox( InteractiveMarker &msg ) +{ + Marker marker; + + marker.type = Marker::CUBE; + marker.scale.x = msg.scale * 0.45; + marker.scale.y = msg.scale * 0.45; + marker.scale.z = msg.scale * 0.45; + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 0.5; + marker.color.a = 1.0; + + return marker; +} + +InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg ) +{ + InteractiveMarkerControl control; + control.always_visible = true; + control.markers.push_back( makeBox(msg) ); + msg.controls.push_back( control ); + + return msg.controls.back(); +} + +void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) +{ + //// SEND OBJECTIVE //// + if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK) + { + //TRES DEGEU -> FAIRE EN OBJET + //ros::Publisher objective_pub = n.advertise("state_objective", 10); + + interface_tests::StateSpaceT obj; + obj.name ="Objective_test"; + // obj.state_type = interface_tests::STATE_2D; + obj.state_type = 1; + //obj.dimension = 3; + obj.discrete_data.push_back(1); + obj.discrete_data.push_back(false); + obj.real_data.push_back(10.2); + obj.description="Test du message StateSpaceT !"; + + //objective_pub.publish(obj); + } + + //// PRINT INFO //// + // ROS_INFO_STREAM( feedback->marker_name << " is now at " + // << feedback->pose.position.x << ", " << feedback->pose.position.y + // << ", " << feedback->pose.position.z ); + + std::ostringstream s; + s << "Feedback from marker '" << feedback->marker_name << "' "<< " / control '" << feedback->control_name << "'"; + + std::ostringstream mouse_point_ss; + if( feedback->mouse_point_valid ) + { + mouse_point_ss << " at " << feedback->mouse_point.x + << ", " << feedback->mouse_point.y + << ", " << feedback->mouse_point.z + << " in frame " << feedback->header.frame_id; + } + + switch ( feedback->event_type ) + { + case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: + ROS_INFO_STREAM( s.str() << ": button click" << mouse_point_ss.str() << "." ); + break; + + case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE: + ROS_INFO_STREAM( s.str() << ": pose changed" + << "\nposition = " + << feedback->pose.position.x + << ", " << feedback->pose.position.y + << ", " << feedback->pose.position.z + << "\norientation = " + << feedback->pose.orientation.w + << ", " << feedback->pose.orientation.x + << ", " << feedback->pose.orientation.y + << ", " << feedback->pose.orientation.z + << "\nframe: " << feedback->header.frame_id + << " time: " << feedback->header.stamp.sec << "sec, " + << feedback->header.stamp.nsec << " nsec" ); + break; + + case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN: + ROS_INFO_STREAM( s.str() << ": mouse down" << mouse_point_ss.str() << "." ); + break; + + case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: + ROS_INFO_STREAM( s.str() << ": mouse up" << mouse_point_ss.str() << "." ); + break; + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "marker_test"); + + ros::NodeHandle n; + + //Create Objective topic + ros::Publisher objective_pub = n.advertise("state_objective", 10); + + //Test du topic + interface_tests::StateSpaceT obj; + obj.name ="Objective_test"; + // obj.state_type = interface_tests::STATE_2D; + obj.state_type = 1; + //obj.dimension = 3; + obj.discrete_data.push_back(1); + obj.discrete_data.push_back(false); + obj.real_data.push_back(10.2); + obj.description="Test du message StateSpaceT !"; + + objective_pub.publish(obj); + + ROS_INFO_STREAM("Starting Marker Test"); + // create an interactive marker server on the topic namespace simple_marker + interactive_markers::InteractiveMarkerServer server("marker_test"); + + // create an interactive marker for our server + visualization_msgs::InteractiveMarker int_marker; + int_marker.header.frame_id = "base_link"; + int_marker.header.stamp=ros::Time::now(); + int_marker.name = "my_marker"; + int_marker.description = "Test Control"; + + // create a grey box marker + visualization_msgs::Marker box_marker; + box_marker.type = visualization_msgs::Marker::CUBE; + box_marker.scale.x = 0.45; + box_marker.scale.y = 0.45; + box_marker.scale.z = 0.45; + box_marker.color.r = 0.5; + box_marker.color.g = 0.5; + box_marker.color.b = 0.5; + box_marker.color.a = 1.0; + + // create a non-interactive control which contains the box + visualization_msgs::InteractiveMarkerControl box_control; + box_control.always_visible = true; //Toujours visible hors du mode interaction + box_control.markers.push_back( box_marker ); + + // add the control to the interactive marker + int_marker.controls.push_back( box_control ); + + // create a control which will move the box + // this control does not contain any markers, + // which will cause RViz to insert two arrows + visualization_msgs::InteractiveMarkerControl new_control; + + new_control.name = "move_test"; + + //MODE 3D fonctionnel ??? + // new_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D; + + // add the control to the interactive marker + //int_marker.controls.push_back(new_control); + + //Orientation du vecteur + // new_control.orientation.w = 1; + // new_control.orientation.x = 0; + // new_control.orientation.y = 1; + // new_control.orientation.z = 0; + + // new_control.name = "rotate_y"; + // new_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + // int_marker.controls.push_back(new_control); + + // new_control.name = "move_plane_y"; + // new_control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE; //Attention par défaut utilise un halo (comme le rotate Axis) peut etre ignorer -> utiliser MOVE_ROTATE dans ce cas + // int_marker.controls.push_back(new_control); + + // new_control.name = "move_rotate_y"; + // new_control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE; //Difficile pour des mouvements précis + // int_marker.controls.push_back(new_control); + + //Boutton + // new_control.interaction_mode = InteractiveMarkerControl::BUTTON; + // new_control.name = "button_control"; + + // Marker marker = makeBox( int_marker ); + // new_control.markers.push_back( marker ); + // new_control.always_visible = true; + // int_marker.controls.push_back(new_control); + + //TEST 6DOF + + //int_marker.controls[0].interaction_mode = InteractiveMarkerControl::MOVE_ROTATE_3D; //Deplacement par "grab" de la souris (CTRL + Souris = rotation) + int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON; //Ajout fonctionalité Boutton (Validation ?) + + new_control.orientation_mode = InteractiveMarkerControl::FIXED; + //Orientation du vecteur + new_control.orientation.w = 1; + new_control.orientation.x = 1; + new_control.orientation.y = 0; + new_control.orientation.z = 0; + //Ajout des controles associées + new_control.name = "rotate_x"; + new_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(new_control); + new_control.name = "move_x"; + new_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(new_control); + + //Orientation du vecteur + new_control.orientation.w = 1; + new_control.orientation.x = 0; + new_control.orientation.y = 1; + new_control.orientation.z = 0; + //Ajout des controles associées + new_control.name = "rotate_y"; + new_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(new_control); + new_control.name = "move_y"; + new_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(new_control); + + //Orientation du vecteur + new_control.orientation.w = 1; + new_control.orientation.x = 0; + new_control.orientation.y = 0; + new_control.orientation.z = 1; + //Ajout des controles associées + new_control.name = "rotate_z"; + new_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(new_control); + new_control.name = "move_z"; + new_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(new_control); + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + server.insert(int_marker, &processFeedback); + + // 'commit' changes and send to all clients + server.applyChanges(); + + // start the ROS main loop + ros::spin(); + + // ros::Rate loop_rate(10); + // while (ros::ok()) + // { + // objective_pub.publish(obj); + // ros::spinOnce(); + // loop_rate.sleep(); + // } +} +// %Tag(fullSource)% diff --git a/rviz_interface/interface_tests/src/panel_test.cpp b/rviz_interface/interface_tests/src/panel_test.cpp new file mode 100644 index 0000000..3f18420 --- /dev/null +++ b/rviz_interface/interface_tests/src/panel_test.cpp @@ -0,0 +1,172 @@ +#include "panel_test.hpp" + +//#include + +namespace interface_tests +{ + +// BEGIN_TUTORIAL +// Here is the implementation of the TestPanel class. TestPanel +// has these responsibilities: +// +// - Act as a container for GUI elements DriveWidget and QLineEdit. +// - Publish command velocities 10 times per second (whether 0 or not). +// - Saving and restoring internal state from a config file. +// +// We start with the constructor, doing the standard Qt thing of +// passing the optional *parent* argument on to the superclass +// constructor, and also zero-ing the velocities we will be +// publishing. +TestPanel::TestPanel( QWidget* parent ): rviz::Panel( parent ) +{ + // Next we lay out the "output topic" text entry field using a + // QLabel and a QLineEdit in a QHBoxLayout. + QHBoxLayout* topic_layout = new QHBoxLayout; + topic_layout->addWidget( new QLabel( "Max Error:" )); + max_error_editor = new QLineEdit; + // max_error_editor->setValidator( new QDoubleValidator(0,MAX_ERROR,1000,max_error_editor)); // Ne gère pas les exceptions tout seul (retourne QValidator::Intermediate pour les valeurs hors bornes) + topic_layout->addWidget( max_error_editor ); + setLayout( topic_layout ); + + // Then create the control widget. + // drive_widget_ = new DriveWidget; + + // // Lay out the topic field above the control widget. + // QVBoxLayout* layout = new QVBoxLayout; + // layout->addLayout( topic_layout ); + // layout->addWidget( drive_widget_ ); + // setLayout( layout ); + + // Create a timer for sending the output. Motor controllers want to + // be reassured frequently that they are doing the right thing, so + // we keep re-sending velocities even when they aren't changing. + // + // Here we take advantage of QObject's memory management behavior: + // since "this" is passed to the new QTimer as its parent, the + // QTimer is deleted by the QObject destructor when this TestPanel + // object is destroyed. Therefore we don't need to keep a pointer + // to the timer. + // QTimer* output_timer = new QTimer( this ); + + // Next we make signal/slot connections. + // connect( drive_widget_, SIGNAL( outputVelocity( float, float )), this, SLOT( setVel( float, float ))); + connect( max_error_editor, SIGNAL( editingFinished() ), this, SLOT( updateError() )); + // connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() )); + + // Start the timer. + // output_timer->start( 100 ); + + // Make the control widget start disabled, since we don't start with an output topic. + // drive_widget_->setEnabled( false ); + + _config_publisher = nh_.advertise( "interface_config", 1 ); +} + +// setVel() is connected to the DriveWidget's output, which is sent +// whenever it changes due to a mouse event. This just records the +// values it is given. The data doesn't actually get sent until the +// next timer callback. +// void TestPanel::setVel( float lin, float ang ) +// { +// linear_velocity_ = lin; +// angular_velocity_ = ang; +// } + +// Read the topic name from the QLineEdit and call setTopic() with the +// results. This is connected to QLineEdit::editingFinished() which +// fires when the user presses Enter or Tab or otherwise moves focus +// away. +void TestPanel::updateError() +{ + //setTopic( max_error_editor->text() ); + setError(max_error_editor->text()); +} + +void TestPanel::setError( const QString& error ) +{ + if( ros::ok() && _config_publisher ) + { + interface_tests::InterfaceConfigT new_config; + new_config.max_error = error.toDouble(); + _config_publisher.publish( new_config ); + } +} + +// Set the topic name we are publishing to. +// void TestPanel::setTopic( const QString& new_topic ) +// { +// // Only take action if the name has changed. +// if( new_topic != output_topic_ ) +// { +// output_topic_ = new_topic; +// // If the topic is the empty string, don't publish anything. +// if( output_topic_ == "" ) +// { +// _config_publisher.shutdown(); +// } +// else +// { +// // The old ``_config_publisher`` is destroyed by this assignment, +// // and thus the old topic advertisement is removed. The call to +// // nh_advertise() says we want to publish data on the new topic +// // name. +// _config_publisher = nh_.advertise( output_topic_.toStdString(), 1 ); +// } +// // rviz::Panel defines the configChanged() signal. Emitting it +// // tells RViz that something in this panel has changed that will +// // affect a saved config file. Ultimately this signal can cause +// // QWidget::setWindowModified(true) to be called on the top-level +// // rviz::VisualizationFrame, which causes a little asterisk ("*") +// // to show in the window's title bar indicating unsaved changes. +// Q_EMIT configChanged(); +// } + +// // Gray out the control widget when the output topic is empty. +// // drive_widget_->setEnabled( output_topic_ != "" ); +// } + +// Publish the control velocities if ROS is not shutting down and the +// publisher is ready with a valid topic name. +// void TestPanel::sendVel() +// { +// if( ros::ok() && _config_publisher ) +// { +// geometry_msgs::Twist msg; +// msg.linear.x = linear_velocity_; +// msg.linear.y = 0; +// msg.linear.z = 0; +// msg.angular.x = 0; +// msg.angular.y = 0; +// msg.angular.z = angular_velocity_; +// _config_publisher.publish( msg ); +// } +// } + +// Save all configuration data from this panel to the given +// Config object. It is important here that you call save() +// on the parent class so the class id and panel name get saved. +void TestPanel::save( rviz::Config config ) const +{ + rviz::Panel::save( config ); + config.mapSetValue( "Topic", output_topic_ ); +} + +// Load all configuration data for this panel from the given Config object. +void TestPanel::load( const rviz::Config& config ) +{ + rviz::Panel::load( config ); + QString topic; + if( config.mapGetString( "Topic", &topic )) + { + max_error_editor->setText( topic ); + updateError(); + } +} + +} // end namespace + +// Tell pluginlib about this class. Every class which should be +// loadable by pluginlib::ClassLoader must have these two lines +// compiled in its .cpp file, outside of any namespace scope. +#include +PLUGINLIB_EXPORT_CLASS(interface_tests::TestPanel,rviz::Panel ) \ No newline at end of file diff --git a/rviz_interface/interface_tests/src/panel_test.hpp b/rviz_interface/interface_tests/src/panel_test.hpp new file mode 100644 index 0000000..626c68e --- /dev/null +++ b/rviz_interface/interface_tests/src/panel_test.hpp @@ -0,0 +1,101 @@ +#ifndef PANEL_TEST_HPP +#define PANEL_TEST_HPP + +//#ifndef Q_MOC_RUN +# include + +# include +//#endif + +#include + +#include +#include +// #include +#include +#include +#include +#include + +#include + +#define MAX_ERROR 999999999 + +class QLineEdit; + +namespace interface_tests +{ +// BEGIN_TUTORIAL +// Here we declare our new subclass of rviz::Panel. Every panel which +// can be added via the Panels/Add_New_Panel menu is a subclass of +// rviz::Panel. +// +class TestPanel: public rviz::Panel +{ +// This class uses Qt slots and is a subclass of QObject, so it needs +// the Q_OBJECT macro. +Q_OBJECT +public: + // QWidget subclass constructors usually take a parent widget + // parameter (which usually defaults to 0). At the same time, + // pluginlib::ClassLoader creates instances by calling the default + // constructor (with no arguments). Taking the parameter and giving + // a default of 0 lets the default constructor work and also lets + // someone using the class for something else to pass in a parent + // widget as they normally would with Qt. + TestPanel( QWidget* parent = 0 ); + + // Now we declare overrides of rviz::Panel functions for saving and + // loading data from the config file. Here the data is the + // topic name. + virtual void load( const rviz::Config& config ); + virtual void save( rviz::Config config ) const; + + // Next come a couple of public Qt slots. +public Q_SLOTS: + // The control area, DriveWidget, sends its output to a Qt signal + // for ease of re-use, so here we declare a Qt slot to receive it. + // void setVel( float linear_velocity_, float angular_velocity_ ); + + // In this example setTopic() does not get connected to any signal + // (it is called directly), but it is easy to define it as a public + // slot instead of a private function in case it would be useful to + // some other user. + // void setTopic( const QString& topic ); + void setError( const QString& error ); + + // Here we declare some internal slots. +protected Q_SLOTS: + // sendvel() publishes the current velocity values to a ROS + // topic. Internally this is connected to a timer which calls it 10 + // times per second. + // void sendVel(); + + // updateError() reads the topic name from the QLineEdit and calls + // setTopic() with the result. + void updateError(); + + // Then we finish up with protected member variables. +protected: + + // One-line text editor for entering the outgoing ROS topic name. + QLineEdit* max_error_editor; + + // The current name of the output topic. + QString output_topic_; + + // The ROS publisher for the command velocity. + ros::Publisher _config_publisher; + + // The ROS node handle. + ros::NodeHandle nh_; + + // The latest velocity values from the drive widget. + // float linear_velocity_; + // float angular_velocity_; + // END_TUTORIAL +}; + +} // end namespace rviz_plugin_tutorials + +#endif \ No newline at end of file diff --git a/rviz_interface/package.xml~ b/rviz_interface/package.xml~ new file mode 100644 index 0000000..7d77de8 --- /dev/null +++ b/rviz_interface/package.xml~ @@ -0,0 +1,69 @@ + + + rviz_interface + 0.0.0 + The rviz_interface package + + + + + blue + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + + roscpp + interactive_markers + visualization_msgs + tf + + roscpp + interactive_markers + visualization_msgs + tf + diff --git a/rviz_interface/rviz_interface/CMakeLists.txt b/rviz_interface/rviz_interface/CMakeLists.txt new file mode 100644 index 0000000..f4a4963 --- /dev/null +++ b/rviz_interface/rviz_interface/CMakeLists.txt @@ -0,0 +1,212 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rviz_interface) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS interactive_markers roscpp visualization_msgs tf rospy std_msgs message_generation genmsg) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + StateSpace.msg + InterfaceConfig.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + CATKIN_DEPENDS interactive_markers roscpp visualization_msgs tf message_runtime +) + +## QT ## +## This setting causes Qt's "MOC" generation to happen automatically. +set(CMAKE_AUTOMOC ON) + +## This plugin includes Qt widgets, so we must include Qt. +## We'll use the version that rviz used so they are compatible. +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + ## pull in all required include dirs, define QT_LIBRARIES, etc. + include(${QT_USE_FILE}) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies + set(QT_LIBRARIES Qt5::Widgets) +endif() + +## I prefer the Qt signals and slots to avoid defining "emit", "slots", +## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. +add_definitions(-DQT_NO_KEYWORDS) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +## Here we specify the list of source files for the panel. +## The generated MOC files are included automatically as headers. +set(SRC_FILES + src/InterfacePanel.cpp +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/beginner_tutorials.cpp +# ) +## An rviz plugin is just a shared library, so here we declare the +## library to be called ``${PROJECT_NAME}`` (which is +## "rviz_plugin_tutorials", or whatever your version of this project +## is called) and specify the list of source files we collected above +## in ``${SRC_FILES}``. +add_library(${PROJECT_NAME} ${SRC_FILES}) +#add_library(rviz_interface_plugin src/panel_test.cpp) + +## Link the myviz executable with whatever Qt libraries have been defined by +## the ``find_package(Qt4 ...)`` line above, or by the +## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries +## catkin has included. +## +## Although this puts "rviz_plugin_tutorials" (or whatever you have +## called the project) as the name of the library, cmake knows it is a +## library and names the actual file something like +## "librviz_plugin_tutorials.so", or whatever is appropriate for your +## particular OS. +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(RvizInterface src/RvizInterface.cpp src/InteractiveObject.cpp) +target_link_libraries(RvizInterface ${catkin_LIBRARIES}) + + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(RvizInterface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination + +#install(PROGRAMS + #scripts/basic_controls.py + #scripts/cube.py + #scripts/menu.py + #scripts/simple_marker.py + #DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +## Mark executables and/or libraries for installation +install(TARGETS + RvizInterface + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) +install(FILES + panel_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_beginner_tutorials.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/rviz_interface/rviz_interface/CMakeLists.txt~ b/rviz_interface/rviz_interface/CMakeLists.txt~ new file mode 100644 index 0000000..f4a4963 --- /dev/null +++ b/rviz_interface/rviz_interface/CMakeLists.txt~ @@ -0,0 +1,212 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rviz_interface) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS interactive_markers roscpp visualization_msgs tf rospy std_msgs message_generation genmsg) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + StateSpace.msg + InterfaceConfig.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + CATKIN_DEPENDS interactive_markers roscpp visualization_msgs tf message_runtime +) + +## QT ## +## This setting causes Qt's "MOC" generation to happen automatically. +set(CMAKE_AUTOMOC ON) + +## This plugin includes Qt widgets, so we must include Qt. +## We'll use the version that rviz used so they are compatible. +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + ## pull in all required include dirs, define QT_LIBRARIES, etc. + include(${QT_USE_FILE}) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies + set(QT_LIBRARIES Qt5::Widgets) +endif() + +## I prefer the Qt signals and slots to avoid defining "emit", "slots", +## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. +add_definitions(-DQT_NO_KEYWORDS) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +## Here we specify the list of source files for the panel. +## The generated MOC files are included automatically as headers. +set(SRC_FILES + src/InterfacePanel.cpp +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/beginner_tutorials.cpp +# ) +## An rviz plugin is just a shared library, so here we declare the +## library to be called ``${PROJECT_NAME}`` (which is +## "rviz_plugin_tutorials", or whatever your version of this project +## is called) and specify the list of source files we collected above +## in ``${SRC_FILES}``. +add_library(${PROJECT_NAME} ${SRC_FILES}) +#add_library(rviz_interface_plugin src/panel_test.cpp) + +## Link the myviz executable with whatever Qt libraries have been defined by +## the ``find_package(Qt4 ...)`` line above, or by the +## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries +## catkin has included. +## +## Although this puts "rviz_plugin_tutorials" (or whatever you have +## called the project) as the name of the library, cmake knows it is a +## library and names the actual file something like +## "librviz_plugin_tutorials.so", or whatever is appropriate for your +## particular OS. +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(RvizInterface src/RvizInterface.cpp src/InteractiveObject.cpp) +target_link_libraries(RvizInterface ${catkin_LIBRARIES}) + + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(RvizInterface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination + +#install(PROGRAMS + #scripts/basic_controls.py + #scripts/cube.py + #scripts/menu.py + #scripts/simple_marker.py + #DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) + +## Mark executables and/or libraries for installation +install(TARGETS + RvizInterface + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) +install(FILES + panel_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_beginner_tutorials.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg new file mode 100644 index 0000000..d91f75e --- /dev/null +++ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg @@ -0,0 +1,8 @@ +# Configuration of the Rviz Interface. + +# General info. +#Header header + +#Maximum distance error allowed arround the objective +float32 max_error + diff --git a/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ new file mode 100644 index 0000000..5ae3257 --- /dev/null +++ b/rviz_interface/rviz_interface/msg/InterfaceConfig.msg~ @@ -0,0 +1,35 @@ +# Configuration of the Rviz Interface. + +# General info. +#Header header + +# Identifying string for this state space. (ID ?) +string name + +# State Space type: controls how the state space is displayed / computed. +# Default: ORDINARY +# ORDINARY : General display / compute of the state space without any assumption. +# STATE_2D : Expect a 2D position and orientation (Dimension = 3). +# STATE_3D : Expect a 3D position and orientation (quaternion) (Dimension = 7). +# STATE_BOOL : Expect a single boolean information (ie bouton on/off) (Dimension = 1). +uint8 ORDINARY=0 +uint8 STATE_2D=1 +uint8 STATE_3D=2 +uint8 STATE_BOOL=3 + +uint8 state_type + +#State Space dimension : must be equal to the sum of all the data dimensions. +#uint8 dimension + +#Maximum distance error allowed arround the objective +float32 max_error + +#State Space data : contains the state space data. +uint8[] discrete_data +float32[] real_data + +# Short description (< 40 characters) of what this state space represent, +# e.g. "Button A pressed". +# Default: ? +string description diff --git a/rviz_interface/rviz_interface/msg/StateSpace.msg b/rviz_interface/rviz_interface/msg/StateSpace.msg new file mode 100644 index 0000000..b848f1b --- /dev/null +++ b/rviz_interface/rviz_interface/msg/StateSpace.msg @@ -0,0 +1,35 @@ +# Represents a state space. + +# General info. +#Header header + +# Identifying string for this state space. (ID ?) +string name + +# State Space type: controls how the state space is displayed / computed. +# Default: ORDINARY +# ORDINARY : General display / compute of the state space without any assumption. +# STATE_2D : Expect a 2D position and orientation (Dimension = 3). +# STATE_3D : Expect a 3D position and orientation (quaternion) (Dimension = 7). +# STATE_BOOL : Expect a single boolean information (ie bouton on/off) (Dimension = 1). +uint8 ORDINARY=0 +uint8 STATE_2D=1 +uint8 STATE_3D=2 +uint8 STATE_BOOL=3 + +uint8 state_type + +#State Space dimension : must be equal to the sum of all the data dimensions. +#uint8 dimension + +#Maximum distance error allowed arround the objective +float32 max_error + +#State Space data : contains the state space data. +uint8[] discrete_data +float32[] real_data + +# Short description (< 40 characters) of what this state space represent, +# e.g. "Button A pressed". +# Default: ? +string description diff --git a/rviz_interface/rviz_interface/msg/StateSpace.msg~ b/rviz_interface/rviz_interface/msg/StateSpace.msg~ new file mode 100644 index 0000000..893409e --- /dev/null +++ b/rviz_interface/rviz_interface/msg/StateSpace.msg~ @@ -0,0 +1,32 @@ +# Represents a state space. + +# General info. +#Header header + +# Identifying string for this state space. (ID ?) +string name + +# State Space type: controls how the state space is displayed / computed. +# Default: ORDINARY +# ORDINARY : General display / compute of the state space without any assumption. +# STATE_2D : Expect a 2D position and orientation (Dimension = 3). +# STATE_3D : Expect a 3D position and orientation (quaternion) (Dimension = 7). +# STATE_BOOL : Expect a single boolean information (ie bouton on/off) (Dimension = 1). +uint8 ORDINARY=0 +uint8 STATE_2D=1 +uint8 STATE_3D=2 +uint8 STATE_BOOL=3 + +uint8 state_type + +#State Space dimension : must be equal to the sum of all the data dimensions. +#uint8 dimension + +#State Space data : contains the state space data. +uint8[] discrete_data +float32[] real_data + +# Short description (< 40 characters) of what this state space represent, +# e.g. "Button A pressed". +# Default: ? +string description diff --git a/rviz_interface/rviz_interface/package.xml b/rviz_interface/rviz_interface/package.xml new file mode 100644 index 0000000..f193cf2 --- /dev/null +++ b/rviz_interface/rviz_interface/package.xml @@ -0,0 +1,81 @@ + + + rviz_interface + 0.0.0 + The rviz_interface package + + + + + blue + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + roscpp + interactive_markers + visualization_msgs + tf + + roscpp + interactive_markers + visualization_msgs + tf + + message_generation + message_runtime + + catkin + + + qtbase5-dev + rviz + + libqt5-core + libqt5-gui + libqt5-widgets + rviz + + + + + + + diff --git a/rviz_interface/rviz_interface/package.xml~ b/rviz_interface/rviz_interface/package.xml~ new file mode 100644 index 0000000..4a0f66a --- /dev/null +++ b/rviz_interface/rviz_interface/package.xml~ @@ -0,0 +1,72 @@ + + + rviz_interface + 0.0.0 + The rviz_interface package + + + + + blue + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + roscpp + interactive_markers + visualization_msgs + tf + + roscpp + interactive_markers + visualization_msgs + tf + + message_generation + message_runtime + + catkin + + + + + + + diff --git a/rviz_interface/rviz_interface/panel_plugin.xml b/rviz_interface/rviz_interface/panel_plugin.xml new file mode 100644 index 0000000..0dcefec --- /dev/null +++ b/rviz_interface/rviz_interface/panel_plugin.xml @@ -0,0 +1,9 @@ + + + + A panel for an Rviz Interface. + + + diff --git a/rviz_interface/rviz_interface/panel_plugin.xml~ b/rviz_interface/rviz_interface/panel_plugin.xml~ new file mode 100644 index 0000000..1a51891 --- /dev/null +++ b/rviz_interface/rviz_interface/panel_plugin.xml~ @@ -0,0 +1,8 @@ + + + + A panel for an Rviz Interface. + + + diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.cpp b/rviz_interface/rviz_interface/src/InteractiveObject.cpp new file mode 100644 index 0000000..c832260 --- /dev/null +++ b/rviz_interface/rviz_interface/src/InteractiveObject.cpp @@ -0,0 +1,193 @@ +#include "InteractiveObject.hpp" + +unsigned int InteractiveObject::nextObjectID = 1; + +InteractiveObject::InteractiveObject(ros::Publisher* objective_pub, interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape = Marker::CUBE, const tf::Vector3& position = tf::Vector3(0,0,0)) : _name(name), _type(type), _server(server), _objective_pub(objective_pub) +{ + _objectID = nextObjectID++; + _state.state_type=type; + _state.max_error = 0; //Default error + + Marker marker; + marker.type = shape; + marker.scale.x = 0.45; + marker.scale.y = 0.45; + marker.scale.z = 0.45; + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 0.5; + marker.color.a = 1.0; + + createInteractiveMarker(marker, position); + addButtoncontrol(); + // add6DOFcontrol(); +} + + //Name must be unique +void InteractiveObject::createInteractiveMarker(Marker& marker, const tf::Vector3& position = tf::Vector3(0,0,0)) +{ + //// Création d'un marker interactif //// + _int_marker.header.frame_id = "map"; + _int_marker.header.stamp=ros::Time::now(); + _int_marker.name = _name; //ATTENTION ! + _int_marker.description = _name; + tf::pointTFToMsg(position, _int_marker.pose.position); + + // create a non-interactive control which contains the box + InteractiveMarkerControl container; + container.always_visible = true; //Toujours visible hors du mode interaction + container.markers.push_back( marker ); + + // add the control to the interactive marker + _int_marker.controls.push_back( container ); + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + _server->insert(_int_marker, boost::bind(&InteractiveObject::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test + + // 'commit' changes and send to all clients + _server->applyChanges(); +} + +void InteractiveObject::processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback ) + { + //// SEND OBJECTIVE //// + if(feedback->event_type == InteractiveMarkerFeedback::BUTTON_CLICK) + { + _state.name = _name; + + //Ajout des informations de position + _state.real_data.push_back(feedback->pose.position.x); + _state.real_data.push_back(feedback->pose.position.y); + _state.real_data.push_back(feedback->pose.position.z); + _state.real_data.push_back(feedback->pose.orientation.w); + _state.real_data.push_back(feedback->pose.orientation.x); + _state.real_data.push_back(feedback->pose.orientation.y); + _state.real_data.push_back(feedback->pose.orientation.z); + + //Publication de l'objectif + _objective_pub->publish(_state); + + //Problème d'ajout continue de data + _state.real_data.clear(); //Attention peut poser problème pour ajouter des infos supplémentaires (nécessité de les rajouter à chaque feedback) + } +} + +void InteractiveObject::addButtoncontrol() +{ + //// Ajout d'interactions //// + InteractiveMarkerControl marker_control; + + _int_marker.controls[0].interaction_mode = InteractiveMarkerControl::BUTTON; + // _int_marker.controls[0].name= "Button"; + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + _server->insert(_int_marker, boost::bind(&InteractiveObject::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test + + // 'commit' changes and send to all clients + _server->applyChanges(); +} + +//6DOF + Boutton +void InteractiveObject::add6DOFcontrol() +{ + //// Ajout d'interactions //// + InteractiveMarkerControl marker_control; + + _int_marker.controls[0].name= "3D"; + + marker_control.orientation_mode = InteractiveMarkerControl::FIXED; + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 1; + marker_control.orientation.y = 0; + marker_control.orientation.z = 0; + //Ajout des controles associées + marker_control.name = "rotate_x"; + marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + _int_marker.controls.push_back(marker_control); + marker_control.name = "move_x"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + _int_marker.controls.push_back(marker_control); + + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 0; + marker_control.orientation.y = 1; + marker_control.orientation.z = 0; + //Ajout des controles associées + marker_control.name = "rotate_y"; + marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + _int_marker.controls.push_back(marker_control); + marker_control.name = "move_y"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + _int_marker.controls.push_back(marker_control); + + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 0; + marker_control.orientation.y = 0; + marker_control.orientation.z = 1; + //Ajout des controles associées + marker_control.name = "rotate_z"; + marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + _int_marker.controls.push_back(marker_control); + marker_control.name = "move_z"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + _int_marker.controls.push_back(marker_control); + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + _server->insert(_int_marker, boost::bind(&InteractiveObject::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test + + // 'commit' changes and send to all clients + _server->applyChanges(); +} + +//3DOF + Boutton +void InteractiveObject::add3DOFcontrol() +{ + //// Ajout d'interactions //// + InteractiveMarkerControl marker_control; + + _int_marker.controls[0].name= "2D"; + + marker_control.orientation_mode = InteractiveMarkerControl::FIXED; + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 1; + marker_control.orientation.y = 0; + marker_control.orientation.z = 0; + //Ajout des controles associées + marker_control.name = "move_x"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + _int_marker.controls.push_back(marker_control); + + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 0; + marker_control.orientation.y = 1; + marker_control.orientation.z = 0; + //Ajout des controles associées + marker_control.name = "rotate_y"; + marker_control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; + _int_marker.controls.push_back(marker_control); + + //Orientation du vecteur + marker_control.orientation.w = 1; + marker_control.orientation.x = 0; + marker_control.orientation.y = 0; + marker_control.orientation.z = 1; + //Ajout des controles associées + marker_control.name = "move_z"; + marker_control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; + _int_marker.controls.push_back(marker_control); + + // add the interactive marker to our collection & + // tell the server to call processFeedback() when feedback arrives for it + _server->insert(_int_marker, boost::bind(&InteractiveObject::processFeedback,this,_1)); //Boost bind -> passage en argument de la fonction membre de interface_test + + // 'commit' changes and send to all clients + _server->applyChanges(); +} \ No newline at end of file diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.hpp b/rviz_interface/rviz_interface/src/InteractiveObject.hpp new file mode 100644 index 0000000..6fc10cb --- /dev/null +++ b/rviz_interface/rviz_interface/src/InteractiveObject.hpp @@ -0,0 +1,42 @@ +#ifndef INTERACTIVEOBJECT_HPP +#define INTERACTIVEOBJECT_HPP + +#include +#include + +#include + +using namespace visualization_msgs; + +class InteractiveObject +{ +protected: + unsigned int _objectID; //Identifiant de l'objet + static unsigned int nextObjectID; + std::string _name; + unsigned int _type; + //std::vector _int_markers; + InteractiveMarker _int_marker; + + interactive_markers::InteractiveMarkerServer* _server; + rviz_interface::StateSpace _state; + + ros::Publisher* _objective_pub; + +public: + InteractiveObject(ros::Publisher* objective_pub, interactive_markers::InteractiveMarkerServer* server, const std::string& name, unsigned int type, unsigned int shape, const tf::Vector3& position); + + void createInteractiveMarker(Marker& marker, const tf::Vector3& position); + + void processFeedback( const InteractiveMarkerFeedbackConstPtr &feedback ); + + void addButtoncontrol(); + void add6DOFcontrol(); + void add3DOFcontrol(); + + //std::vector& markers(){ return _int_markers;} + InteractiveMarker& marker(){ return _int_marker;} + rviz_interface::StateSpace& state(){ return _state;} +}; + +#endif \ No newline at end of file diff --git a/rviz_interface/rviz_interface/src/InterfacePanel.cpp b/rviz_interface/rviz_interface/src/InterfacePanel.cpp new file mode 100644 index 0000000..75daf44 --- /dev/null +++ b/rviz_interface/rviz_interface/src/InterfacePanel.cpp @@ -0,0 +1,109 @@ +#include "InterfacePanel.hpp" + +namespace rviz_interface +{ + +// BEGIN_TUTORIAL +// Here is the implementation of the InterfacePanel class. InterfacePanel +// has these responsibilities: +// +// - Act as a container for GUI elements DriveWidget and QLineEdit. +// - Publish command velocities 10 times per second (whether 0 or not). +// - Saving and restoring internal state from a config file. +// +// We start with the constructor, doing the standard Qt thing of +// passing the optional *parent* argument on to the superclass +// constructor, and also zero-ing the velocities we will be +// publishing. +InterfacePanel::InterfacePanel( QWidget* parent ): rviz::Panel( parent ) +{ + // Next we lay out the "output topic" text entry field using a + // QLabel and a QLineEdit in a QHBoxLayout. + QHBoxLayout* topic_layout = new QHBoxLayout; + topic_layout->addWidget( new QLabel( "Max Error:" )); + max_error_editor = new QLineEdit; + // max_error_editor->setValidator( new QDoubleValidator(0,MAX_ERROR,1000,max_error_editor)); // Ne gère pas les exceptions tout seul (retourne QValidator::Intermediate pour les valeurs hors bornes) + topic_layout->addWidget( max_error_editor ); + setLayout( topic_layout ); + + // Then create the control widget. + // drive_widget_ = new DriveWidget; + + // // Lay out the topic field above the control widget. + // QVBoxLayout* layout = new QVBoxLayout; + // layout->addLayout( topic_layout ); + // layout->addWidget( drive_widget_ ); + // setLayout( layout ); + + // Create a timer for sending the output. Motor controllers want to + // be reassured frequently that they are doing the right thing, so + // we keep re-sending velocities even when they aren't changing. + // + // Here we take advantage of QObject's memory management behavior: + // since "this" is passed to the new QTimer as its parent, the + // QTimer is deleted by the QObject destructor when this InterfacePanel + // object is destroyed. Therefore we don't need to keep a pointer + // to the timer. + // QTimer* output_timer = new QTimer( this ); + + // Next we make signal/slot connections. + connect( max_error_editor, SIGNAL( editingFinished() ), this, SLOT( updateError() )); + // connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() )); + + // Start the timer. + // output_timer->start( 100 ); + + // Make the control widget start disabled, since we don't start with an output topic. + // drive_widget_->setEnabled( false ); + + _config_publisher = nh_.advertise( "/RvizInterface/interface_config", 1 ); +} + +// Read the topic name from the QLineEdit and call setTopic() with the +// results. This is connected to QLineEdit::editingFinished() which +// fires when the user presses Enter or Tab or otherwise moves focus +// away. +void InterfacePanel::updateError() +{ + //setTopic( max_error_editor->text() ); + setError(max_error_editor->text()); +} + +void InterfacePanel::setError( const QString& error ) +{ + if( ros::ok() && _config_publisher ) + { + rviz_interface::InterfaceConfig new_config; + new_config.max_error = error.toDouble(); + _config_publisher.publish( new_config ); + } +} + +// Save all configuration data from this panel to the given +// Config object. It is important here that you call save() +// on the parent class so the class id and panel name get saved. +void InterfacePanel::save( rviz::Config config ) const +{ + rviz::Panel::save( config ); + config.mapSetValue( "Error", output_topic_ ); +} + +// Load all configuration data for this panel from the given Config object. +void InterfacePanel::load( const rviz::Config& config ) +{ + rviz::Panel::load( config ); + QString error; + if( config.mapGetString( "Error", &error )) + { + max_error_editor->setText( error ); + updateError(); + } +} + +} // end namespace + +// Tell pluginlib about this class. Every class which should be +// loadable by pluginlib::ClassLoader must have these two lines +// compiled in its .cpp file, outside of any namespace scope. +#include +PLUGINLIB_EXPORT_CLASS(rviz_interface::InterfacePanel,rviz::Panel ) \ No newline at end of file diff --git a/rviz_interface/rviz_interface/src/InterfacePanel.hpp b/rviz_interface/rviz_interface/src/InterfacePanel.hpp new file mode 100644 index 0000000..855e181 --- /dev/null +++ b/rviz_interface/rviz_interface/src/InterfacePanel.hpp @@ -0,0 +1,88 @@ +#ifndef INTERFACE_PANEL_HPP +#define INTERFACE_PANEL_HPP + +//#ifndef Q_MOC_RUN +# include + +# include +//#endif + +#include + +#include +#include +// #include +#include +#include +#include +#include + +#include + +#define MAX_ERROR 999999999 + +class QLineEdit; + +namespace rviz_interface +{ +// BEGIN_TUTORIAL +// Here we declare our new subclass of rviz::Panel. Every panel which +// can be added via the Panels/Add_New_Panel menu is a subclass of +// rviz::Panel. +// +class InterfacePanel: public rviz::Panel +{ +// This class uses Qt slots and is a subclass of QObject, so it needs +// the Q_OBJECT macro. +Q_OBJECT +public: + // QWidget subclass constructors usually take a parent widget + // parameter (which usually defaults to 0). At the same time, + // pluginlib::ClassLoader creates instances by calling the default + // constructor (with no arguments). Taking the parameter and giving + // a default of 0 lets the default constructor work and also lets + // someone using the class for something else to pass in a parent + // widget as they normally would with Qt. + InterfacePanel( QWidget* parent = 0 ); + + // Now we declare overrides of rviz::Panel functions for saving and + // loading data from the config file. Here the data is the + // topic name. + virtual void load( const rviz::Config& config ); + virtual void save( rviz::Config config ) const; + + // Next come a couple of public Qt slots. +public Q_SLOTS: + + void setError( const QString& error ); + + // Here we declare some internal slots. +protected Q_SLOTS: + + + void updateError(); + + // Then we finish up with protected member variables. +protected: + + // One-line text editor for entering the outgoing ROS topic name. + QLineEdit* max_error_editor; + + // The current name of the output topic. + QString output_topic_; + + // The ROS publisher for the command velocity. + ros::Publisher _config_publisher; + + // The ROS node handle. + ros::NodeHandle nh_; + + // The latest velocity values from the drive widget. + // float linear_velocity_; + // float angular_velocity_; + // END_TUTORIAL +}; + +} // end namespace rviz_plugin_tutorials + +#endif \ No newline at end of file diff --git a/rviz_interface/rviz_interface/src/RvizInterface.cpp b/rviz_interface/rviz_interface/src/RvizInterface.cpp new file mode 100644 index 0000000..94c189c --- /dev/null +++ b/rviz_interface/rviz_interface/src/RvizInterface.cpp @@ -0,0 +1,46 @@ +#include "RvizInterface.hpp" + +RvizInterface::RvizInterface(): _server("RvizInterface") +{ + //Topic you want to publish + _objective_pub = _n.advertise("/RvizInterface/state_objective", 10); + + //Topic you want to subscribe + _config_sub = _n.subscribe("/RvizInterface/interface_config", 1, &RvizInterface::configCallback, this); + + _objects.push_back(new InteractiveObject(&_objective_pub, &_server, "Test", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::CUBE, tf::Vector3(0,0,0))); + // _objects[0].addButtonControl(); + _objects[0]->add6DOFcontrol(); + + _objects.push_back(new InteractiveObject(&_objective_pub, &_server, "Test 2", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::CUBE, tf::Vector3(3,3,0))); + // _objects[0].addButtonControl(); + _objects[1]->add3DOFcontrol(); +} + +RvizInterface::~RvizInterface() +{ + for(unsigned int i=0;i<_objects.size();i++) + { + delete _objects[i]; + } +} + +void RvizInterface::configCallback(const rviz_interface::InterfaceConfig & new_config) +{ + for(unsigned int i=0;i<_objects.size();i++) + { + _objects[i]->state().max_error = new_config.max_error; + } +} + +int main(int argc, char **argv) +{ + //Initiate ROS + ros::init(argc, argv, "Rviz_Interface"); + + RvizInterface Interface; + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/rviz_interface/rviz_interface/src/RvizInterface.hpp b/rviz_interface/rviz_interface/src/RvizInterface.hpp new file mode 100644 index 0000000..3c764b2 --- /dev/null +++ b/rviz_interface/rviz_interface/src/RvizInterface.hpp @@ -0,0 +1,30 @@ +#ifndef RVIZINTERFACE_HPP +#define RVIZINTERFACE_HPP + +#include + +#include +#include + +#include "InteractiveObject.hpp" + +#include + +class RvizInterface +{ +protected: + ros::NodeHandle _n; + ros::Publisher _objective_pub; + ros::Subscriber _config_sub; + interactive_markers::InteractiveMarkerServer _server; + + std::vector _objects; + +public: + RvizInterface(); + ~RvizInterface(); + + void configCallback(const rviz_interface::InterfaceConfig & new_config); +}; + +#endif \ No newline at end of file