From e21a655c0daf80c9f38152186d0e9a546c2b56fd Mon Sep 17 00:00:00 2001 From: Remo Diethelm Date: Tue, 17 Mar 2015 11:12:20 +0100 Subject: [PATCH 1/2] added rviz plugin for MultiDOFJointTrajectory --- .../CMakeLists.txt | 95 ++++ .../icons/classes/MultiDOFJointTrajectory.png | Bin 0 -> 273 bytes .../MultiDOFJointTrajectoryDisplay.hpp | 126 +++++ ...OFJointTrajectoryPointConnectionVisual.hpp | 43 ++ .../MultiDOFJointTrajectoryPointVisual.hpp | 117 +++++ .../package.xml | 25 + .../plugin_description.xml | 9 + .../src/MultiDOFJointTrajectoryDisplay.cpp | 489 ++++++++++++++++++ ...OFJointTrajectoryPointConnectionVisual.cpp | 51 ++ .../MultiDOFJointTrajectoryPointVisual.cpp | 389 ++++++++++++++ 10 files changed, 1344 insertions(+) create mode 100644 multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt create mode 100644 multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png create mode 100644 multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp create mode 100644 multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp create mode 100644 multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp create mode 100644 multi_dof_joint_trajectory_rviz_plugins/package.xml create mode 100644 multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml create mode 100644 multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp create mode 100644 multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp create mode 100644 multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp 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 0000000000000000000000000000000000000000..4e415e0fd4a84ccdec52f63f547cb9edb01c71eb GIT binary patch literal 273 zcmeAS@N?(olHy`uVBq!ia0vp^0wB!61|;P_|4#%`Y)RhkE)4%caKYZ?lYt_f1s;*b z3=DinK$vl=HlH+5kiEpy*OmP~vmh52EBor}i-AJ(JY5_^EKXmYw3nC3kf(LO%Zn8Y z(gn4`5+?RIoaTGuc-G)c$V$cXekC#cos&IQ9GI%S|M~fCO!mUw6CP-FOmla9Gp|x; z&3-nHGTDrfhywx6!Z}m6u1=e!Fym*4^}A*@4ztJ=ru#3l)K0!BZen`Leam#wZNc** zKYm+!;NU^M`Vg}nESw)c2mS91+kNa_pT&+lZ`59_{pecoEm;0l{bRoA3lm@R=R~~# Px|G4w)z4*}Q$iB}6|iW6 literal 0 HcmV?d00001 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..dce2eb3 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp @@ -0,0 +1,126 @@ +#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 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 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>> points_visuals_; + boost::circular_buffer>> connections_visuals_; + + rviz::FloatProperty* size_property_transform_rotation_; + rviz::FloatProperty* diameter_property_arrows_; + rviz::FloatProperty* scale_property_velocity_linear_; + rviz::FloatProperty* scale_property_velocity_angular_; + rviz::FloatProperty* scale_property_acceleration_linear_; + rviz::FloatProperty* scale_property_acceleration_angular_; + + rviz::ColorProperty* color_property_connection_; + rviz::ColorProperty* color_property_velocity_linear_; + rviz::ColorProperty* color_property_velocity_angular_; + rviz::ColorProperty* color_property_acceleration_linear_; + rviz::ColorProperty* color_property_acceleration_angular_; + + rviz::FloatProperty* alpha_property_; + + rviz::FloatProperty* font_size_property_; + rviz::BoolProperty* show_text_property_; + + rviz::IntProperty* history_length_property_; + + 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..fd8e65d --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp @@ -0,0 +1,43 @@ +#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, + const Ogre::ColourValue& color); + virtual ~MultiDOFJointTrajectoryPointConnectionVisual(); + + void setColor(const Ogre::ColourValue& color); + +private: + void updateColor(); + + Ogre::SceneManager* scene_manager_; + + Ogre::SceneNode* scene_node_; + + std::vector> lines_; + + 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..8357f30 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp @@ -0,0 +1,117 @@ +#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, + 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 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(); + + 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_; + + 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..7ebdbc1 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp @@ -0,0 +1,489 @@ +#include + +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp" + + +namespace multi_dof_joint_trajectory_rviz_plugins { + +MultiDOFJointTrajectoryDisplay::MultiDOFJointTrajectoryDisplay() +: 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) +{ + size_property_transform_rotation_ = new rviz::FloatProperty( + "Size Transform Rotation", size_transform_rotation_, + "Size of the axes of the rotation transform.", + this, SLOT(setSizeTransformRotation())); + size_property_transform_rotation_->setMin(0); + + diameter_property_arrows_ = new rviz::FloatProperty( + "Diameter Arrows", diameter_arrows_, + "Diameter of the arrows.", + this, SLOT(setDiameterArrows())); + diameter_property_arrows_->setMin(0); + + scale_property_velocity_linear_ = new rviz::FloatProperty( + "Scale Velocity Linear", scale_velocity_linear_, + "Scale of the linear velocity.", + this, SLOT(setScaleVelocityLinear())); + + scale_property_velocity_angular_ = new rviz::FloatProperty( + "Scale Velocity Angular", scale_velocity_angular_, + "Scale of the angular velocity.", + this, SLOT(setScaleVelocityAngular())); + + scale_property_acceleration_linear_ = new rviz::FloatProperty( + "Scale Acceleration Linear", scale_acceleration_linear_, + "Scale of the linear acceleration.", + this, SLOT(setScaleAccelerationLinear())); + + scale_property_acceleration_angular_ = new rviz::FloatProperty( + "Scale Acceleration Angular", scale_acceleration_angular_, + "Scale of the angular acceleration.", + this, SLOT(setScaleAccelerationAngular())); + + color_property_connection_ = new rviz::ColorProperty( + "Color Connection", rviz::ogreToQt(color_connection_), + "Color of connection lines.", + this, SLOT(setColorConnection())); + + color_property_velocity_linear_ = new rviz::ColorProperty( + "Color Velocity Linear", rviz::ogreToQt(color_velocity_linear_), + "Color of the linear velocity.", + this, SLOT(setColorVelocityLinear())); + + color_property_velocity_angular_ = new rviz::ColorProperty( + "Color Velocity Angular", rviz::ogreToQt(color_velocity_angular_), + "Color of the angular velocity.", + this, SLOT(setColorVelocityAngular())); + + color_property_acceleration_linear_ = new rviz::ColorProperty( + "Color Acceleration Linear", rviz::ogreToQt(color_acceleration_linear_), + "Color of the linear acceleration.", + this, SLOT(setColorAccelerationLinear())); + + color_property_acceleration_angular_ = new rviz::ColorProperty( + "Color Acceleration Angular", rviz::ogreToQt(color_acceleration_angular_), + "Color of the angular acceleration.", + this, SLOT(setColorAccelerationAngular())); + + alpha_property_ = new rviz::FloatProperty( + "Alpha", alpha_, + "0 is fully transparent, 1.0 is fully opaque.", + this, SLOT(setAlpha())); + alpha_property_->setMin(0); + alpha_property_->setMax(1); + + font_size_property_ = new rviz::FloatProperty( + "Font Size", font_size_, + "Size of the font.", + this, SLOT(setFontSize())); + font_size_property_->setMin(0); + + show_text_property_ = new rviz::BoolProperty( + "Show Caption", show_text_, + "Enable or disable text rendering.", + this, SLOT(setShowText())); + + history_length_property_ = new rviz::IntProperty( + "History Length", 1, + "Number of prior measurements to display.", + this, SLOT(setHistoryLength())); + history_length_property_->setMin(1); + history_length_property_->setMax(100000); +} + +MultiDOFJointTrajectoryDisplay::~MultiDOFJointTrajectoryDisplay() +{ + +} + +void MultiDOFJointTrajectoryDisplay::onInitialize() +{ + MFDClass::onInitialize(); + setHistoryLength(); +} + +void MultiDOFJointTrajectoryDisplay::reset() +{ + MFDClass::reset(); + points_visuals_.clear(); + connections_visuals_.clear(); +} + +void MultiDOFJointTrajectoryDisplay::setSizeTransformRotation() +{ + size_transform_rotation_ = size_property_transform_rotation_->getFloat(); + updateSizeTransformRotation(); +} + +void MultiDOFJointTrajectoryDisplay::setDiameterArrows() +{ + diameter_arrows_ = diameter_property_arrows_->getFloat(); + updateDiameterArrows(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleVelocityLinear() +{ + scale_velocity_linear_ = scale_property_velocity_linear_->getFloat(); + updateScaleVelocityLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleVelocityAngular() +{ + scale_velocity_angular_ = scale_property_velocity_angular_->getFloat(); + updateScaleVelocityAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleAccelerationLinear() +{ + scale_acceleration_linear_ = scale_property_acceleration_linear_->getFloat(); + updateScaleAccelerationLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleAccelerationAngular() +{ + scale_acceleration_angular_ = scale_property_acceleration_angular_->getFloat(); + updateScaleAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setAlpha() +{ + alpha_ = alpha_property_->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(color_property_connection_->getColor()); + color_connection_.a = alpha_property_->getFloat(); + updateColorConnection(); +} + +void MultiDOFJointTrajectoryDisplay::setColorVelocityLinear() +{ + color_velocity_linear_ = rviz::qtToOgre(color_property_velocity_linear_->getColor()); + color_velocity_linear_.a = alpha_property_->getFloat(); + updateColorVelocityLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setColorVelocityAngular() +{ + color_velocity_angular_ = rviz::qtToOgre(color_property_velocity_angular_->getColor()); + color_velocity_angular_.a = alpha_property_->getFloat(); + updateColorVelocityAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setColorAccelerationLinear() +{ + color_acceleration_linear_ = rviz::qtToOgre(color_property_acceleration_linear_->getColor()); + color_acceleration_linear_.a = alpha_property_->getFloat(); + updateColorAccelerationLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setColorAccelerationAngular() +{ + color_acceleration_angular_ = rviz::qtToOgre(color_property_acceleration_angular_->getColor()); + color_acceleration_angular_.a = alpha_property_->getFloat(); + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setFontSize() +{ + font_size_ = font_size_property_->getFloat(); + updateFontSize(); +} + +void MultiDOFJointTrajectoryDisplay::setShowText() +{ + show_text_ = show_text_property_->getBool(); + updateShowText(); +} + + +void MultiDOFJointTrajectoryDisplay::setHistoryLength() +{ + points_visuals_.rset_capacity(history_length_property_->getInt()); + connections_visuals_.rset_capacity(history_length_property_->getInt()); +} + +void MultiDOFJointTrajectoryDisplay::processMessage(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg) +{ + size_transform_rotation_ = size_property_transform_rotation_->getFloat(); + scale_velocity_linear_ = scale_property_velocity_linear_->getFloat(); + scale_velocity_angular_ = scale_property_velocity_angular_->getFloat(); + scale_acceleration_linear_ = scale_property_acceleration_linear_->getFloat(); + scale_acceleration_angular_ = scale_property_acceleration_angular_->getFloat(); + + color_connection_ = rviz::qtToOgre(color_property_connection_->getColor()); + color_velocity_linear_ = rviz::qtToOgre(color_property_velocity_linear_->getColor()); + color_velocity_angular_ = rviz::qtToOgre(color_property_velocity_angular_->getColor()); + color_acceleration_linear_ = rviz::qtToOgre(color_property_acceleration_linear_->getColor()); + color_acceleration_angular_ = rviz::qtToOgre(color_property_acceleration_angular_->getColor()); + alpha_ = alpha_property_->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_ = font_size_property_->getFloat(); + show_text_ = show_text_property_->getBool(); + + std::vector> points_visuals; + std::vector> connections_visuals; + + trajectory_msgs::MultiDOFJointTrajectoryPoint last_point; + trajectory_msgs::MultiDOFJointTrajectoryPoint current_point = msg->points[0]; + + // add first point + points_visuals.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointVisual( + context_->getSceneManager(), + scene_node_, + current_point, + 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 + connections_visuals.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointConnectionVisual(context_->getSceneManager(), + scene_node_, + last_point, + current_point, + color_connection_))); + + // add pose + points_visuals.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointVisual( + context_->getSceneManager(), + scene_node_, + current_point, + 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_))); + } + + points_visuals_.push_back(points_visuals); + connections_visuals_.push_back(connections_visuals); +} + +void MultiDOFJointTrajectoryDisplay::updateSizeTransformRotation() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setSizeTransformRotation(size_transform_rotation_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateDiameterArrows() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setDiametersArrows(diameter_arrows_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleVelocityLinear() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setScaleVelocityLinear(scale_velocity_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleVelocityAngular() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setScaleVelocityAngular(scale_velocity_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleAccelerationLinear() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setScaleAccelerationLinear(scale_acceleration_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleAccelerationAngular() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setScaleAccelerationAngular(scale_acceleration_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorConnection() +{ + for(size_t i = 0; i < connections_visuals_.size(); i++) + { + for (unsigned int j = 0; j < connections_visuals_[i].size(); j++) + { + connections_visuals_[i][j]->setColor(color_connection_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateAlphaTransformRotation() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setAlphaTransformRotation(alpha_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorVelocityLinear() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setColorVelocityLinear(color_velocity_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorVelocityAngular() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setColorVelocityAngular(color_velocity_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorAccelerationLinear() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setColorAccelerationLinear(color_acceleration_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorAccelerationAngular() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setColorAccelerationAngular(color_acceleration_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateFontSize() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[i][j]->setFontSize(font_size_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowText() +{ + for(size_t i = 0; i < points_visuals_.size(); i++) + { + for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + { + points_visuals_[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..a5ea5f4 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp @@ -0,0 +1,51 @@ +#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, + const Ogre::ColourValue& color) +: scene_manager_(scene_manager), + 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(); + } +} + +MultiDOFJointTrajectoryPointConnectionVisual::~MultiDOFJointTrajectoryPointConnectionVisual() +{ + scene_manager_->destroySceneNode(scene_node_); +} + +void MultiDOFJointTrajectoryPointConnectionVisual::setColor(const Ogre::ColourValue& color) +{ + color_ = color; + updateColor(); +} + +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..8380410 --- /dev/null +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp @@ -0,0 +1,389 @@ +#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, + 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), + size_transform_rotation_(size_transform_rotation), + diameter_arrows_(diameter_arrows), + alpha_transform_rotatation_(alpha_transform_rotatation), + scale_velocity_linear_(scale_velocity_linear), + scale_velocity_angular_(scale_velocity_angular), + scale_acceleration_linear_(scale_acceleration_linear), + scale_acceleration_angular_(scale_acceleration_angular), + 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); + Ogre::ColourValue color; + color = transforms_rotation_[i]->getDefaultXColor(); + color.a = alpha_transform_rotatation_; + transforms_rotation_.back()->setXColor(color); + color = transforms_rotation_[i]->getDefaultYColor(); + color.a = alpha_transform_rotatation_; + transforms_rotation_.back()->setYColor(color); + color = transforms_rotation_[i]->getDefaultZColor(); + color.a = alpha_transform_rotatation_; + transforms_rotation_.back()->setZColor(color); + + // 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); + velocities_linear_.back()->setColor(color_velocity_linear_); + + // 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); + velocities_angular_.back()->setColor(color_velocity_angular_); + } + + 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); + accelerations_linear_.back()->setColor(color_acceleration_linear_); + + // 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); + accelerations_angular_.back()->setColor(color_acceleration_angular_); + } + + // 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()); + } +} + +MultiDOFJointTrajectoryPointVisual::~MultiDOFJointTrajectoryPointVisual() +{ + for (unsigned int i = 0; i < transforms_position_.size(); i++) + { + scene_manager_->destroySceneNode(transforms_position_[i]); + } +} + +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 = alpha_transform_rotatation_; + transforms_rotation_[i]->setXColor(color); + color = transforms_rotation_[i]->getDefaultYColor(); + color.a = alpha_transform_rotatation_; + transforms_rotation_[i]->setYColor(color); + color = transforms_rotation_[i]->getDefaultZColor(); + color.a = 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(color_velocity_linear_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorVelocityAngular() +{ + for (unsigned int i = 0; i < velocities_angular_.size(); i++) + { + velocities_angular_[i]->setColor(color_velocity_angular_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorAccelerationLinear() +{ + for (unsigned int i = 0; i < accelerations_linear_.size(); i++) + { + accelerations_linear_[i]->setColor(color_acceleration_linear_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorAccelerationAngular() +{ + for (unsigned int i = 0; i < accelerations_angular_.size(); i++) + { + accelerations_angular_[i]->setColor(color_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); + } +} + +float MultiDOFJointTrajectoryPointVisual::getCharacterHeight() +{ + if (show_text_) + { + return font_size_; + } + else + { + return 0; + } +} + +} // multi_dof_joint_trajectory_rviz_plugins + From 8ba8b48fdbad2386c4dc7a44e9da22a91fc71ea0 Mon Sep 17 00:00:00 2001 From: Remo Diethelm Date: Tue, 17 Mar 2015 16:29:50 +0100 Subject: [PATCH 2/2] toggle visualizations on and off --- .../MultiDOFJointTrajectoryDisplay.hpp | 72 +++- ...OFJointTrajectoryPointConnectionVisual.hpp | 4 + .../MultiDOFJointTrajectoryPointVisual.hpp | 18 + .../src/MultiDOFJointTrajectoryDisplay.cpp | 392 ++++++++++++------ ...OFJointTrajectoryPointConnectionVisual.cpp | 28 +- .../MultiDOFJointTrajectoryPointVisual.cpp | 91 +++- 6 files changed, 438 insertions(+), 167 deletions(-) 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 index dce2eb3..dc4e0a6 100644 --- 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 @@ -42,6 +42,13 @@ Q_OBJECT private Q_SLOTS: + void setShowConnection(); + void setShowTransformRotation(); + void setShowVelocityLinear(); + void setShowVelocityAngular(); + void setShowAccelerationLinear(); + void setShowAccelerationAngular(); + void setSizeTransformRotation(); void setDiameterArrows(); void setScaleVelocityLinear(); @@ -65,6 +72,13 @@ private Q_SLOTS: 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(); @@ -82,28 +96,42 @@ private Q_SLOTS: void updateFontSize(); void updateShowText(); - boost::circular_buffer>> points_visuals_; - boost::circular_buffer>> connections_visuals_; - - rviz::FloatProperty* size_property_transform_rotation_; - rviz::FloatProperty* diameter_property_arrows_; - rviz::FloatProperty* scale_property_velocity_linear_; - rviz::FloatProperty* scale_property_velocity_angular_; - rviz::FloatProperty* scale_property_acceleration_linear_; - rviz::FloatProperty* scale_property_acceleration_angular_; - - rviz::ColorProperty* color_property_connection_; - rviz::ColorProperty* color_property_velocity_linear_; - rviz::ColorProperty* color_property_velocity_angular_; - rviz::ColorProperty* color_property_acceleration_linear_; - rviz::ColorProperty* color_property_acceleration_angular_; - - rviz::FloatProperty* alpha_property_; - - rviz::FloatProperty* font_size_property_; - rviz::BoolProperty* show_text_property_; - - rviz::IntProperty* history_length_property_; + 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_; 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 index fd8e65d..7e49ebd 100644 --- 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 @@ -22,12 +22,15 @@ class MultiDOFJointTrajectoryPointConnectionVisual 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_; @@ -36,6 +39,7 @@ class MultiDOFJointTrajectoryPointConnectionVisual std::vector> lines_; + float show_connection_; Ogre::ColourValue color_; }; 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 index 8357f30..d6c7aa5 100644 --- 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 @@ -24,6 +24,11 @@ class 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, @@ -40,6 +45,12 @@ class MultiDOFJointTrajectoryPointVisual 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); @@ -76,6 +87,7 @@ class MultiDOFJointTrajectoryPointVisual void updateFontSize(); void updateShowText(); + Ogre::ColourValue getColor(const Ogre::ColourValue& color, bool visible); float getCharacterHeight(); Ogre::SceneManager* scene_manager_; @@ -93,6 +105,12 @@ class MultiDOFJointTrajectoryPointVisual 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_; diff --git a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp index 7ebdbc1..76df894 100644 --- a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp @@ -6,7 +6,13 @@ namespace multi_dof_joint_trajectory_rviz_plugins { MultiDOFJointTrajectoryDisplay::MultiDOFJointTrajectoryDisplay() -: size_transform_rotation_(0.2), +: 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), @@ -21,87 +27,117 @@ MultiDOFJointTrajectoryDisplay::MultiDOFJointTrajectoryDisplay() font_size_(0.05), show_text_(true) { - size_property_transform_rotation_ = new rviz::FloatProperty( + 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 transform.", + "Size of the axes of the rotation transforms.", this, SLOT(setSizeTransformRotation())); - size_property_transform_rotation_->setMin(0); + property_size_transform_rotation_->setMin(0); - diameter_property_arrows_ = new rviz::FloatProperty( + property_diameter_arrows_ = new rviz::FloatProperty( "Diameter Arrows", diameter_arrows_, "Diameter of the arrows.", this, SLOT(setDiameterArrows())); - diameter_property_arrows_->setMin(0); + property_diameter_arrows_->setMin(0); - scale_property_velocity_linear_ = new rviz::FloatProperty( + property_scale_velocity_linear_ = new rviz::FloatProperty( "Scale Velocity Linear", scale_velocity_linear_, - "Scale of the linear velocity.", + "Scale of the linear velocities.", this, SLOT(setScaleVelocityLinear())); - scale_property_velocity_angular_ = new rviz::FloatProperty( + property_scale_velocity_angular_ = new rviz::FloatProperty( "Scale Velocity Angular", scale_velocity_angular_, - "Scale of the angular velocity.", + "Scale of the angular velocities.", this, SLOT(setScaleVelocityAngular())); - scale_property_acceleration_linear_ = new rviz::FloatProperty( + property_scale_acceleration_linear_ = new rviz::FloatProperty( "Scale Acceleration Linear", scale_acceleration_linear_, - "Scale of the linear acceleration.", + "Scale of the linear accelerations.", this, SLOT(setScaleAccelerationLinear())); - scale_property_acceleration_angular_ = new rviz::FloatProperty( + property_scale_acceleration_angular_ = new rviz::FloatProperty( "Scale Acceleration Angular", scale_acceleration_angular_, - "Scale of the angular acceleration.", + "Scale of the angular accelerations.", this, SLOT(setScaleAccelerationAngular())); - color_property_connection_ = new rviz::ColorProperty( + property_color_connection_ = new rviz::ColorProperty( "Color Connection", rviz::ogreToQt(color_connection_), "Color of connection lines.", this, SLOT(setColorConnection())); - color_property_velocity_linear_ = new rviz::ColorProperty( + property_color_velocity_linear_ = new rviz::ColorProperty( "Color Velocity Linear", rviz::ogreToQt(color_velocity_linear_), - "Color of the linear velocity.", + "Color of the linear velocities.", this, SLOT(setColorVelocityLinear())); - color_property_velocity_angular_ = new rviz::ColorProperty( + property_color_velocity_angular_ = new rviz::ColorProperty( "Color Velocity Angular", rviz::ogreToQt(color_velocity_angular_), - "Color of the angular velocity.", + "Color of the angular velocities.", this, SLOT(setColorVelocityAngular())); - color_property_acceleration_linear_ = new rviz::ColorProperty( + property_color_acceleration_linear_ = new rviz::ColorProperty( "Color Acceleration Linear", rviz::ogreToQt(color_acceleration_linear_), - "Color of the linear acceleration.", + "Color of the linear accelerations.", this, SLOT(setColorAccelerationLinear())); - color_property_acceleration_angular_ = new rviz::ColorProperty( + property_color_acceleration_angular_ = new rviz::ColorProperty( "Color Acceleration Angular", rviz::ogreToQt(color_acceleration_angular_), - "Color of the angular acceleration.", + "Color of the angular accelerations.", this, SLOT(setColorAccelerationAngular())); - alpha_property_ = new rviz::FloatProperty( + property_alpha_ = new rviz::FloatProperty( "Alpha", alpha_, "0 is fully transparent, 1.0 is fully opaque.", this, SLOT(setAlpha())); - alpha_property_->setMin(0); - alpha_property_->setMax(1); + property_alpha_->setMin(0); + property_alpha_->setMax(1); - font_size_property_ = new rviz::FloatProperty( + property_font_size_ = new rviz::FloatProperty( "Font Size", font_size_, "Size of the font.", this, SLOT(setFontSize())); - font_size_property_->setMin(0); + property_font_size_->setMin(0); - show_text_property_ = new rviz::BoolProperty( + property_show_text_ = new rviz::BoolProperty( "Show Caption", show_text_, "Enable or disable text rendering.", this, SLOT(setShowText())); - history_length_property_ = new rviz::IntProperty( + property_history_length_ = new rviz::IntProperty( "History Length", 1, "Number of prior measurements to display.", this, SLOT(setHistoryLength())); - history_length_property_->setMin(1); - history_length_property_->setMax(100000); + property_history_length_->setMin(1); + property_history_length_->setMax(100000); } MultiDOFJointTrajectoryDisplay::~MultiDOFJointTrajectoryDisplay() @@ -118,49 +154,85 @@ void MultiDOFJointTrajectoryDisplay::onInitialize() void MultiDOFJointTrajectoryDisplay::reset() { MFDClass::reset(); - points_visuals_.clear(); - connections_visuals_.clear(); + 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_ = size_property_transform_rotation_->getFloat(); + size_transform_rotation_ = property_size_transform_rotation_->getFloat(); updateSizeTransformRotation(); } void MultiDOFJointTrajectoryDisplay::setDiameterArrows() { - diameter_arrows_ = diameter_property_arrows_->getFloat(); + diameter_arrows_ = property_diameter_arrows_->getFloat(); updateDiameterArrows(); } void MultiDOFJointTrajectoryDisplay::setScaleVelocityLinear() { - scale_velocity_linear_ = scale_property_velocity_linear_->getFloat(); + scale_velocity_linear_ = property_scale_velocity_linear_->getFloat(); updateScaleVelocityLinear(); } void MultiDOFJointTrajectoryDisplay::setScaleVelocityAngular() { - scale_velocity_angular_ = scale_property_velocity_angular_->getFloat(); + scale_velocity_angular_ = property_scale_velocity_angular_->getFloat(); updateScaleVelocityAngular(); } void MultiDOFJointTrajectoryDisplay::setScaleAccelerationLinear() { - scale_acceleration_linear_ = scale_property_acceleration_linear_->getFloat(); + scale_acceleration_linear_ = property_scale_acceleration_linear_->getFloat(); updateScaleAccelerationLinear(); } void MultiDOFJointTrajectoryDisplay::setScaleAccelerationAngular() { - scale_acceleration_angular_ = scale_property_acceleration_angular_->getFloat(); + scale_acceleration_angular_ = property_scale_acceleration_angular_->getFloat(); updateScaleAccelerationAngular(); } void MultiDOFJointTrajectoryDisplay::setAlpha() { - alpha_ = alpha_property_->getFloat(); + alpha_ = property_alpha_->getFloat(); color_connection_.a = alpha_; color_velocity_linear_.a = alpha_; color_velocity_angular_.a = alpha_; @@ -176,72 +248,79 @@ void MultiDOFJointTrajectoryDisplay::setAlpha() void MultiDOFJointTrajectoryDisplay::setColorConnection() { - color_connection_ = rviz::qtToOgre(color_property_connection_->getColor()); - color_connection_.a = alpha_property_->getFloat(); + color_connection_ = rviz::qtToOgre(property_color_connection_->getColor()); + color_connection_.a = property_alpha_->getFloat(); updateColorConnection(); } void MultiDOFJointTrajectoryDisplay::setColorVelocityLinear() { - color_velocity_linear_ = rviz::qtToOgre(color_property_velocity_linear_->getColor()); - color_velocity_linear_.a = alpha_property_->getFloat(); + 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(color_property_velocity_angular_->getColor()); - color_velocity_angular_.a = alpha_property_->getFloat(); + 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(color_property_acceleration_linear_->getColor()); - color_acceleration_linear_.a = alpha_property_->getFloat(); + 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(color_property_acceleration_angular_->getColor()); - color_acceleration_angular_.a = alpha_property_->getFloat(); + color_acceleration_angular_ = rviz::qtToOgre(property_color_acceleration_angular_->getColor()); + color_acceleration_angular_.a = property_alpha_->getFloat(); updateColorAccelerationAngular(); } void MultiDOFJointTrajectoryDisplay::setFontSize() { - font_size_ = font_size_property_->getFloat(); + font_size_ = property_font_size_->getFloat(); updateFontSize(); } void MultiDOFJointTrajectoryDisplay::setShowText() { - show_text_ = show_text_property_->getBool(); + show_text_ = property_show_text_->getBool(); updateShowText(); } void MultiDOFJointTrajectoryDisplay::setHistoryLength() { - points_visuals_.rset_capacity(history_length_property_->getInt()); - connections_visuals_.rset_capacity(history_length_property_->getInt()); + 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) { - size_transform_rotation_ = size_property_transform_rotation_->getFloat(); - scale_velocity_linear_ = scale_property_velocity_linear_->getFloat(); - scale_velocity_angular_ = scale_property_velocity_angular_->getFloat(); - scale_acceleration_linear_ = scale_property_acceleration_linear_->getFloat(); - scale_acceleration_angular_ = scale_property_acceleration_angular_->getFloat(); - - color_connection_ = rviz::qtToOgre(color_property_connection_->getColor()); - color_velocity_linear_ = rviz::qtToOgre(color_property_velocity_linear_->getColor()); - color_velocity_angular_ = rviz::qtToOgre(color_property_velocity_angular_->getColor()); - color_acceleration_linear_ = rviz::qtToOgre(color_property_acceleration_linear_->getColor()); - color_acceleration_angular_ = rviz::qtToOgre(color_property_acceleration_angular_->getColor()); - alpha_ = alpha_property_->getFloat(); + 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_; @@ -261,20 +340,25 @@ void MultiDOFJointTrajectoryDisplay::processMessage(const trajectory_msgs::Multi captions.push_back(caption_point); } - font_size_ = font_size_property_->getFloat(); - show_text_ = show_text_property_->getBool(); + font_size_ = property_font_size_->getFloat(); + show_text_ = property_show_text_->getBool(); - std::vector> points_visuals; - std::vector> connections_visuals; + std::vector> visuals_points; + std::vector> visuals_connections; trajectory_msgs::MultiDOFJointTrajectoryPoint last_point; trajectory_msgs::MultiDOFJointTrajectoryPoint current_point = msg->points[0]; // add first point - points_visuals.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointVisual( + 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_, @@ -298,17 +382,23 @@ void MultiDOFJointTrajectoryDisplay::processMessage(const trajectory_msgs::Multi current_point = msg->points[i]; // add edge to predecessor - connections_visuals.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointConnectionVisual(context_->getSceneManager(), + visuals_connections.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointConnectionVisual(context_->getSceneManager(), scene_node_, last_point, current_point, + show_connection_, color_connection_))); // add pose - points_visuals.push_back(boost::shared_ptr(new MultiDOFJointTrajectoryPointVisual( + 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_, @@ -325,160 +415,226 @@ void MultiDOFJointTrajectoryDisplay::processMessage(const trajectory_msgs::Multi show_text_))); } - points_visuals_.push_back(points_visuals); - connections_visuals_.push_back(connections_visuals); + 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 < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setSizeTransformRotation(size_transform_rotation_); + visuals_points_[i][j]->setSizeTransformRotation(size_transform_rotation_); } } } void MultiDOFJointTrajectoryDisplay::updateDiameterArrows() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setDiametersArrows(diameter_arrows_); + visuals_points_[i][j]->setDiametersArrows(diameter_arrows_); } } } void MultiDOFJointTrajectoryDisplay::updateScaleVelocityLinear() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setScaleVelocityLinear(scale_velocity_linear_); + visuals_points_[i][j]->setScaleVelocityLinear(scale_velocity_linear_); } } } void MultiDOFJointTrajectoryDisplay::updateScaleVelocityAngular() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setScaleVelocityAngular(scale_velocity_angular_); + visuals_points_[i][j]->setScaleVelocityAngular(scale_velocity_angular_); } } } void MultiDOFJointTrajectoryDisplay::updateScaleAccelerationLinear() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setScaleAccelerationLinear(scale_acceleration_linear_); + visuals_points_[i][j]->setScaleAccelerationLinear(scale_acceleration_linear_); } } } void MultiDOFJointTrajectoryDisplay::updateScaleAccelerationAngular() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setScaleAccelerationAngular(scale_acceleration_angular_); + visuals_points_[i][j]->setScaleAccelerationAngular(scale_acceleration_angular_); } } } void MultiDOFJointTrajectoryDisplay::updateColorConnection() { - for(size_t i = 0; i < connections_visuals_.size(); i++) + for(size_t i = 0; i < visuals_connections_.size(); i++) { - for (unsigned int j = 0; j < connections_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_connections_[i].size(); j++) { - connections_visuals_[i][j]->setColor(color_connection_); + visuals_connections_[i][j]->setColor(color_connection_); } } } void MultiDOFJointTrajectoryDisplay::updateAlphaTransformRotation() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setAlphaTransformRotation(alpha_); + visuals_points_[i][j]->setAlphaTransformRotation(alpha_); } } } void MultiDOFJointTrajectoryDisplay::updateColorVelocityLinear() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setColorVelocityLinear(color_velocity_linear_); + visuals_points_[i][j]->setColorVelocityLinear(color_velocity_linear_); } } } void MultiDOFJointTrajectoryDisplay::updateColorVelocityAngular() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setColorVelocityAngular(color_velocity_angular_); + visuals_points_[i][j]->setColorVelocityAngular(color_velocity_angular_); } } } void MultiDOFJointTrajectoryDisplay::updateColorAccelerationLinear() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setColorAccelerationLinear(color_acceleration_linear_); + visuals_points_[i][j]->setColorAccelerationLinear(color_acceleration_linear_); } } } void MultiDOFJointTrajectoryDisplay::updateColorAccelerationAngular() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setColorAccelerationAngular(color_acceleration_angular_); + visuals_points_[i][j]->setColorAccelerationAngular(color_acceleration_angular_); } } } void MultiDOFJointTrajectoryDisplay::updateFontSize() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setFontSize(font_size_); + visuals_points_[i][j]->setFontSize(font_size_); } } } void MultiDOFJointTrajectoryDisplay::updateShowText() { - for(size_t i = 0; i < points_visuals_.size(); i++) + for(size_t i = 0; i < visuals_points_.size(); i++) { - for (unsigned int j = 0; j < points_visuals_[i].size(); j++) + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { - points_visuals_[i][j]->setShowText(show_text_); + visuals_points_[i][j]->setShowText(show_text_); } } } diff --git a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp index a5ea5f4..e85cde4 100644 --- a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp @@ -3,12 +3,15 @@ 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, - const Ogre::ColourValue& color) +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 @@ -25,6 +28,7 @@ MultiDOFJointTrajectoryPointConnectionVisual::MultiDOFJointTrajectoryPointConnec 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_); } } @@ -33,12 +37,26 @@ MultiDOFJointTrajectoryPointConnectionVisual::~MultiDOFJointTrajectoryPointConne 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++) diff --git a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp index 8380410..f50fe09 100644 --- a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp @@ -9,6 +9,11 @@ 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, @@ -25,13 +30,18 @@ MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( 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), - alpha_transform_rotatation_(alpha_transform_rotatation), 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), @@ -59,16 +69,6 @@ MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( 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); - Ogre::ColourValue color; - color = transforms_rotation_[i]->getDefaultXColor(); - color.a = alpha_transform_rotatation_; - transforms_rotation_.back()->setXColor(color); - color = transforms_rotation_[i]->getDefaultYColor(); - color.a = alpha_transform_rotatation_; - transforms_rotation_.back()->setYColor(color); - color = transforms_rotation_[i]->getDefaultZColor(); - color.a = alpha_transform_rotatation_; - transforms_rotation_.back()->setZColor(color); // define common variables Ogre::Vector3 vector; @@ -86,7 +86,6 @@ MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( 0.2*scale_velocity_linear_*velocities_linear_absolute_.back(), 2.0*diameter_arrows_))); velocities_linear_.back()->setDirection(vector); - velocities_linear_.back()->setColor(color_velocity_linear_); // velocity angular vector = Ogre::Vector3(msg.velocities[i].angular.x, msg.velocities[i].angular.y, msg.velocities[i].angular.z); @@ -99,7 +98,6 @@ MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( 0.2*scale_velocity_angular_*velocities_angular_absolute_.back(), 2.0*diameter_arrows_))); velocities_angular_.back()->setDirection(vector); - velocities_angular_.back()->setColor(color_velocity_angular_); } if (message_contains_accelerations) @@ -115,7 +113,6 @@ MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( 0.2*scale_acceleration_linear_*accelerations_linear_absolute_.back(), 2.0*diameter_arrows_))); accelerations_linear_.back()->setDirection(vector); - accelerations_linear_.back()->setColor(color_acceleration_linear_); // acceleration angular vector = Ogre::Vector3(msg.accelerations[i].angular.x, msg.accelerations[i].angular.y, msg.accelerations[i].angular.z); @@ -128,7 +125,6 @@ MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( 0.2*scale_acceleration_angular_*accelerations_angular_absolute_.back(), 2.0*diameter_arrows_))); accelerations_angular_.back()->setDirection(vector); - accelerations_angular_.back()->setColor(color_acceleration_angular_); } // text @@ -136,6 +132,13 @@ MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( 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() @@ -146,6 +149,36 @@ MultiDOFJointTrajectoryPointVisual::~MultiDOFJointTrajectoryPointVisual() } } +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; @@ -304,13 +337,13 @@ void MultiDOFJointTrajectoryPointVisual::updateAlphaTransformRotation() for (unsigned int i = 0; i < transforms_rotation_.size(); i++) { color = transforms_rotation_[i]->getDefaultXColor(); - color.a = alpha_transform_rotatation_; + color.a = show_transform_rotation_*alpha_transform_rotatation_; transforms_rotation_[i]->setXColor(color); color = transforms_rotation_[i]->getDefaultYColor(); - color.a = alpha_transform_rotatation_; + color.a = show_transform_rotation_*alpha_transform_rotatation_; transforms_rotation_[i]->setYColor(color); color = transforms_rotation_[i]->getDefaultZColor(); - color.a = alpha_transform_rotatation_; + color.a = show_transform_rotation_*alpha_transform_rotatation_; transforms_rotation_[i]->setZColor(color); } } @@ -319,7 +352,7 @@ void MultiDOFJointTrajectoryPointVisual::updateColorVelocityLinear() { for (unsigned int i = 0; i < velocities_linear_.size(); i++) { - velocities_linear_[i]->setColor(color_velocity_linear_); + velocities_linear_[i]->setColor(getColor(color_velocity_linear_, show_velocity_linear_)); } } @@ -327,7 +360,7 @@ void MultiDOFJointTrajectoryPointVisual::updateColorVelocityAngular() { for (unsigned int i = 0; i < velocities_angular_.size(); i++) { - velocities_angular_[i]->setColor(color_velocity_angular_); + velocities_angular_[i]->setColor(getColor(color_velocity_angular_, show_velocity_angular_)); } } @@ -335,7 +368,7 @@ void MultiDOFJointTrajectoryPointVisual::updateColorAccelerationLinear() { for (unsigned int i = 0; i < accelerations_linear_.size(); i++) { - accelerations_linear_[i]->setColor(color_acceleration_linear_); + accelerations_linear_[i]->setColor(getColor(color_acceleration_linear_, show_acceleration_linear_)); } } @@ -343,7 +376,7 @@ void MultiDOFJointTrajectoryPointVisual::updateColorAccelerationAngular() { for (unsigned int i = 0; i < accelerations_angular_.size(); i++) { - accelerations_angular_[i]->setColor(color_acceleration_angular_); + accelerations_angular_[i]->setColor(getColor(color_acceleration_angular_, show_acceleration_angular_)); } } @@ -373,6 +406,20 @@ void MultiDOFJointTrajectoryPointVisual::updateShowText() } } +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_)