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 @@
+<?xml version="1.0"?>
+<package>
+  <name>interface_tests</name>
+  <version>0.0.0</version>
+  <description>The interface_tests package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="blue@todo.todo">blue</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rviz_interface</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>interactive_markers</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>interactive_markers</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf</run_depend>
+
+  <build_depend>message_generation</build_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- QT Panel -->
+  <build_depend>qtbase5-dev</build_depend>
+  <build_depend>rviz</build_depend>
+
+  <run_depend>libqt5-core</run_depend>
+  <run_depend>libqt5-gui</run_depend>
+  <run_depend>libqt5-widgets</run_depend>
+  <run_depend>rviz</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+     <rviz plugin="${prefix}/panel_plugin.xml"/>
+  </export>
+</package>
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 @@
+<?xml version="1.0"?>
+<package>
+  <name>interface_tests</name>
+  <version>0.0.0</version>
+  <description>The interface_tests package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="blue@todo.todo">blue</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rviz_interface</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>interactive_markers</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>interactive_markers</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf</run_depend>
+
+  <build_depend>message_generation</build_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- QT Panel -->
+  <build_depend>qtbase5-dev</build_depend>
+  <build_depend>rviz</build_depend>
+
+  <run_depend>libqt5-core</run_depend>
+  <run_depend>libqt5-gui</run_depend>
+  <run_depend>libqt5-widgets</run_depend>
+  <run_depend>rviz</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+     <rviz plugin="${prefix}/panel_plugin.xml"/>
+  </export>
+</package>
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 @@
+<library path="lib/libinterface_tests">
+  <class type="interface_tests::TestPanel"
+         base_class_type="rviz::Panel">
+    <description>
+     	A panel test.
+    </description>
+  </class>
+</library>
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 @@
+<library path="lib/interface_tests">
+  <class type="interface_tests::TestPanel"
+         base_class_type="rviz::Panel">
+    <description>
+     	A panel test.
+    </description>
+  </class>
+</library>
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 @@
+<library path="lib/interface_tests">
+  <class name="interface_tests/TestPanel"
+         type="interface_tests::TestPanel"
+         base_class_type="rviz::Panel">
+    <description>
+      A panel widget allowing simple diff-drive style robot base control.
+    </description>
+  </class>
+</library>
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 <ros/ros.h>
+
+#include <interactive_markers/interactive_marker_server.h>
+#include <tf/tf.h>
+
+#include <interface_tests/StateSpaceT.h>
+
+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<visualization_msgs::InteractiveMarker> _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<visualization_msgs::InteractiveMarker>& 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<visualization_msgs::InteractiveMarker> _int_markers;
+ // std::vector<InteractiveObject> _objects;
+
+public:
+
+  InterfaceTest(): server("interface_tests")
+  {
+	//Topic you want to publish
+	_objective_pub = _n.advertise<interface_tests::StateSpaceT>("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<visualization_msgs::InteractiveMarker>& markers() const { return _int_markers;}
+	// std::vector<visualization_msgs::InteractiveMarker>& 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 <interface_tests/StateSpaceT.h>
+
+/**
+ * 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<uint8_t> discrete_data = msg.discrete_data;
+  std::vector<float> 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<discrete_data.size();i++)
+      {
+         s << "    "<< discrete_data[i]<<std::endl;
+      }
+      s << "  Real data : "<< real_data.size() << std::endl;
+      for(unsigned int i =0; i<real_data.size();i++)
+      {
+         s << "    "<< real_data[i]<<std::endl;
+      }
+      break;
+  }
+
+  ROS_INFO_STREAM(s.str());
+}
+
+int main(int argc, char **argv)
+{
+  /**
+   * The ros::init() function needs to see argc and argv so that it can perform
+   * any ROS arguments and name remapping that were provided at the command line.
+   * For programmatic remappings you can use a different version of init() which takes
+   * remappings directly, but for most command-line programs, passing argc and argv is
+   * the easiest way to do it.  The third argument to init() is the name of the node.
+   *
+   * You must call one of the versions of ros::init() before using any other
+   * part of the ROS system.
+   */
+  ros::init(argc, argv, "listener");
+
+  /**
+   * NodeHandle is the main access point to communications with the ROS system.
+   * The first NodeHandle constructed will fully initialize this node, and the last
+   * NodeHandle destructed will close down the node.
+   */
+  ros::NodeHandle n;
+
+  /**
+   * The subscribe() call is how you tell ROS that you want to receive messages
+   * on a given topic.  This invokes a call to the ROS
+   * master node, which keeps a registry of who is publishing and who
+   * is subscribing.  Messages are passed to a callback function, here
+   * called chatterCallback.  subscribe() returns a Subscriber object that you
+   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
+   * object go out of scope, this callback will automatically be unsubscribed from
+   * this topic.
+   *
+   * The second parameter to the subscribe() function is the size of the message
+   * queue.  If messages are arriving faster than they are being processed, this
+   * is the number of messages that will be buffered up before beginning to throw
+   * away the oldest ones.
+   */
+  ros::Subscriber sub = n.subscribe("state_objective", 1000, objectiveCallback);
+
+  /**
+   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
+   * callbacks will be called from within this thread (the main one).  ros::spin()
+   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
+   */
+  ros::spin();
+
+  return 0;
+}
\ No newline at end of file
diff --git a/rviz_interface/interface_tests/src/marker_test.cpp b/rviz_interface/interface_tests/src/marker_test.cpp
new file mode 100644
index 0000000..0e21034
--- /dev/null
+++ b/rviz_interface/interface_tests/src/marker_test.cpp
@@ -0,0 +1,292 @@
+/*
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+// %Tag(fullSource)%
+#include <ros/ros.h>
+
+#include <interactive_markers/interactive_marker_server.h>
+//#include <tf/tf.h>
+
+#include <interface_tests/StateSpaceT.h>
+
+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<interface_tests::StateSpaceT>("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<interface_tests::StateSpaceT>("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 <geometry_msgs/Twist.h>
+
+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_tests::InterfaceConfigT>( "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<geometry_msgs::Twist>( 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/class_list_macros.h>
+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 <ros/ros.h>
+
+# include <rviz/panel.h>
+//#endif
+
+#include <stdio.h>
+
+#include <QPainter>
+#include <QLineEdit>
+// #include <QDoubleValidator>
+#include <QVBoxLayout>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QTimer>
+
+#include <interface_tests/InterfaceConfigT.h>
+
+#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 @@
+<?xml version="1.0"?>
+<package>
+  <name>rviz_interface</name>
+  <version>0.0.0</version>
+  <description>The rviz_interface package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="blue@todo.todo">blue</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rviz_interface</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>interactive_markers</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>interactive_markers</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf</run_depend>
+</package>
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 @@
+<?xml version="1.0"?>
+<package>
+  <name>rviz_interface</name>
+  <version>0.0.0</version>
+  <description>The rviz_interface package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="blue@todo.todo">blue</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rviz_interface</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>interactive_markers</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>interactive_markers</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf</run_depend>
+
+  <build_depend>message_generation</build_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+   <!-- QT Panel -->
+  <build_depend>qtbase5-dev</build_depend>
+  <build_depend>rviz</build_depend>
+
+  <run_depend>libqt5-core</run_depend>
+  <run_depend>libqt5-gui</run_depend>
+  <run_depend>libqt5-widgets</run_depend>
+  <run_depend>rviz</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+     <rviz plugin="${prefix}/panel_plugin.xml"/>
+  </export>
+</package>
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 @@
+<?xml version="1.0"?>
+<package>
+  <name>rviz_interface</name>
+  <version>0.0.0</version>
+  <description>The rviz_interface package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="blue@todo.todo">blue</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rviz_interface</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>interactive_markers</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>interactive_markers</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf</run_depend>
+
+  <build_depend>message_generation</build_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
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 @@
+<library path="lib/librviz_interface">
+  <class name="Rviz Interface Panel"
+	 type="rviz_interface::InterfacePanel"
+         base_class_type="rviz::Panel">
+    <description>
+     	A panel for an Rviz Interface.
+    </description>
+  </class>
+</library>
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 @@
+<library path="lib/librviz_interface">
+  <class type="rviz_interface::InterfacePanel"
+         base_class_type="rviz::Panel">
+    <description>
+     	A panel for an Rviz Interface.
+    </description>
+  </class>
+</library>
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 <interactive_markers/interactive_marker_server.h>
+#include <tf/tf.h>
+
+#include <rviz_interface/StateSpace.h>
+
+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<visualization_msgs::InteractiveMarker> _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<visualization_msgs::InteractiveMarker>& 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<rviz_interface::InterfaceConfig>( "/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/class_list_macros.h>
+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 <ros/ros.h>
+
+# include <rviz/panel.h>
+//#endif
+
+#include <stdio.h>
+
+#include <QPainter>
+#include <QLineEdit>
+// #include <QDoubleValidator>
+#include <QVBoxLayout>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QTimer>
+
+#include <rviz_interface/InterfaceConfig.h>
+
+#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<rviz_interface::StateSpace>("/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 <ros/ros.h>
+
+#include <interactive_markers/interactive_marker_server.h>
+#include <tf/tf.h>
+
+#include "InteractiveObject.hpp"
+
+#include <rviz_interface/InterfaceConfig.h>
+
+class RvizInterface
+{
+protected:
+	ros::NodeHandle _n; 
+	ros::Publisher _objective_pub;
+	ros::Subscriber _config_sub;
+	interactive_markers::InteractiveMarkerServer _server;
+
+	std::vector<InteractiveObject*> _objects;
+
+public: 
+	RvizInterface();
+	~RvizInterface();
+
+	void configCallback(const rviz_interface::InterfaceConfig & new_config);
+};
+
+#endif
\ No newline at end of file