diff --git a/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt b/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt new file mode 100644 index 0000000..625755c --- /dev/null +++ b/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}) + diff --git a/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png b/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png new file mode 100644 index 0000000..4e415e0 Binary files /dev/null and b/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png differ diff --git a/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp new file mode 100644 index 0000000..dc4e0a6 --- /dev/null +++ b/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 + +#include + +#include +#include +#endif + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#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 +{ +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>> visuals_points_; + boost::circular_buffer>> 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 diff --git a/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp new file mode 100644 index 0000000..7e49ebd --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include + +#include + + +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> lines_; + + float show_connection_; + Ogre::ColourValue color_; +}; + +} // multi_dof_joint_trajectory_rviz_plugins + diff --git a/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp new file mode 100644 index 0000000..d6c7aa5 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp @@ -0,0 +1,135 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include + + +namespace multi_dof_joint_trajectory_rviz_plugins { + +class MultiDOFJointTrajectoryPointVisual +{ +public: + MultiDOFJointTrajectoryPointVisual( + Ogre::SceneManager* scene_manager, + Ogre::SceneNode* parent_node, + const trajectory_msgs::MultiDOFJointTrajectoryPoint& msg, + 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_transform_rotatation, + const Ogre::ColourValue& color_velocity_linear, + const Ogre::ColourValue& color_velocity_angular, + const Ogre::ColourValue& color_acceleration_linear, + const Ogre::ColourValue& color_acceleration_angular, + const std::vector& captions, + float font_size, + bool show_text); + virtual ~MultiDOFJointTrajectoryPointVisual(); + + void setShowTransformRotation(bool visible); + void setShowVelocityLinear(bool visible); + void setShowVelocityAngular(bool visible); + void setShowAccelerationLinear(bool visible); + void setShowAccelerationAngular(bool visible); + + void setSizeTransformRotation(float size); + void setDiametersArrows(float diameter); + void setScaleVelocityLinear(float scale); + void setScaleVelocityAngular(float scale); + void setScaleAccelerationLinear(float scale); + void setScaleAccelerationAngular(float scale); + + void setAlphaTransformRotation(float alpha); + void setColorVelocityLinear(const Ogre::ColourValue& color); + void setColorVelocityAngular(const Ogre::ColourValue& color); + void setColorAccelerationLinear(const Ogre::ColourValue& color); + void setColorAccelerationAngular(const Ogre::ColourValue& color); + + void setCaptions(const std::vector& captions); + void setFontSize(float font_size); + void setShowText(bool show_text); + + +private: + void updateSizeTransformRotation(); + void updateDiametersArrows(); + void updateScaleVelocityLinear(); + void updateScaleVelocityAngular(); + void updateScaleAccelerationLinear(); + void updateScaleAccelerationAngular(); + + void updateAlphaTransformRotation(); + void updateColorVelocityLinear(); + void updateColorVelocityAngular(); + void updateColorAccelerationLinear(); + void updateColorAccelerationAngular(); + + void updateCaptions(); + void updateFontSize(); + void updateShowText(); + + Ogre::ColourValue getColor(const Ogre::ColourValue& color, bool visible); + float getCharacterHeight(); + + Ogre::SceneManager* scene_manager_; + + std::vector transforms_position_; + std::vector> transforms_rotation_; + std::vector> velocities_linear_; + std::vector> velocities_angular_; + std::vector> accelerations_linear_; + std::vector> accelerations_angular_; + std::vector> texts_; + + std::vector velocities_linear_absolute_; + std::vector velocities_angular_absolute_; + std::vector accelerations_linear_absolute_; + std::vector accelerations_angular_absolute_; + + bool show_transform_rotation_; + bool show_velocity_linear_; + bool show_velocity_angular_; + bool show_acceleration_linear_; + bool show_acceleration_angular_; + + const float axis_radius_per_size_; + + 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_transform_rotatation_; + Ogre::ColourValue color_velocity_linear_; + Ogre::ColourValue color_velocity_angular_; + Ogre::ColourValue color_acceleration_linear_; + Ogre::ColourValue color_acceleration_angular_; + + std::vector captions_; + float font_size_; + bool show_text_; +}; + +} // multi_dof_joint_trajectory_rviz_plugins + diff --git a/multi_dof_joint_trajectory_rviz_plugins/package.xml b/multi_dof_joint_trajectory_rviz_plugins/package.xml new file mode 100644 index 0000000..42e0b04 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/package.xml @@ -0,0 +1,25 @@ + + + multi_dof_joint_trajectory_rviz_plugins + 0.0.0 + + Plugins for visualizing trajectory_msgs/MultiDOFJointTrajectory in rviz. + + Remo Diethelm + BSD + + none + Remo Diethelm + + catkin + + rviz + trajectory_msgs + + rviz + trajectory_msgs + + + + + diff --git a/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml b/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml new file mode 100644 index 0000000..2c70a7e --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + A class for displaying the trajectory_msgs/MultiDOFJointTrajectory message. + + + diff --git a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp new file mode 100644 index 0000000..76df894 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp @@ -0,0 +1,645 @@ +#include + +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp" + + +namespace multi_dof_joint_trajectory_rviz_plugins { + +MultiDOFJointTrajectoryDisplay::MultiDOFJointTrajectoryDisplay() +: show_connection_(true), + show_transform_rotation_(true), + show_velocity_linear_(true), + show_velocity_angular_(true), + show_acceleration_linear_(true), + show_acceleration_angular_(true), + size_transform_rotation_(0.2), + diameter_arrows_(0.05), + scale_velocity_linear_(1.0), + scale_velocity_angular_(1.0), + scale_acceleration_linear_(1.0), + scale_acceleration_angular_(1.0), + alpha_(1.0), + color_connection_(Ogre::ColourValue(1.0, 1.0, 1.0, alpha_)), // white + color_velocity_linear_(Ogre::ColourValue(0.4, 0.0, 0.0, alpha_)), // dark red + color_velocity_angular_(Ogre::ColourValue(0.0, 0.4, 0.0, alpha_)), // dark green + color_acceleration_linear_(Ogre::ColourValue(1.0, 1.0, 0.0, alpha_)), // yellow + color_acceleration_angular_(Ogre::ColourValue(0.75, 0.0, 0.75, alpha_)), // purple + font_size_(0.05), + show_text_(true) +{ + property_show_connection_ = new rviz::BoolProperty( + "Show Connections", show_connection_, + "Enable or disable connections rendering.", + this, SLOT(setShowConnection())); + + property_show_transform_rotation_ = new rviz::BoolProperty( + "Show Transform Rotation", show_transform_rotation_, + "Enable or disable rotation transforms rendering.", + this, SLOT(setShowTransformRotation())); + + property_show_velocity_linear_ = new rviz::BoolProperty( + "Show Velocity Linear", show_velocity_linear_, + "Enable or disable linear velocities rendering.", + this, SLOT(setShowVelocityLinear())); + + property_show_velocity_angular_ = new rviz::BoolProperty( + "Show Velocity Angular", show_velocity_angular_, + "Enable or disable angular velocities rendering.", + this, SLOT(setShowVelocityAngular())); + + property_show_acceleration_linear_ = new rviz::BoolProperty( + "Show Acceleration Linear", show_acceleration_linear_, + "Enable or disable linear accelerations rendering.", + this, SLOT(setShowAccelerationLinear())); + + property_show_acceleration_angular_ = new rviz::BoolProperty( + "Show Acceleration Angular", show_acceleration_angular_, + "Enable or disable angular accelerations rendering.", + this, SLOT(setShowAccelerationAngular())); + + property_size_transform_rotation_ = new rviz::FloatProperty( + "Size Transform Rotation", size_transform_rotation_, + "Size of the axes of the rotation transforms.", + this, SLOT(setSizeTransformRotation())); + property_size_transform_rotation_->setMin(0); + + property_diameter_arrows_ = new rviz::FloatProperty( + "Diameter Arrows", diameter_arrows_, + "Diameter of the arrows.", + this, SLOT(setDiameterArrows())); + property_diameter_arrows_->setMin(0); + + property_scale_velocity_linear_ = new rviz::FloatProperty( + "Scale Velocity Linear", scale_velocity_linear_, + "Scale of the linear velocities.", + this, SLOT(setScaleVelocityLinear())); + + property_scale_velocity_angular_ = new rviz::FloatProperty( + "Scale Velocity Angular", scale_velocity_angular_, + "Scale of the angular velocities.", + this, SLOT(setScaleVelocityAngular())); + + property_scale_acceleration_linear_ = new rviz::FloatProperty( + "Scale Acceleration Linear", scale_acceleration_linear_, + "Scale of the linear accelerations.", + this, SLOT(setScaleAccelerationLinear())); + + property_scale_acceleration_angular_ = new rviz::FloatProperty( + "Scale Acceleration Angular", scale_acceleration_angular_, + "Scale of the angular accelerations.", + this, SLOT(setScaleAccelerationAngular())); + + property_color_connection_ = new rviz::ColorProperty( + "Color Connection", rviz::ogreToQt(color_connection_), + "Color of connection lines.", + this, SLOT(setColorConnection())); + + property_color_velocity_linear_ = new rviz::ColorProperty( + "Color Velocity Linear", rviz::ogreToQt(color_velocity_linear_), + "Color of the linear velocities.", + this, SLOT(setColorVelocityLinear())); + + property_color_velocity_angular_ = new rviz::ColorProperty( + "Color Velocity Angular", rviz::ogreToQt(color_velocity_angular_), + "Color of the angular velocities.", + this, SLOT(setColorVelocityAngular())); + + property_color_acceleration_linear_ = new rviz::ColorProperty( + "Color Acceleration Linear", rviz::ogreToQt(color_acceleration_linear_), + "Color of the linear accelerations.", + this, SLOT(setColorAccelerationLinear())); + + property_color_acceleration_angular_ = new rviz::ColorProperty( + "Color Acceleration Angular", rviz::ogreToQt(color_acceleration_angular_), + "Color of the angular accelerations.", + this, SLOT(setColorAccelerationAngular())); + + property_alpha_ = new rviz::FloatProperty( + "Alpha", alpha_, + "0 is fully transparent, 1.0 is fully opaque.", + this, SLOT(setAlpha())); + property_alpha_->setMin(0); + property_alpha_->setMax(1); + + property_font_size_ = new rviz::FloatProperty( + "Font Size", font_size_, + "Size of the font.", + this, SLOT(setFontSize())); + property_font_size_->setMin(0); + + property_show_text_ = new rviz::BoolProperty( + "Show Caption", show_text_, + "Enable or disable text rendering.", + this, SLOT(setShowText())); + + property_history_length_ = new rviz::IntProperty( + "History Length", 1, + "Number of prior measurements to display.", + this, SLOT(setHistoryLength())); + property_history_length_->setMin(1); + property_history_length_->setMax(100000); +} + +MultiDOFJointTrajectoryDisplay::~MultiDOFJointTrajectoryDisplay() +{ + +} + +void MultiDOFJointTrajectoryDisplay::onInitialize() +{ + MFDClass::onInitialize(); + setHistoryLength(); +} + +void MultiDOFJointTrajectoryDisplay::reset() +{ + MFDClass::reset(); + visuals_points_.clear(); + visuals_connections_.clear(); +} + +void MultiDOFJointTrajectoryDisplay::setShowConnection() +{ + show_connection_ = property_show_connection_->getBool(); + updateShowConnection(); +} + +void MultiDOFJointTrajectoryDisplay::setShowTransformRotation() +{ + show_transform_rotation_ = property_show_transform_rotation_->getBool(); + updateShowTransformRotation(); +} + +void MultiDOFJointTrajectoryDisplay::setShowVelocityLinear() +{ + show_velocity_linear_ = property_show_velocity_linear_->getBool(); + updateShowVelocityLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setShowVelocityAngular() +{ + show_velocity_angular_ = property_show_velocity_angular_->getBool(); + updateShowVelocityAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setShowAccelerationLinear() +{ + show_acceleration_linear_ = property_show_acceleration_linear_->getBool(); + updateShowAccelerationLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setShowAccelerationAngular() +{ + show_acceleration_angular_ = property_show_acceleration_angular_->getBool(); + updateShowAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setSizeTransformRotation() +{ + size_transform_rotation_ = property_size_transform_rotation_->getFloat(); + updateSizeTransformRotation(); +} + +void MultiDOFJointTrajectoryDisplay::setDiameterArrows() +{ + diameter_arrows_ = property_diameter_arrows_->getFloat(); + updateDiameterArrows(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleVelocityLinear() +{ + scale_velocity_linear_ = property_scale_velocity_linear_->getFloat(); + updateScaleVelocityLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleVelocityAngular() +{ + scale_velocity_angular_ = property_scale_velocity_angular_->getFloat(); + updateScaleVelocityAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleAccelerationLinear() +{ + scale_acceleration_linear_ = property_scale_acceleration_linear_->getFloat(); + updateScaleAccelerationLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleAccelerationAngular() +{ + scale_acceleration_angular_ = property_scale_acceleration_angular_->getFloat(); + updateScaleAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setAlpha() +{ + alpha_ = property_alpha_->getFloat(); + color_connection_.a = alpha_; + color_velocity_linear_.a = alpha_; + color_velocity_angular_.a = alpha_; + color_acceleration_linear_.a = alpha_; + color_acceleration_angular_.a = alpha_; + updateColorConnection(); + updateAlphaTransformRotation(); + updateColorVelocityLinear(); + updateColorVelocityAngular(); + updateColorAccelerationLinear(); + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setColorConnection() +{ + color_connection_ = rviz::qtToOgre(property_color_connection_->getColor()); + color_connection_.a = property_alpha_->getFloat(); + updateColorConnection(); +} + +void MultiDOFJointTrajectoryDisplay::setColorVelocityLinear() +{ + color_velocity_linear_ = rviz::qtToOgre(property_color_velocity_linear_->getColor()); + color_velocity_linear_.a = property_alpha_->getFloat(); + updateColorVelocityLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setColorVelocityAngular() +{ + color_velocity_angular_ = rviz::qtToOgre(property_color_velocity_angular_->getColor()); + color_velocity_angular_.a = property_alpha_->getFloat(); + updateColorVelocityAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setColorAccelerationLinear() +{ + color_acceleration_linear_ = rviz::qtToOgre(property_color_acceleration_linear_->getColor()); + color_acceleration_linear_.a = property_alpha_->getFloat(); + updateColorAccelerationLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setColorAccelerationAngular() +{ + color_acceleration_angular_ = rviz::qtToOgre(property_color_acceleration_angular_->getColor()); + color_acceleration_angular_.a = property_alpha_->getFloat(); + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setFontSize() +{ + font_size_ = property_font_size_->getFloat(); + updateFontSize(); +} + +void MultiDOFJointTrajectoryDisplay::setShowText() +{ + show_text_ = property_show_text_->getBool(); + updateShowText(); +} + + +void MultiDOFJointTrajectoryDisplay::setHistoryLength() +{ + visuals_points_.rset_capacity(property_history_length_->getInt()); + visuals_connections_.rset_capacity(property_history_length_->getInt()); +} + +void MultiDOFJointTrajectoryDisplay::processMessage(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg) +{ + show_connection_ = property_show_connection_->getBool(); + show_transform_rotation_ = property_show_transform_rotation_->getBool(); + show_velocity_linear_ = property_show_velocity_linear_->getBool(); + show_velocity_angular_ = property_show_velocity_angular_->getBool(); + show_acceleration_linear_ = property_show_acceleration_linear_->getBool(); + show_acceleration_angular_ = property_show_acceleration_angular_->getBool(); + + size_transform_rotation_ = property_size_transform_rotation_->getFloat(); + scale_velocity_linear_ = property_scale_velocity_linear_->getFloat(); + scale_velocity_angular_ = property_scale_velocity_angular_->getFloat(); + scale_acceleration_linear_ = property_scale_acceleration_linear_->getFloat(); + scale_acceleration_angular_ = property_scale_acceleration_angular_->getFloat(); + + color_connection_ = rviz::qtToOgre(property_color_connection_->getColor()); + color_velocity_linear_ = rviz::qtToOgre(property_color_velocity_linear_->getColor()); + color_velocity_angular_ = rviz::qtToOgre(property_color_velocity_angular_->getColor()); + color_acceleration_linear_ = rviz::qtToOgre(property_color_acceleration_linear_->getColor()); + color_acceleration_angular_ = rviz::qtToOgre(property_color_acceleration_angular_->getColor()); + alpha_ = property_alpha_->getFloat(); + color_connection_.a = alpha_; + color_velocity_linear_.a = alpha_; + color_velocity_angular_.a = alpha_; + color_acceleration_linear_.a = alpha_; + color_acceleration_angular_.a = alpha_; + + std::vector> captions; + for (unsigned int i = 0; i < msg->points.size(); i++) + { + std::vector caption_point; + for (unsigned int j = 0; j < msg->joint_names.size(); j++) + { + std::stringstream ss; + ss << msg->joint_names[j] << ": t" << i << " = " << msg->points[i].time_from_start.toSec() << "s"; + caption_point.push_back(ss.str()); + } + captions.push_back(caption_point); + } + + font_size_ = property_font_size_->getFloat(); + show_text_ = property_show_text_->getBool(); + + std::vector> visuals_points; + std::vector> visuals_connections; + + trajectory_msgs::MultiDOFJointTrajectoryPoint last_point; + trajectory_msgs::MultiDOFJointTrajectoryPoint current_point = msg->points[0]; + + // add first point + visuals_points.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointVisual( + context_->getSceneManager(), + scene_node_, + current_point, + show_transform_rotation_, + show_velocity_linear_, + show_velocity_angular_, + show_acceleration_linear_, + show_acceleration_angular_, + size_transform_rotation_, + diameter_arrows_, + scale_velocity_linear_, + scale_velocity_angular_, + scale_acceleration_linear_, + scale_acceleration_angular_, + alpha_, + color_velocity_linear_, + color_velocity_angular_, + color_acceleration_linear_, + color_acceleration_angular_, + captions[0], + font_size_, + show_text_))); + + // add second to last points and connections to predecessors + for (unsigned int i = 1; i < msg->points.size(); i++) + { + // go one pose further + last_point = current_point; + current_point = msg->points[i]; + + // add edge to predecessor + visuals_connections.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointConnectionVisual(context_->getSceneManager(), + scene_node_, + last_point, + current_point, + show_connection_, + color_connection_))); + + // add pose + visuals_points.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointVisual( + context_->getSceneManager(), + scene_node_, + current_point, + show_transform_rotation_, + show_velocity_linear_, + show_velocity_angular_, + show_acceleration_linear_, + show_acceleration_angular_, + size_transform_rotation_, + diameter_arrows_, + scale_velocity_linear_, + scale_velocity_angular_, + scale_acceleration_linear_, + scale_acceleration_angular_, + alpha_, + color_velocity_linear_, + color_velocity_angular_, + color_acceleration_linear_, + color_acceleration_angular_, + captions[i], + font_size_, + show_text_))); + } + + visuals_points_.push_back(visuals_points); + visuals_connections_.push_back(visuals_connections); +} + +void MultiDOFJointTrajectoryDisplay::updateShowConnection() +{ + for(size_t i = 0; i < visuals_connections_.size(); i++) + { + for (unsigned int j = 0; j < visuals_connections_[i].size(); j++) + { + visuals_connections_[i][j]->setShowConnection(show_connection_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowTransformRotation() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setShowTransformRotation(show_transform_rotation_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowVelocityLinear() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setShowVelocityLinear(show_velocity_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowVelocityAngular() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setShowVelocityAngular(show_velocity_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowAccelerationLinear() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setShowAccelerationLinear(show_acceleration_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowAccelerationAngular() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setShowAccelerationAngular(show_acceleration_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateSizeTransformRotation() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setSizeTransformRotation(size_transform_rotation_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateDiameterArrows() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setDiametersArrows(diameter_arrows_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleVelocityLinear() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setScaleVelocityLinear(scale_velocity_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleVelocityAngular() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setScaleVelocityAngular(scale_velocity_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleAccelerationLinear() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setScaleAccelerationLinear(scale_acceleration_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleAccelerationAngular() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setScaleAccelerationAngular(scale_acceleration_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorConnection() +{ + for(size_t i = 0; i < visuals_connections_.size(); i++) + { + for (unsigned int j = 0; j < visuals_connections_[i].size(); j++) + { + visuals_connections_[i][j]->setColor(color_connection_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateAlphaTransformRotation() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setAlphaTransformRotation(alpha_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorVelocityLinear() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setColorVelocityLinear(color_velocity_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorVelocityAngular() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setColorVelocityAngular(color_velocity_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorAccelerationLinear() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setColorAccelerationLinear(color_acceleration_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorAccelerationAngular() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setColorAccelerationAngular(color_acceleration_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateFontSize() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setFontSize(font_size_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowText() +{ + for(size_t i = 0; i < visuals_points_.size(); i++) + { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) + { + visuals_points_[i][j]->setShowText(show_text_); + } + } +} + +} // multi_dof_joint_trajectory_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(multi_dof_joint_trajectory_rviz_plugins::MultiDOFJointTrajectoryDisplay, rviz::Display) diff --git a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp new file mode 100644 index 0000000..e85cde4 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp @@ -0,0 +1,69 @@ +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp" + + +namespace multi_dof_joint_trajectory_rviz_plugins { + +MultiDOFJointTrajectoryPointConnectionVisual::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) +: scene_manager_(scene_manager), + show_connection_(show_connection), + color_(color) +{ + // check vector lengths + assert(from.transforms.size() == to.transforms.size() && "ERROR: MultiDOFJointTrajectoryPointConnectionVisual: from and to vectors do not have the same length."); + + // scene node + scene_node_ = parent_node->createChildSceneNode(); + + // lines + for (unsigned int i = 0; i < from.transforms.size(); i++) + { + lines_.push_back(boost::shared_ptr(new rviz::Line(scene_manager_, scene_node_))); + const Ogre::Vector3 fromVector(from.transforms[i].translation.x, from.transforms[i].translation.y, from.transforms[i].translation.z); + const Ogre::Vector3 toVector(to.transforms[i].translation.x, to.transforms[i].translation.y, to.transforms[i].translation.z); + lines_.back()->setPoints(fromVector, toVector); + updateColor(); + lines_.back()->setVisible(show_connection_); + } +} + +MultiDOFJointTrajectoryPointConnectionVisual::~MultiDOFJointTrajectoryPointConnectionVisual() +{ + scene_manager_->destroySceneNode(scene_node_); +} + +void MultiDOFJointTrajectoryPointConnectionVisual::setShowConnection(bool visible) +{ + show_connection_ = visible; + updateShowConnection(); +} + +void MultiDOFJointTrajectoryPointConnectionVisual::setColor(const Ogre::ColourValue& color) +{ + color_ = color; + updateColor(); +} + +void MultiDOFJointTrajectoryPointConnectionVisual::updateShowConnection() +{ + for (unsigned int i = 0; i < lines_.size(); i++) + { + lines_[i]->setVisible(show_connection_); + } +} + +void MultiDOFJointTrajectoryPointConnectionVisual::updateColor() +{ + for (unsigned int i = 0; i < lines_.size(); i++) + { + lines_[i]->setColor(color_); + } +} + +} // multi_dof_joint_trajectory_rviz_plugins + diff --git a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp new file mode 100644 index 0000000..f50fe09 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp @@ -0,0 +1,436 @@ +#include + +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp" + + +namespace multi_dof_joint_trajectory_rviz_plugins { + +MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( + Ogre::SceneManager* scene_manager, + Ogre::SceneNode* parent_node, + const trajectory_msgs::MultiDOFJointTrajectoryPoint& msg, + 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_transform_rotatation, + const Ogre::ColourValue& color_velocity_linear, + const Ogre::ColourValue& color_velocity_angular, + const Ogre::ColourValue& color_acceleration_linear, + const Ogre::ColourValue& color_acceleration_angular, + const std::vector& captions, + float font_size, + bool show_text) +: scene_manager_(scene_manager), + axis_radius_per_size_(0.1), + show_transform_rotation_(show_transform_rotation), + show_velocity_linear_(show_velocity_linear), + show_velocity_angular_(show_velocity_angular), + show_acceleration_linear_(show_acceleration_linear), + show_acceleration_angular_(show_acceleration_angular), + size_transform_rotation_(size_transform_rotation), + diameter_arrows_(diameter_arrows), + scale_velocity_linear_(scale_velocity_linear), + scale_velocity_angular_(scale_velocity_angular), + scale_acceleration_linear_(scale_acceleration_linear), + scale_acceleration_angular_(scale_acceleration_angular), + alpha_transform_rotatation_(alpha_transform_rotatation), + color_velocity_linear_(color_velocity_linear), + color_velocity_angular_(color_velocity_angular), + color_acceleration_linear_(color_acceleration_linear), + color_acceleration_angular_(color_acceleration_angular), + captions_(captions), + font_size_(font_size), + show_text_(show_text) +{ + // check vector lengths + assert((msg.transforms.size() == captions.size()) && "ERROR: MultiDOFJointTrajectoryPointVisual: captions and transforms vectors do not have the same length."); + assert((msg.velocities.size() == captions.size() || msg.velocities.size() == 0) && "ERROR: MultiDOFJointTrajectoryPointVisual: velocity vector has invalid length."); + assert((msg.accelerations.size() == captions.size() || msg.accelerations.size() == 0) && "ERROR: MultiDOFJointTrajectoryPointVisual: acceleration vector has invalid length."); + + const bool message_contains_velocities = (msg.velocities.size() == msg.transforms.size()); + const bool message_contains_accelerations = (msg.accelerations.size() == msg.transforms.size()); + + // loop through all joints + for (unsigned int i = 0; i < msg.transforms.size(); i++) + { + // transform position + transforms_position_.push_back(parent_node->createChildSceneNode()); + transforms_position_.back()->setPosition(msg.transforms[i].translation.x, msg.transforms[i].translation.y, msg.transforms[i].translation.z); + + // transform rotation + const Ogre::Quaternion orientation(msg.transforms[i].rotation.w, msg.transforms[i].rotation.x, msg.transforms[i].rotation.y, msg.transforms[i].rotation.z); + transforms_rotation_.push_back(boost::shared_ptr(new rviz::Axes(scene_manager_, transforms_position_.back(), size_transform_rotation, axis_radius_per_size_*size_transform_rotation))); + transforms_rotation_.back()->setOrientation(orientation); + + // define common variables + Ogre::Vector3 vector; + + if (message_contains_velocities) + { + // velocity linear + vector = Ogre::Vector3(msg.velocities[i].linear.x, msg.velocities[i].linear.y, msg.velocities[i].linear.z); + velocities_linear_absolute_.push_back(vector.length()); + velocities_linear_.push_back(boost::shared_ptr(new rviz::Arrow( + scene_manager, + transforms_position_.back(), + 0.8*scale_velocity_linear_*velocities_linear_absolute_.back(), + diameter_arrows_, + 0.2*scale_velocity_linear_*velocities_linear_absolute_.back(), + 2.0*diameter_arrows_))); + velocities_linear_.back()->setDirection(vector); + + // velocity angular + vector = Ogre::Vector3(msg.velocities[i].angular.x, msg.velocities[i].angular.y, msg.velocities[i].angular.z); + velocities_angular_absolute_.push_back(vector.length()); + velocities_angular_.push_back(boost::shared_ptr(new rviz::Arrow( + scene_manager, + transforms_position_.back(), + 0.8*scale_velocity_angular_*velocities_angular_absolute_.back(), + diameter_arrows_, + 0.2*scale_velocity_angular_*velocities_angular_absolute_.back(), + 2.0*diameter_arrows_))); + velocities_angular_.back()->setDirection(vector); + } + + if (message_contains_accelerations) + { + // acceleration linear + vector = Ogre::Vector3(msg.accelerations[i].linear.x, msg.accelerations[i].linear.y, msg.accelerations[i].linear.z); + accelerations_linear_absolute_.push_back(vector.length()); + accelerations_linear_.push_back(boost::shared_ptr(new rviz::Arrow( + scene_manager, + transforms_position_.back(), + 0.8*scale_acceleration_linear_*accelerations_linear_absolute_.back(), + diameter_arrows_, + 0.2*scale_acceleration_linear_*accelerations_linear_absolute_.back(), + 2.0*diameter_arrows_))); + accelerations_linear_.back()->setDirection(vector); + + // acceleration angular + vector = Ogre::Vector3(msg.accelerations[i].angular.x, msg.accelerations[i].angular.y, msg.accelerations[i].angular.z); + accelerations_angular_absolute_.push_back(vector.length()); + accelerations_angular_.push_back(boost::shared_ptr(new rviz::Arrow( + scene_manager, + transforms_position_.back(), + 0.8*scale_acceleration_angular_*accelerations_angular_absolute_.back(), + diameter_arrows_, + 0.2*scale_acceleration_angular_*accelerations_angular_absolute_.back(), + 2.0*diameter_arrows_))); + accelerations_angular_.back()->setDirection(vector); + } + + // text + texts_.push_back(boost::shared_ptr(new rviz::MovableText(captions_[i], "Arial", getCharacterHeight()))); + texts_.back()->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_BELOW); + transforms_position_.back()->attachObject(texts_.back().get()); + } + + // update all colors + updateAlphaTransformRotation(); + updateColorVelocityLinear(); + updateColorVelocityAngular(); + updateColorAccelerationLinear(); + updateColorAccelerationAngular(); +} + +MultiDOFJointTrajectoryPointVisual::~MultiDOFJointTrajectoryPointVisual() +{ + for (unsigned int i = 0; i < transforms_position_.size(); i++) + { + scene_manager_->destroySceneNode(transforms_position_[i]); + } +} + +void MultiDOFJointTrajectoryPointVisual::setShowTransformRotation(bool visible) +{ + show_transform_rotation_ = visible; + updateAlphaTransformRotation(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowVelocityLinear(bool visible) +{ + show_velocity_linear_ = visible; + updateColorVelocityLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowVelocityAngular(bool visible) +{ + show_velocity_angular_ = visible; + updateColorVelocityAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowAccelerationLinear(bool visible) +{ + show_acceleration_linear_ = visible; + updateColorAccelerationLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowAccelerationAngular(bool visible) +{ + show_acceleration_angular_ = visible; + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setSizeTransformRotation(float size) +{ + size_transform_rotation_ = size; + updateSizeTransformRotation(); +} + +void MultiDOFJointTrajectoryPointVisual::setDiametersArrows(float diameter) +{ + diameter_arrows_ = diameter; + updateDiametersArrows(); +} + +void MultiDOFJointTrajectoryPointVisual::setScaleVelocityLinear(float scale) +{ + scale_velocity_linear_ = scale; + updateScaleVelocityLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setScaleVelocityAngular(float scale) +{ + scale_velocity_angular_ = scale; + updateScaleVelocityAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setScaleAccelerationLinear(float scale) +{ + scale_acceleration_linear_ = scale; + updateScaleAccelerationLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setScaleAccelerationAngular(float scale) +{ + scale_acceleration_angular_ = scale; + updateScaleAccelerationAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setAlphaTransformRotation(float alpha) +{ + alpha_transform_rotatation_ = alpha; + updateAlphaTransformRotation(); +} + +void MultiDOFJointTrajectoryPointVisual::setColorVelocityLinear(const Ogre::ColourValue& color) +{ + color_velocity_linear_ = color; + updateColorVelocityLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setColorVelocityAngular(const Ogre::ColourValue& color) +{ + color_velocity_angular_ = color; + updateColorVelocityAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setColorAccelerationLinear(const Ogre::ColourValue& color) +{ + color_acceleration_linear_ = color; + updateColorAccelerationLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setColorAccelerationAngular(const Ogre::ColourValue& color) +{ + color_acceleration_angular_ = color; + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setCaptions(const std::vector& captions) +{ + // check vector lengths + assert(captions_.size() == captions.size() && "ERROR: MultiDOFJointTrajectoryPointVisual: old and new captions vectors do not have the same length."); + + captions_ = captions; + updateCaptions(); +} + +void MultiDOFJointTrajectoryPointVisual::setFontSize(float font_size) +{ + font_size_ = font_size; + updateFontSize(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowText(bool show_text) +{ + show_text_ = show_text; + updateShowText(); +} + +void MultiDOFJointTrajectoryPointVisual::updateSizeTransformRotation() +{ + for (unsigned int i = 0; i < transforms_rotation_.size(); i++) + { + transforms_rotation_[i]->set(size_transform_rotation_, axis_radius_per_size_*size_transform_rotation_); + } + updateAlphaTransformRotation(); // colors are reset by set() +} + +void MultiDOFJointTrajectoryPointVisual::updateDiametersArrows() +{ + updateScaleVelocityLinear(); + updateScaleVelocityAngular(); + updateScaleAccelerationLinear(); + updateScaleAccelerationAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::updateScaleVelocityLinear() +{ + for (unsigned int i = 0; i < velocities_linear_.size(); i++) + { + velocities_linear_[i]->set( + 0.8*scale_velocity_linear_*velocities_linear_absolute_[i], + diameter_arrows_, + 0.2*scale_velocity_linear_*velocities_linear_absolute_[i], + 2.0*diameter_arrows_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateScaleVelocityAngular() +{ + for (unsigned int i = 0; i < velocities_angular_.size(); i++) + { + velocities_angular_[i]->set( + 0.8*scale_velocity_angular_*velocities_angular_absolute_[i], + diameter_arrows_, + 0.2*scale_velocity_angular_*velocities_angular_absolute_[i], + 2.0*diameter_arrows_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateScaleAccelerationLinear() +{ + for (unsigned int i = 0; i < accelerations_linear_.size(); i++) + { + accelerations_linear_[i]->set( + 0.8*scale_acceleration_linear_*accelerations_linear_absolute_[i], + diameter_arrows_, + 0.2*scale_acceleration_linear_*accelerations_linear_absolute_[i], + 2.0*diameter_arrows_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateScaleAccelerationAngular() +{ + for (unsigned int i = 0; i < accelerations_angular_.size(); i++) + { + accelerations_angular_[i]->set( + 0.8*scale_acceleration_angular_*accelerations_angular_absolute_[i], + diameter_arrows_, + 0.2*scale_acceleration_angular_*accelerations_angular_absolute_[i], + 2.0*diameter_arrows_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateAlphaTransformRotation() +{ + Ogre::ColourValue color; + for (unsigned int i = 0; i < transforms_rotation_.size(); i++) + { + color = transforms_rotation_[i]->getDefaultXColor(); + color.a = show_transform_rotation_*alpha_transform_rotatation_; + transforms_rotation_[i]->setXColor(color); + color = transforms_rotation_[i]->getDefaultYColor(); + color.a = show_transform_rotation_*alpha_transform_rotatation_; + transforms_rotation_[i]->setYColor(color); + color = transforms_rotation_[i]->getDefaultZColor(); + color.a = show_transform_rotation_*alpha_transform_rotatation_; + transforms_rotation_[i]->setZColor(color); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorVelocityLinear() +{ + for (unsigned int i = 0; i < velocities_linear_.size(); i++) + { + velocities_linear_[i]->setColor(getColor(color_velocity_linear_, show_velocity_linear_)); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorVelocityAngular() +{ + for (unsigned int i = 0; i < velocities_angular_.size(); i++) + { + velocities_angular_[i]->setColor(getColor(color_velocity_angular_, show_velocity_angular_)); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorAccelerationLinear() +{ + for (unsigned int i = 0; i < accelerations_linear_.size(); i++) + { + accelerations_linear_[i]->setColor(getColor(color_acceleration_linear_, show_acceleration_linear_)); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorAccelerationAngular() +{ + for (unsigned int i = 0; i < accelerations_angular_.size(); i++) + { + accelerations_angular_[i]->setColor(getColor(color_acceleration_angular_, show_acceleration_angular_)); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateCaptions() +{ + for (unsigned int i = 0; i < texts_.size(); i++) + { + texts_[i]->setCaption(captions_[i]); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateFontSize() +{ + double character_height = getCharacterHeight(); + for (unsigned int i = 0; i < texts_.size(); i++) + { + texts_[i]->setCharacterHeight(character_height); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateShowText() +{ + double character_height = getCharacterHeight(); + for (unsigned int i = 0; i < texts_.size(); i++) + { + texts_[i]->setCharacterHeight(character_height); + } +} + +Ogre::ColourValue MultiDOFJointTrajectoryPointVisual::getColor(const Ogre::ColourValue& color, bool visible) +{ + if (!visible) + { + Ogre::ColourValue color_invisible = color; + color_invisible.a = 0; + return color_invisible; + } + else + { + return color; + } +} + +float MultiDOFJointTrajectoryPointVisual::getCharacterHeight() +{ + if (show_text_) + { + return font_size_; + } + else + { + return 0; + } +} + +} // multi_dof_joint_trajectory_rviz_plugins +