Merge pull request #1 from ethz-asl/feature/multi_dof_joint_trajector…

…y_rviz_plugins

Feature/multi dof joint trajectory rviz plugins

multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt

@@ -0,0 +1,95 @@
+## BEGIN_TUTORIAL
+## This CMakeLists.txt file for rviz_plugin_tutorials builds both the TeleopPanel tutorial and the ImuDisplay tutorial.
+##
+## First start with some standard catkin stuff.
+cmake_minimum_required(VERSION 2.8.3)
+project(multi_dof_joint_trajectory_rviz_plugins)
+
+# Set compiler flags
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+
+find_package(catkin REQUIRED COMPONENTS
+   rviz
+   trajectory_msgs
+)
+catkin_package(
+ INCLUDE_DIRS
+   include
+ LIBRARIES
+   ${PROJECT_NAME}
+ CATKIN_DEPENDS
+   rviz
+   trajectory_msgs
+)
+include_directories(include ${catkin_INCLUDE_DIRS})
+link_directories(${catkin_LIBRARY_DIRS})
+
+## This plugin includes Qt widgets, so we must include Qt like so:
+find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
+include(${QT_USE_FILE})
+
+## 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)
+
+## Here we specify which header files need to be run through "moc",
+## Qt's meta-object compiler.
+qt4_wrap_cpp(MOC_FILES
+  include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp
+  include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp
+  include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp
+)
+
+## Here we specify the list of source files, including the output of
+## the previous command which is stored in ``${MOC_FILES}``.
+set(SOURCE_FILES
+  src/MultiDOFJointTrajectoryDisplay.cpp
+  src/MultiDOFJointTrajectoryPointConnectionVisual.cpp
+  src/MultiDOFJointTrajectoryPointVisual.cpp
+  ${MOC_FILES}
+)
+
+## 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 ``${SOURCE_FILES}``.
+add_library(${PROJECT_NAME} ${SOURCE_FILES})
+
+# Specify dependencies on automatically generated service files
+#add_dependencies(${PROJECT_NAME})
+
+## Link the library with whatever Qt libraries have been defined by
+## the ``find_package(Qt4 ...)`` line above, 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})
+## END_TUTORIAL
+
+## Install rules
+
+install(TARGETS
+  ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES 
+  plugin_description.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+#install(DIRECTORY media/
+#  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media)
+
+install(DIRECTORY icons/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons)
+
+#install(PROGRAMS scripts/send_test_msgs.py
+#  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+

multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp

@@ -0,0 +1,154 @@
+#pragma once
+
+#ifndef Q_MOC_RUN
+#include <vector>
+
+#include <boost/circular_buffer.hpp>
+
+#include <rviz/message_filter_display.h>
+#include <trajectory_msgs/MultiDOFJointTrajectory.h>
+#endif
+
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+
+#include <tf/transform_listener.h>
+
+#include <rviz/visualization_manager.h>
+#include <rviz/properties/bool_property.h>
+#include <rviz/properties/int_property.h>
+#include <rviz/properties/float_property.h>
+#include <rviz/properties/color_property.h>
+#include <rviz/properties/parse_color.h>
+#include <rviz/frame_manager.h>
+
+#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp"
+#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp"
+
+
+namespace multi_dof_joint_trajectory_rviz_plugins {
+
+class MultiDOFJointTrajectoryDisplay: public rviz::MessageFilterDisplay<trajectory_msgs::MultiDOFJointTrajectory>
+{
+Q_OBJECT
+public:
+  MultiDOFJointTrajectoryDisplay();
+  virtual ~MultiDOFJointTrajectoryDisplay();
+
+
+protected:
+  virtual void onInitialize();
+  virtual void reset();
+
+
+private Q_SLOTS:
+  void setShowConnection();
+  void setShowTransformRotation();
+  void setShowVelocityLinear();
+  void setShowVelocityAngular();
+  void setShowAccelerationLinear();
+  void setShowAccelerationAngular();
+
+  void setSizeTransformRotation();
+  void setDiameterArrows();
+  void setScaleVelocityLinear();
+  void setScaleVelocityAngular();
+  void setScaleAccelerationLinear();
+  void setScaleAccelerationAngular();
+
+  void setAlpha();
+  void setColorConnection();
+  void setColorVelocityLinear();
+  void setColorVelocityAngular();
+  void setColorAccelerationLinear();
+  void setColorAccelerationAngular();
+
+  void setFontSize();
+  void setShowText();
+
+  void setHistoryLength();
+
+
+private:
+  void processMessage(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg);
+
+  void updateShowConnection();
+  void updateShowTransformRotation();
+  void updateShowVelocityLinear();
+  void updateShowVelocityAngular();
+  void updateShowAccelerationLinear();
+  void updateShowAccelerationAngular();
+
+  void updateSizeTransformRotation();
+  void updateDiameterArrows();
+  void updateScaleVelocityLinear();
+  void updateScaleVelocityAngular();
+  void updateScaleAccelerationLinear();
+  void updateScaleAccelerationAngular();
+
+  void updateColorConnection();
+  void updateAlphaTransformRotation();
+  void updateColorVelocityLinear();
+  void updateColorVelocityAngular();
+  void updateColorAccelerationLinear();
+  void updateColorAccelerationAngular();
+
+  void updateFontSize();
+  void updateShowText();
+
+  boost::circular_buffer<std::vector<boost::shared_ptr<MultiDOFJointTrajectoryPointVisual>>> visuals_points_;
+  boost::circular_buffer<std::vector<boost::shared_ptr<MultiDOFJointTrajectoryPointConnectionVisual>>> visuals_connections_;
+
+  rviz::BoolProperty* property_show_connection_;
+  rviz::BoolProperty* property_show_transform_rotation_;
+  rviz::BoolProperty* property_show_velocity_linear_;
+  rviz::BoolProperty* property_show_velocity_angular_;
+  rviz::BoolProperty* property_show_acceleration_linear_;
+  rviz::BoolProperty* property_show_acceleration_angular_;
+
+  rviz::FloatProperty* property_size_transform_rotation_;
+  rviz::FloatProperty* property_diameter_arrows_;
+  rviz::FloatProperty* property_scale_velocity_linear_;
+  rviz::FloatProperty* property_scale_velocity_angular_;
+  rviz::FloatProperty* property_scale_acceleration_linear_;
+  rviz::FloatProperty* property_scale_acceleration_angular_;
+
+  rviz::ColorProperty* property_color_connection_;
+  rviz::ColorProperty* property_color_velocity_linear_;
+  rviz::ColorProperty* property_color_velocity_angular_;
+  rviz::ColorProperty* property_color_acceleration_linear_;
+  rviz::ColorProperty* property_color_acceleration_angular_;
+
+  rviz::FloatProperty* property_alpha_;
+
+  rviz::FloatProperty* property_font_size_;
+  rviz::BoolProperty* property_show_text_;
+
+  rviz::IntProperty* property_history_length_;
+
+  bool show_connection_;
+  bool show_transform_rotation_;
+  bool show_velocity_linear_;
+  bool show_velocity_angular_;
+  bool show_acceleration_linear_;
+  bool show_acceleration_angular_;
+
+  float size_transform_rotation_;
+  float diameter_arrows_;
+  float scale_velocity_linear_;
+  float scale_velocity_angular_;
+  float scale_acceleration_linear_;
+  float scale_acceleration_angular_;
+
+  float alpha_;
+  Ogre::ColourValue color_connection_;
+  Ogre::ColourValue color_velocity_linear_;
+  Ogre::ColourValue color_velocity_angular_;
+  Ogre::ColourValue color_acceleration_linear_;
+  Ogre::ColourValue color_acceleration_angular_;
+
+  float font_size_;
+  bool show_text_;
+};
+
+} // multi_dof_joint_trajectory_rviz_plugins

multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+
+#include <rviz/ogre_helpers/line.h>
+#include <rviz/ogre_helpers/movable_text.h>
+
+#include <rviz/default_plugin/markers/text_view_facing_marker.h>
+
+#include <trajectory_msgs/MultiDOFJointTrajectoryPoint.h>
+
+
+namespace multi_dof_joint_trajectory_rviz_plugins {
+
+class MultiDOFJointTrajectoryPointConnectionVisual
+{
+public:
+  MultiDOFJointTrajectoryPointConnectionVisual(
+      Ogre::SceneManager* scene_manager,
+      Ogre::SceneNode* parent_node,
+      const trajectory_msgs::MultiDOFJointTrajectoryPoint& from,
+      const trajectory_msgs::MultiDOFJointTrajectoryPoint& to,
+      float show_connection,
+      const Ogre::ColourValue& color);
+  virtual ~MultiDOFJointTrajectoryPointConnectionVisual();
+
+  void setShowConnection(bool visible);
+  void setColor(const Ogre::ColourValue& color);
+
+private:
+  void updateShowConnection();
+  void updateColor();
+
+  Ogre::SceneManager* scene_manager_;
+
+  Ogre::SceneNode* scene_node_;
+
+  std::vector<boost::shared_ptr<rviz::Line>> lines_;
+
+  float show_connection_;
+  Ogre::ColourValue color_;
+};
+
+} // multi_dof_joint_trajectory_rviz_plugins
+