diff --git a/rviz/src/rviz/default_plugin/tf_display.cpp b/rviz/src/rviz/default_plugin/tf_display.cpp deleted file mode 100644 index bdfb26d04..000000000 --- a/rviz/src/rviz/default_plugin/tf_display.cpp +++ /dev/null @@ -1,793 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include -#include - -#include - -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/ogre_helpers/arrow.h" -#include "rviz/ogre_helpers/axes.h" -#include "rviz/ogre_helpers/movable_text.h" -#include "rviz/properties/bool_property.h" -#include "rviz/properties/float_property.h" -#include "rviz/properties/quaternion_property.h" -#include "rviz/properties/string_property.h" -#include "rviz/properties/vector_property.h" -#include "rviz/selection/forwards.h" -#include "rviz/selection/selection_manager.h" - -#include "rviz/default_plugin/tf_display.h" - -namespace rviz -{ - -class FrameSelectionHandler: public SelectionHandler -{ -public: - FrameSelectionHandler( FrameInfo* frame, TFDisplay* display, DisplayContext* context ); - virtual ~FrameSelectionHandler() {} - - virtual void createProperties( const Picked& obj, Property* parent_property ); - virtual void destroyProperties( const Picked& obj, Property* parent_property ); - - bool getEnabled(); - void setEnabled( bool enabled ); - void setParentName( std::string parent_name ); - void setPosition( const Ogre::Vector3& position ); - void setOrientation( const Ogre::Quaternion& orientation ); - -private: - FrameInfo* frame_; - TFDisplay* display_; - Property* category_property_; - BoolProperty* enabled_property_; - StringProperty* parent_property_; - VectorProperty* position_property_; - QuaternionProperty* orientation_property_; -}; - -FrameSelectionHandler::FrameSelectionHandler(FrameInfo* frame, TFDisplay* display, DisplayContext* context ) - : SelectionHandler( context ) - , frame_( frame ) - , display_( display ) - , category_property_( NULL ) - , enabled_property_( NULL ) - , parent_property_( NULL ) - , position_property_( NULL ) - , orientation_property_( NULL ) -{ -} - -void FrameSelectionHandler::createProperties( const Picked& obj, Property* parent_property ) -{ - category_property_ = new Property( "Frame " + QString::fromStdString( frame_->name_ ), QVariant(), "", parent_property ); - - enabled_property_ = new BoolProperty( "Enabled", true, "", category_property_, SLOT( updateVisibilityFromSelection() ), frame_ ); - - parent_property_ = new StringProperty( "Parent", "", "", category_property_ ); - parent_property_->setReadOnly( true ); - - position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, "", category_property_ ); - position_property_->setReadOnly( true ); - - orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, "", category_property_ ); - orientation_property_->setReadOnly( true ); -} - -void FrameSelectionHandler::destroyProperties( const Picked& obj, Property* parent_property ) -{ - delete category_property_; // This deletes its children as well. - category_property_ = NULL; - enabled_property_ = NULL; - parent_property_ = NULL; - position_property_ = NULL; - orientation_property_ = NULL; -} - -bool FrameSelectionHandler::getEnabled() -{ - if( enabled_property_ ) - { - return enabled_property_->getBool(); - } - return false; // should never happen, but don't want to crash if it does. -} - -void FrameSelectionHandler::setEnabled( bool enabled ) -{ - if( enabled_property_ ) - { - enabled_property_->setBool( enabled ); - } -} - -void FrameSelectionHandler::setParentName( std::string parent_name ) -{ - if( parent_property_ ) - { - parent_property_->setStdString( parent_name ); - } -} - -void FrameSelectionHandler::setPosition( const Ogre::Vector3& position ) -{ - if( position_property_ ) - { - position_property_->setVector( position ); - } -} - -void FrameSelectionHandler::setOrientation( const Ogre::Quaternion& orientation ) -{ - if( orientation_property_ ) - { - orientation_property_->setQuaternion( orientation ); - } -} - -typedef std::set S_FrameInfo; - -TFDisplay::TFDisplay() - : Display() - , update_timer_( 0.0f ) - , changing_single_frame_enabled_state_( false ) -{ - show_names_property_ = new BoolProperty( "Show Names", true, "Whether or not names should be shown next to the frames.", - this, SLOT( updateShowNames() )); - - show_axes_property_ = new BoolProperty( "Show Axes", true, "Whether or not the axes of each frame should be shown.", - this, SLOT( updateShowAxes() )); - - show_arrows_property_ = new BoolProperty( "Show Arrows", true, "Whether or not arrows from child to parent should be shown.", - this, SLOT( updateShowArrows() )); - - scale_property_ = new FloatProperty( "Marker Scale", 1, "Scaling factor for all names, axes and arrows.", this ); - - update_rate_property_ = new FloatProperty( "Update Interval", 0, - "The interval, in seconds, at which to update the frame transforms. 0 means to do so every update cycle.", - this ); - update_rate_property_->setMin( 0 ); - - frame_timeout_property_ = new FloatProperty( "Frame Timeout", 15, - "The length of time, in seconds, before a frame that has not been updated is considered \"dead\"." - " For 1/3 of this time the frame will appear correct, for the second 1/3rd it will fade to gray," - " and then it will fade out completely.", - this ); - frame_timeout_property_->setMin( 1 ); - - frames_category_ = new Property( "Frames", QVariant(), "The list of all frames.", this ); - - all_enabled_property_ = new BoolProperty( "All Enabled", true, - "Whether all the frames should be enabled or not.", - frames_category_, SLOT( allEnabledChanged() ), this ); - - tree_category_ = new Property( "Tree", QVariant(), "A tree-view of the frames, showing the parent/child relationships.", this ); -} - -TFDisplay::~TFDisplay() -{ - if ( initialized() ) - { - root_node_->removeAndDestroyAllChildren(); - scene_manager_->destroySceneNode( root_node_->getName() ); - } -} - -void TFDisplay::onInitialize() -{ - frame_config_enabled_state_.clear(); - - root_node_ = scene_node_->createChildSceneNode(); - - names_node_ = root_node_->createChildSceneNode(); - arrows_node_ = root_node_->createChildSceneNode(); - axes_node_ = root_node_->createChildSceneNode(); -} - -void TFDisplay::load(const Config& config) -{ - Display::load(config); - - // Load the enabled state for all frames specified in the config, and store - // the values in a map so that the enabled state can be properly set once - // the frame is created - Config c = config.mapGetChild("Frames"); - for( Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance() ) - { - QString key = iter.currentKey(); - if( key != "All Enabled" ) - { - const Config& child = iter.currentChild(); - bool enabled = child.mapGetChild("Value").getValue().toBool(); - - frame_config_enabled_state_[key.toStdString()] = enabled; - } - } -} - -void TFDisplay::clear() -{ - // Clear the tree. - tree_category_->removeChildren(); - - // Clear the frames category, except for the "All enabled" property, which is first. - frames_category_->removeChildren( 1 ); - - S_FrameInfo to_delete; - M_FrameInfo::iterator frame_it = frames_.begin(); - M_FrameInfo::iterator frame_end = frames_.end(); - for ( ; frame_it != frame_end; ++frame_it ) - { - to_delete.insert( frame_it->second ); - } - - S_FrameInfo::iterator delete_it = to_delete.begin(); - S_FrameInfo::iterator delete_end = to_delete.end(); - for ( ; delete_it != delete_end; ++delete_it ) - { - deleteFrame( *delete_it, false ); - } - - frames_.clear(); - - update_timer_ = 0.0f; - - clearStatuses(); -} - -void TFDisplay::onEnable() -{ - root_node_->setVisible( true ); - - names_node_->setVisible( show_names_property_->getBool() ); - arrows_node_->setVisible( show_arrows_property_->getBool() ); - axes_node_->setVisible( show_axes_property_->getBool() ); -} - -void TFDisplay::onDisable() -{ - root_node_->setVisible( false ); - clear(); -} - -void TFDisplay::updateShowNames() -{ - names_node_->setVisible( show_names_property_->getBool() ); - - M_FrameInfo::iterator it = frames_.begin(); - M_FrameInfo::iterator end = frames_.end(); - for (; it != end; ++it) - { - FrameInfo* frame = it->second; - - frame->updateVisibilityFromFrame(); - } -} - -void TFDisplay::updateShowAxes() -{ - axes_node_->setVisible( show_axes_property_->getBool() ); - - M_FrameInfo::iterator it = frames_.begin(); - M_FrameInfo::iterator end = frames_.end(); - for (; it != end; ++it) - { - FrameInfo* frame = it->second; - - frame->updateVisibilityFromFrame(); - } -} - -void TFDisplay::updateShowArrows() -{ - arrows_node_->setVisible( show_arrows_property_->getBool() ); - - M_FrameInfo::iterator it = frames_.begin(); - M_FrameInfo::iterator end = frames_.end(); - for (; it != end; ++it) - { - FrameInfo* frame = it->second; - - frame->updateVisibilityFromFrame(); - } -} - -void TFDisplay::allEnabledChanged() -{ - if( changing_single_frame_enabled_state_ ) - { - return; - } - bool enabled = all_enabled_property_->getBool(); - - M_FrameInfo::iterator it = frames_.begin(); - M_FrameInfo::iterator end = frames_.end(); - for (; it != end; ++it) - { - FrameInfo* frame = it->second; - - frame->enabled_property_->setBool( enabled ); - } -} - -void TFDisplay::update(float wall_dt, float ros_dt) -{ - update_timer_ += wall_dt; - float update_rate = update_rate_property_->getFloat(); - if( update_rate < 0.0001f || update_timer_ > update_rate ) - { - updateFrames(); - - update_timer_ = 0.0f; - } -} - -FrameInfo* TFDisplay::getFrameInfo( const std::string& frame ) -{ - M_FrameInfo::iterator it = frames_.find( frame ); - if ( it == frames_.end() ) - { - return NULL; - } - - return it->second; -} - -void TFDisplay::updateFrames() -{ - typedef std::vector V_string; - V_string frames; - context_->getTFClient()->getFrameStrings( frames ); - std::sort(frames.begin(), frames.end()); - - S_FrameInfo current_frames; - - { - V_string::iterator it = frames.begin(); - V_string::iterator end = frames.end(); - for ( ; it != end; ++it ) - { - const std::string& frame = *it; - - if ( frame.empty() ) - { - continue; - } - - FrameInfo* info = getFrameInfo( frame ); - if (!info) - { - info = createFrame(frame); - } - else - { - updateFrame(info); - } - - current_frames.insert( info ); - } - } - - { - S_FrameInfo to_delete; - M_FrameInfo::iterator frame_it = frames_.begin(); - M_FrameInfo::iterator frame_end = frames_.end(); - for ( ; frame_it != frame_end; ++frame_it ) - { - if ( current_frames.find( frame_it->second ) == current_frames.end() ) - { - to_delete.insert( frame_it->second ); - } - } - - S_FrameInfo::iterator delete_it = to_delete.begin(); - S_FrameInfo::iterator delete_end = to_delete.end(); - for ( ; delete_it != delete_end; ++delete_it ) - { - deleteFrame( *delete_it, true ); - } - } - - context_->queueRender(); -} - -static const Ogre::ColourValue ARROW_HEAD_COLOR(1.0f, 0.1f, 0.6f, 1.0f); -static const Ogre::ColourValue ARROW_SHAFT_COLOR(0.8f, 0.8f, 0.3f, 1.0f); - -FrameInfo* TFDisplay::createFrame(const std::string& frame) -{ - FrameInfo* info = new FrameInfo( this ); - frames_.insert( std::make_pair( frame, info ) ); - - info->name_ = frame; - info->last_update_ = ros::Time::now(); - info->axes_ = new Axes( scene_manager_, axes_node_, 0.2, 0.02 ); - info->axes_->getSceneNode()->setVisible( show_axes_property_->getBool() ); - info->selection_handler_.reset( new FrameSelectionHandler( info, this, context_ )); - info->selection_handler_->addTrackedObjects( info->axes_->getSceneNode() ); - - info->name_text_ = new MovableText( frame, "Liberation Sans", 0.1 ); - info->name_text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_BELOW); - info->name_node_ = names_node_->createChildSceneNode(); - info->name_node_->attachObject( info->name_text_ ); - info->name_node_->setVisible( show_names_property_->getBool() ); - - info->parent_arrow_ = new Arrow( scene_manager_, arrows_node_, 1.0f, 0.01, 1.0f, 0.08 ); - info->parent_arrow_->getSceneNode()->setVisible( false ); - info->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR); - info->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR); - - info->enabled_property_ = new BoolProperty( QString::fromStdString( info->name_ ), true, "Enable or disable this individual frame.", - frames_category_, SLOT( updateVisibilityFromFrame() ), info ); - - info->parent_property_ = new StringProperty( "Parent", "", "Parent of this frame. (Not editable)", - info->enabled_property_ ); - info->parent_property_->setReadOnly( true ); - - info->position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, - "Position of this frame, in the current Fixed Frame. (Not editable)", - info->enabled_property_ ); - info->position_property_->setReadOnly( true ); - - info->orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, - "Orientation of this frame, in the current Fixed Frame. (Not editable)", - info->enabled_property_ ); - info->orientation_property_->setReadOnly( true ); - - info->rel_position_property_ = new VectorProperty( "Relative Position", Ogre::Vector3::ZERO, - "Position of this frame, relative to it's parent frame. (Not editable)", - info->enabled_property_ ); - info->rel_position_property_->setReadOnly( true ); - - info->rel_orientation_property_ = new QuaternionProperty( "Relative Orientation", Ogre::Quaternion::IDENTITY, - "Orientation of this frame, relative to it's parent frame. (Not editable)", - info->enabled_property_ ); - info->rel_orientation_property_->setReadOnly( true ); - - // If the current frame was specified as disabled in the config file - // then its enabled state must be updated accordingly - if( frame_config_enabled_state_.count(frame) > 0 && !frame_config_enabled_state_[frame] ) - { - info->enabled_property_->setBool(false); - } - - updateFrame( info ); - - return info; -} - -Ogre::ColourValue lerpColor(const Ogre::ColourValue& start, const Ogre::ColourValue& end, float t) -{ - return start * t + end * (1 - t); -} - -void TFDisplay::updateFrame( FrameInfo* frame ) -{ - tf::TransformListener* tf = context_->getTFClient(); - - // Check last received time so we can grey out/fade out frames that have stopped being published - ros::Time latest_time; - tf->getLatestCommonTime( fixed_frame_.toStdString(), frame->name_, latest_time, 0 ); - - if(( latest_time != frame->last_time_to_fixed_ ) || - ( latest_time == ros::Time() )) - { - frame->last_update_ = ros::Time::now(); - frame->last_time_to_fixed_ = latest_time; - } - - // Fade from color -> grey, then grey -> fully transparent - ros::Duration age = ros::Time::now() - frame->last_update_; - float frame_timeout = frame_timeout_property_->getFloat(); - float one_third_timeout = frame_timeout * 0.3333333f; - if( age > ros::Duration( frame_timeout )) - { - frame->parent_arrow_->getSceneNode()->setVisible(false); - frame->axes_->getSceneNode()->setVisible(false); - frame->name_node_->setVisible(false); - return; - } - else if (age > ros::Duration(one_third_timeout)) - { - Ogre::ColourValue grey(0.7, 0.7, 0.7, 1.0); - - if (age > ros::Duration(one_third_timeout * 2)) - { - float a = std::max(0.0, (frame_timeout - age.toSec())/one_third_timeout); - Ogre::ColourValue c = Ogre::ColourValue(grey.r, grey.g, grey.b, a); - - frame->axes_->setXColor(c); - frame->axes_->setYColor(c); - frame->axes_->setZColor(c); - frame->name_text_->setColor(c); - frame->parent_arrow_->setColor(c.r, c.g, c.b, c.a); - } - else - { - float t = std::max(0.0, (one_third_timeout * 2 - age.toSec())/one_third_timeout); - frame->axes_->setXColor(lerpColor(frame->axes_->getDefaultXColor(), grey, t)); - frame->axes_->setYColor(lerpColor(frame->axes_->getDefaultYColor(), grey, t)); - frame->axes_->setZColor(lerpColor(frame->axes_->getDefaultZColor(), grey, t)); - frame->name_text_->setColor(lerpColor(Ogre::ColourValue::White, grey, t)); - frame->parent_arrow_->setShaftColor(lerpColor(ARROW_SHAFT_COLOR, grey, t)); - frame->parent_arrow_->setHeadColor(lerpColor(ARROW_HEAD_COLOR, grey, t)); - } - } - else - { - frame->axes_->setToDefaultColors(); - frame->name_text_->setColor(Ogre::ColourValue::White); - frame->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR); - frame->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR); - } - - setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK"); - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if( !context_->getFrameManager()->getTransform( frame->name_, ros::Time(), position, orientation )) - { - std::stringstream ss; - ss << "No transform from [" << frame->name_ << "] to frame [" << fixed_frame_.toStdString() << "]"; - setStatusStd(StatusProperty::Warn, frame->name_, ss.str()); - ROS_DEBUG( "Error transforming frame '%s' to frame '%s'", frame->name_.c_str(), qPrintable( fixed_frame_ )); - frame->name_node_->setVisible( false ); - frame->axes_->getSceneNode()->setVisible( false ); - frame->parent_arrow_->getSceneNode()->setVisible( false ); - return; - } - - frame->selection_handler_->setPosition( position ); - frame->selection_handler_->setOrientation( orientation ); - - bool frame_enabled = frame->enabled_property_->getBool(); - - frame->axes_->setPosition( position ); - frame->axes_->setOrientation( orientation ); - frame->axes_->getSceneNode()->setVisible( show_axes_property_->getBool() && frame_enabled); - float scale = scale_property_->getFloat(); - frame->axes_->setScale( Ogre::Vector3( scale, scale, scale )); - - frame->name_node_->setPosition( position ); - frame->name_node_->setVisible( show_names_property_->getBool() && frame_enabled ); - frame->name_node_->setScale( scale, scale, scale ); - - frame->position_property_->setVector( position ); - frame->orientation_property_->setQuaternion( orientation ); - - std::string old_parent = frame->parent_; - frame->parent_.clear(); - bool has_parent = tf->getParent( frame->name_, ros::Time(), frame->parent_ ); - if( has_parent ) - { - // If this frame has no tree property or the parent has changed, - if( !frame->tree_property_ || old_parent != frame->parent_ ) - { - // Look up the new parent. - M_FrameInfo::iterator parent_it = frames_.find( frame->parent_ ); - if( parent_it != frames_.end() ) - { - FrameInfo* parent = parent_it->second; - - // If the parent has a tree property, make a new tree property for this frame. - if( parent->tree_property_ ) - { - if(!frame->tree_property_) { - frame->tree_property_ = new Property( QString::fromStdString( frame->name_ ), QVariant(), "", parent->tree_property_ ); - } else { - frame->tree_property_->setParent(parent->tree_property_); - frame->tree_property_->setName(QString::fromStdString( frame->name_ )); - frame->tree_property_->setValue(QVariant()); - frame->tree_property_->setDescription(""); - } - } - } - } - - tf::StampedTransform transform; - try { - context_->getFrameManager()->getTFClientPtr()->lookupTransform(frame->parent_,frame->name_,ros::Time(0),transform); - } - catch(tf::TransformException& e) - { - ROS_DEBUG( "Error transforming frame '%s' (parent of '%s') to frame '%s'", - frame->parent_.c_str(), frame->name_.c_str(), qPrintable( fixed_frame_ )); - } - - // get the position/orientation relative to the parent frame - Ogre::Vector3 relative_position( transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z() ); - Ogre::Quaternion relative_orientation( transform.getRotation().w(), transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z() ); - frame->rel_position_property_->setVector( relative_position ); - frame->rel_orientation_property_->setQuaternion( relative_orientation ); - - if( show_arrows_property_->getBool() ) - { - Ogre::Vector3 parent_position; - Ogre::Quaternion parent_orientation; - if (!context_->getFrameManager()->getTransform(frame->parent_, ros::Time(), parent_position, parent_orientation)) - { - ROS_DEBUG( "Error transforming frame '%s' (parent of '%s') to frame '%s'", - frame->parent_.c_str(), frame->name_.c_str(), qPrintable( fixed_frame_ )); - } - - Ogre::Vector3 direction = parent_position - position; - float distance = direction.length(); - direction.normalise(); - - Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo( direction ); - - frame->distance_to_parent_ = distance; - float head_length = ( distance < 0.1*scale ) ? (0.1*scale*distance) : 0.1*scale; - float shaft_length = distance - head_length; - // aleeper: This was changed from 0.02 and 0.08 to 0.01 and 0.04 to match proper radius handling in arrow.cpp - frame->parent_arrow_->set( shaft_length, 0.01*scale, head_length, 0.04*scale ); - - if ( distance > 0.001f ) - { - frame->parent_arrow_->getSceneNode()->setVisible( show_arrows_property_->getBool() && frame_enabled ); - } - else - { - frame->parent_arrow_->getSceneNode()->setVisible( false ); - } - - frame->parent_arrow_->setPosition( position ); - frame->parent_arrow_->setOrientation( orient ); - } - else - { - frame->parent_arrow_->getSceneNode()->setVisible( false ); - } - } - else - { - if ( !frame->tree_property_ || old_parent != frame->parent_ ) - { - if(!frame->tree_property_) { - frame->tree_property_ = new Property( QString::fromStdString( frame->name_ ), QVariant(), "", tree_category_ ); - } else { - frame->tree_property_->setName(QString::fromStdString( frame->name_ )); - frame->tree_property_->setValue(QVariant()); - frame->tree_property_->setDescription(""); - frame->tree_property_->setParent(tree_category_); - } - } - - frame->parent_arrow_->getSceneNode()->setVisible( false ); - } - - frame->parent_property_->setStdString( frame->parent_ ); - frame->selection_handler_->setParentName( frame->parent_ ); -} - -void TFDisplay::deleteFrame( FrameInfo* frame, bool delete_properties ) -{ - M_FrameInfo::iterator it = frames_.find( frame->name_ ); - ROS_ASSERT( it != frames_.end() ); - - frames_.erase( it ); - - delete frame->axes_; - context_->getSelectionManager()->removeObject( frame->axes_coll_ ); - delete frame->parent_arrow_; - delete frame->name_text_; - scene_manager_->destroySceneNode( frame->name_node_->getName() ); - if( delete_properties ) - { - delete frame->enabled_property_; - delete frame->tree_property_; - } - delete frame; -} - -void TFDisplay::fixedFrameChanged() -{ - update_timer_ = update_rate_property_->getFloat(); -} - -void TFDisplay::reset() -{ - Display::reset(); - clear(); -} - -///////////////////////////////////////////////////////////////////////////////////////////////////////// -// FrameInfo - -FrameInfo::FrameInfo( TFDisplay* display ) - : display_( display ) - , axes_( NULL ) - , axes_coll_( 0 ) - , parent_arrow_( NULL ) - , name_text_( NULL ) - , distance_to_parent_( 0.0f ) - , arrow_orientation_(Ogre::Quaternion::IDENTITY) - , tree_property_( NULL ) -{} - -void FrameInfo::updateVisibilityFromFrame() -{ - bool enabled = enabled_property_->getBool(); - selection_handler_->setEnabled( enabled ); - setEnabled( enabled ); -} - -void FrameInfo::updateVisibilityFromSelection() -{ - bool enabled = selection_handler_->getEnabled(); - enabled_property_->setBool( enabled ); - setEnabled( enabled ); -} - -void FrameInfo::setEnabled( bool enabled ) -{ - if( name_node_ ) - { - name_node_->setVisible( display_->show_names_property_->getBool() && enabled ); - } - - if( axes_ ) - { - axes_->getSceneNode()->setVisible( display_->show_axes_property_->getBool() && enabled ); - } - - if( parent_arrow_ ) - { - if( distance_to_parent_ > 0.001f ) - { - parent_arrow_->getSceneNode()->setVisible( display_->show_arrows_property_->getBool() && enabled ); - } - else - { - parent_arrow_->getSceneNode()->setVisible( false ); - } - } - - if( display_->all_enabled_property_->getBool() && !enabled) - { - display_->changing_single_frame_enabled_state_ = true; - display_->all_enabled_property_->setBool( false ); - display_->changing_single_frame_enabled_state_ = false; - } - - // Update the configuration that stores the enabled state of all frames - display_->frame_config_enabled_state_[this->name_] = enabled; - - display_->context_->queueRender(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::TFDisplay, rviz::Display ) diff --git a/rviz/src/rviz/default_plugin/tf_display.h b/rviz/src/rviz/default_plugin/tf_display.h deleted file mode 100644 index d6265fe09..000000000 --- a/rviz/src/rviz/default_plugin/tf_display.h +++ /dev/null @@ -1,175 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef RVIZ_TF_DISPLAY_H -#define RVIZ_TF_DISPLAY_H - -#include -#include - -#include -#include - -#include "rviz/selection/forwards.h" - -#include "rviz/display.h" - -namespace Ogre -{ -class SceneNode; -} - -namespace rviz -{ -class Arrow; -class Axes; -class BoolProperty; -class FloatProperty; -class MovableText; -class QuaternionProperty; -class StringProperty; -class VectorProperty; - -class FrameInfo; -class FrameSelectionHandler; -typedef boost::shared_ptr FrameSelectionHandlerPtr; - -/** @brief Displays a visual representation of the TF hierarchy. */ -class TFDisplay: public Display -{ -Q_OBJECT -public: - TFDisplay(); - virtual ~TFDisplay(); - - virtual void update(float wall_dt, float ros_dt); - -protected: - // Overrides from Display - virtual void onInitialize(); - virtual void load(const Config& config); - virtual void fixedFrameChanged(); - virtual void reset(); - -private Q_SLOTS: - void updateShowAxes(); - void updateShowArrows(); - void updateShowNames(); - void allEnabledChanged(); - -private: - void updateFrames(); - FrameInfo* createFrame(const std::string& frame); - void updateFrame(FrameInfo* frame); - void deleteFrame(FrameInfo* frame, bool delete_properties); - - FrameInfo* getFrameInfo(const std::string& frame); - - void clear(); - - // overrides from Display - virtual void onEnable(); - virtual void onDisable(); - - Ogre::SceneNode* root_node_; - Ogre::SceneNode* names_node_; - Ogre::SceneNode* arrows_node_; - Ogre::SceneNode* axes_node_; - - typedef std::map M_FrameInfo; - M_FrameInfo frames_; - - typedef std::map M_EnabledState; - M_EnabledState frame_config_enabled_state_; - - float update_timer_; - - BoolProperty* show_names_property_; - BoolProperty* show_arrows_property_; - BoolProperty* show_axes_property_; - FloatProperty* update_rate_property_; - FloatProperty* frame_timeout_property_; - BoolProperty* all_enabled_property_; - - FloatProperty* scale_property_; - - Property* frames_category_; - Property* tree_category_; - - bool changing_single_frame_enabled_state_; - friend class FrameInfo; -}; - -/** @brief Internal class needed only by TFDisplay. */ -class FrameInfo: public QObject -{ - Q_OBJECT - public: - FrameInfo( TFDisplay* display ); - - /** @brief Set this frame to be visible or invisible. */ - void setEnabled( bool enabled ); - -public Q_SLOTS: - /** @brief Update whether the frame is visible or not, based on the enabled_property_ in this FrameInfo. */ - void updateVisibilityFromFrame(); - - /** @brief Update whether the frame is visible or not, based on the enabled_property_ in the selection handler. */ - void updateVisibilityFromSelection(); - -public: - TFDisplay* display_; - std::string name_; - std::string parent_; - Axes* axes_; - CollObjectHandle axes_coll_; - FrameSelectionHandlerPtr selection_handler_; - Arrow* parent_arrow_; - MovableText* name_text_; - Ogre::SceneNode* name_node_; - - float distance_to_parent_; - Ogre::Quaternion arrow_orientation_; - - ros::Time last_update_; - ros::Time last_time_to_fixed_; - - VectorProperty* rel_position_property_; - QuaternionProperty* rel_orientation_property_; - VectorProperty* position_property_; - QuaternionProperty* orientation_property_; - StringProperty* parent_property_; - BoolProperty* enabled_property_; - - Property* tree_property_; -}; - -} // end namespace rviz - -#endif // RVIZ_TF_DISPLAY_H diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt index c41b3e63f..dc4de0367 100644 --- a/rviz_common/CMakeLists.txt +++ b/rviz_common/CMakeLists.txt @@ -119,8 +119,6 @@ set(rviz_common_headers_to_moc ) # TODO(wjwwood): remove this later -list(APPEND rviz_common_headers_to_moc - src/rviz_common/temp/default_plugins/displays/tf_display.hpp) list(APPEND rviz_common_headers_to_moc src/rviz_common/temp/default_plugins/displays/robot_model_display.hpp) list(APPEND rviz_common_headers_to_moc @@ -207,8 +205,6 @@ set(rviz_common_source_files ) # TODO(wjwwood): remove this later -list(APPEND rviz_common_source_files - src/rviz_common/temp/default_plugins/displays/tf_display.cpp) list(APPEND rviz_common_source_files src/rviz_common/temp/default_plugins/displays/robot_model_display.cpp) list(APPEND rviz_common_headers_to_moc diff --git a/rviz_common/src/rviz_common/add_display_dialog.cpp b/rviz_common/src/rviz_common/add_display_dialog.cpp index f3ebb6b38..44249daeb 100644 --- a/rviz_common/src/rviz_common/add_display_dialog.cpp +++ b/rviz_common/src/rviz_common/add_display_dialog.cpp @@ -116,7 +116,8 @@ bool isSubtopic(const std::string & base, const std::string & topic) } std::string query = topic; - while (query != "/") { + // Both checks are required, otherwise the loop does not terminate when adding 'by topic' + while (query != "" && query != "/") { if (query == base) { return true; } diff --git a/rviz_common/src/rviz_common/display_factory.cpp b/rviz_common/src/rviz_common/display_factory.cpp index 006fca9ee..a7f14b6a6 100644 --- a/rviz_common/src/rviz_common/display_factory.cpp +++ b/rviz_common/src/rviz_common/display_factory.cpp @@ -41,7 +41,6 @@ // TODO(wjwwood): remove this block (within if-endif) once plugins moved to default plugins package #if 1 -#include "./temp/default_plugins/displays/tf_display.hpp" #include "./temp/default_plugins/displays/robot_model_display.hpp" #endif @@ -57,10 +56,6 @@ static Display * newDisplayGroup() // TODO(wjwwood): remove this block (within if-endif) once plugins moved to default plugins package #if 1 -static Display * newTFDisplay() -{ - return new rviz_common::TFDisplay(); -} static Display * newRobotModelDisplay() { return new rviz_common::RobotModelDisplay(); @@ -72,7 +67,6 @@ DisplayFactory::DisplayFactory() : PluginlibFactory("rviz_common", "rviz_common::Display") { addBuiltInClass("rviz", "Group", "A container for Displays", &newDisplayGroup); - addBuiltInClass("rviz", "TF", "tf display", &newTFDisplay); addBuiltInClass("rviz", "RobotModel", "robot model display", &newRobotModelDisplay); } diff --git a/rviz_common/src/rviz_common/temp/default_plugins/displays/tf_display.cpp b/rviz_common/src/rviz_common/temp/default_plugins/displays/tf_display.cpp deleted file mode 100644 index e6176bda0..000000000 --- a/rviz_common/src/rviz_common/temp/default_plugins/displays/tf_display.cpp +++ /dev/null @@ -1,856 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "tf_display.hpp" - -#include -#include -#include -#include -#include -#include -#include - -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wunused-parameter" -#endif - -#include -#include - -#ifndef _WIN32 -# pragma GCC diagnostic pop -#endif - -#include "tf2_ros/transform_listener.h" - -#include "rviz_common/display_context.hpp" -#include "rviz_common/frame_manager.hpp" -#include "rviz_rendering/arrow.hpp" -#include "rviz_rendering/axes.hpp" -#include "rviz_rendering/movable_text.hpp" -#include "rviz_common/properties/bool_property.hpp" -#include "rviz_common/properties/float_property.hpp" -#include "rviz_common/properties/quaternion_property.hpp" -#include "rviz_common/properties/string_property.hpp" -#include "rviz_common/properties/vector_property.hpp" -#include "rviz_common/selection/forwards.hpp" -#include "rviz_common/selection/selection_manager.hpp" -#include "rviz_common/logging.hpp" - -using rviz_common::selection::SelectionHandler; -using rviz_common::properties::BoolProperty; -using rviz_common::properties::FloatProperty; -using rviz_common::properties::StatusProperty; -using rviz_common::properties::StringProperty; -using rviz_common::properties::Property; -using rviz_common::properties::QuaternionProperty; -using rviz_common::properties::VectorProperty; -using rviz_common::selection::Picked; -using rviz_rendering::Axes; -using rviz_rendering::Arrow; -using rviz_rendering::MovableText; - -namespace rviz_common -{ - -class FrameSelectionHandler : public SelectionHandler -{ -public: - FrameSelectionHandler(FrameInfo * frame, TFDisplay * display, DisplayContext * context); - virtual ~FrameSelectionHandler() {} - - virtual void createProperties(const Picked & obj, Property * parent_property); - virtual void destroyProperties(const Picked & obj, Property * parent_property); - - bool getEnabled(); - void setEnabled(bool enabled); - void setParentName(std::string parent_name); - void setPosition(const Ogre::Vector3 & position); - void setOrientation(const Ogre::Quaternion & orientation); - -private: - FrameInfo * frame_; - TFDisplay * display_; - Property * category_property_; - BoolProperty * enabled_property_; - StringProperty * parent_property_; - VectorProperty * position_property_; - QuaternionProperty * orientation_property_; -}; - -FrameSelectionHandler::FrameSelectionHandler( - FrameInfo * frame, TFDisplay * display, - DisplayContext * context) -: SelectionHandler(context), - frame_(frame), - display_(display), - category_property_(NULL), - enabled_property_(NULL), - parent_property_(NULL), - position_property_(NULL), - orientation_property_(NULL) -{ -} - -void FrameSelectionHandler::createProperties(const Picked & obj, Property * parent_property) -{ - (void) obj; - (void) display_; - category_property_ = new Property("Frame " + QString::fromStdString(frame_->name_), - QVariant(), "", parent_property); - - enabled_property_ = - new BoolProperty("Enabled", true, "", category_property_, SLOT( - updateVisibilityFromSelection()), frame_); - - parent_property_ = new StringProperty("Parent", "", "", category_property_); - parent_property_->setReadOnly(true); - - position_property_ = new VectorProperty("Position", Ogre::Vector3::ZERO, "", category_property_); - position_property_->setReadOnly(true); - - orientation_property_ = new QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY, "", - category_property_); - orientation_property_->setReadOnly(true); -} - -void FrameSelectionHandler::destroyProperties(const Picked & obj, Property * parent_property) -{ - (void) obj; - (void) parent_property; - delete category_property_; // This deletes its children as well. - category_property_ = NULL; - enabled_property_ = NULL; - parent_property_ = NULL; - position_property_ = NULL; - orientation_property_ = NULL; -} - -bool FrameSelectionHandler::getEnabled() -{ - if (enabled_property_) { - return enabled_property_->getBool(); - } - return false; // should never happen, but don't want to crash if it does. -} - -void FrameSelectionHandler::setEnabled(bool enabled) -{ - if (enabled_property_) { - enabled_property_->setBool(enabled); - } -} - -void FrameSelectionHandler::setParentName(std::string parent_name) -{ - if (parent_property_) { - parent_property_->setStdString(parent_name); - } -} - -void FrameSelectionHandler::setPosition(const Ogre::Vector3 & position) -{ - if (position_property_) { - position_property_->setVector(position); - } -} - -void FrameSelectionHandler::setOrientation(const Ogre::Quaternion & orientation) -{ - if (orientation_property_) { - orientation_property_->setQuaternion(orientation); - } -} - -typedef std::set S_FrameInfo; - -TFDisplay::TFDisplay() -: Display(), - update_timer_(0.0f), - changing_single_frame_enabled_state_(false) -{ - show_names_property_ = new BoolProperty("Show Names", false, - "Whether or not names should be shown next to the frames.", - this, SLOT(updateShowNames())); - - show_axes_property_ = new BoolProperty("Show Axes", true, - "Whether or not the axes of each frame should be shown.", - this, SLOT(updateShowAxes())); - - show_arrows_property_ = new BoolProperty("Show Arrows", true, - "Whether or not arrows from child to parent should be shown.", - this, SLOT(updateShowArrows())); - - scale_property_ = new FloatProperty("Marker Scale", 1, - "Scaling factor for all names, axes and arrows.", this); - - update_rate_property_ = new FloatProperty("Update Interval", 0, - "The interval, in seconds, at which to update the frame transforms. " - "0 means to do so every update cycle.", - this); - update_rate_property_->setMin(0); - - frame_timeout_property_ = new FloatProperty("Frame Timeout", 15, - "The length of time, in seconds, before a frame that has not been updated is considered" - "\"dead\". For 1/3 of this time the frame will appear correct, for the second 1/3rd it will" - " fade to gray, and then it will fade out completely.", - this); - frame_timeout_property_->setMin(1); - - frames_category_ = new Property("Frames", QVariant(), "The list of all frames.", this); - - all_enabled_property_ = new BoolProperty("All Enabled", true, - "Whether all the frames should be enabled or not.", - frames_category_, SLOT(allEnabledChanged()), this); - - tree_category_ = new Property("Tree", - QVariant(), "A tree-view of the frames, showing the parent/child relationships.", this); -} - -TFDisplay::~TFDisplay() -{ - if (initialized() ) { - root_node_->removeAndDestroyAllChildren(); - scene_manager_->destroySceneNode(root_node_->getName() ); - } -} - -void TFDisplay::onInitialize() -{ - frame_config_enabled_state_.clear(); - - root_node_ = scene_node_->createChildSceneNode(); - - names_node_ = root_node_->createChildSceneNode(); - arrows_node_ = root_node_->createChildSceneNode(); - axes_node_ = root_node_->createChildSceneNode(); -} - -void TFDisplay::load(const Config & config) -{ - Display::load(config); - - // Load the enabled state for all frames specified in the config, and store - // the values in a map so that the enabled state can be properly set once - // the frame is created - Config c = config.mapGetChild("Frames"); - for (Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance() ) { - QString key = iter.currentKey(); - if (key != "All Enabled") { - const Config & child = iter.currentChild(); - bool enabled = child.mapGetChild("Value").getValue().toBool(); - - frame_config_enabled_state_[key.toStdString()] = enabled; - } - } -} - -void TFDisplay::clear() -{ - // Clear the tree. - tree_category_->removeChildren(); - - // Clear the frames category, except for the "All enabled" property, which is first. - frames_category_->removeChildren(1); - - S_FrameInfo to_delete; - M_FrameInfo::iterator frame_it = frames_.begin(); - M_FrameInfo::iterator frame_end = frames_.end(); - for (; frame_it != frame_end; ++frame_it) { - to_delete.insert(frame_it->second); - } - - S_FrameInfo::iterator delete_it = to_delete.begin(); - S_FrameInfo::iterator delete_end = to_delete.end(); - for (; delete_it != delete_end; ++delete_it) { - deleteFrame(*delete_it, false); - } - - frames_.clear(); - - update_timer_ = 0.0f; - - clearStatuses(); -} - -void TFDisplay::onEnable() -{ - root_node_->setVisible(true); - - names_node_->setVisible(show_names_property_->getBool() ); - arrows_node_->setVisible(show_arrows_property_->getBool() ); - axes_node_->setVisible(show_axes_property_->getBool() ); -} - -void TFDisplay::onDisable() -{ - root_node_->setVisible(false); - clear(); -} - -void TFDisplay::updateShowNames() -{ - names_node_->setVisible(show_names_property_->getBool() ); - - M_FrameInfo::iterator it = frames_.begin(); - M_FrameInfo::iterator end = frames_.end(); - for (; it != end; ++it) { - FrameInfo * frame = it->second; - - frame->updateVisibilityFromFrame(); - } -} - -void TFDisplay::updateShowAxes() -{ - axes_node_->setVisible(show_axes_property_->getBool() ); - - M_FrameInfo::iterator it = frames_.begin(); - M_FrameInfo::iterator end = frames_.end(); - for (; it != end; ++it) { - FrameInfo * frame = it->second; - - frame->updateVisibilityFromFrame(); - } -} - -void TFDisplay::updateShowArrows() -{ - arrows_node_->setVisible(show_arrows_property_->getBool() ); - - M_FrameInfo::iterator it = frames_.begin(); - M_FrameInfo::iterator end = frames_.end(); - for (; it != end; ++it) { - FrameInfo * frame = it->second; - - frame->updateVisibilityFromFrame(); - } -} - -void TFDisplay::allEnabledChanged() -{ - if (changing_single_frame_enabled_state_) { - return; - } - bool enabled = all_enabled_property_->getBool(); - - M_FrameInfo::iterator it = frames_.begin(); - M_FrameInfo::iterator end = frames_.end(); - for (; it != end; ++it) { - FrameInfo * frame = it->second; - - frame->enabled_property_->setBool(enabled); - } -} - -void TFDisplay::update(float wall_dt, float ros_dt) -{ - (void) ros_dt; - update_timer_ += wall_dt; - float update_rate = update_rate_property_->getFloat(); - if (update_rate < 0.0001f || update_timer_ > update_rate) { - updateFrames(); - - update_timer_ = 0.0f; - } -} - -FrameInfo * TFDisplay::getFrameInfo(const std::string & frame) -{ - M_FrameInfo::iterator it = frames_.find(frame); - if (it == frames_.end() ) { - return NULL; - } - - return it->second; -} - -void TFDisplay::updateFrames() -{ - typedef std::vector V_string; - V_string frames; - context_->getFrameManager()->getTFBufferPtr()->_getFrameStrings(frames); - std::sort(frames.begin(), frames.end()); - - S_FrameInfo current_frames; - - { - V_string::iterator it = frames.begin(); - V_string::iterator end = frames.end(); - for (; it != end; ++it) { - const std::string & frame = *it; - - if (frame.empty() ) { - continue; - } - - FrameInfo * info = getFrameInfo(frame); - if (!info) { - info = createFrame(frame); - } else { - updateFrame(info); - } - - current_frames.insert(info); - } - } - - { - S_FrameInfo to_delete; - M_FrameInfo::iterator frame_it = frames_.begin(); - M_FrameInfo::iterator frame_end = frames_.end(); - for (; frame_it != frame_end; ++frame_it) { - if (current_frames.find(frame_it->second) == current_frames.end() ) { - to_delete.insert(frame_it->second); - } - } - - S_FrameInfo::iterator delete_it = to_delete.begin(); - S_FrameInfo::iterator delete_end = to_delete.end(); - for (; delete_it != delete_end; ++delete_it) { - deleteFrame(*delete_it, true); - } - } - - context_->queueRender(); -} - -static const Ogre::ColourValue ARROW_HEAD_COLOR(1.0f, 0.1f, 0.6f, 1.0f); -static const Ogre::ColourValue ARROW_SHAFT_COLOR(0.8f, 0.8f, 0.3f, 1.0f); - -FrameInfo * TFDisplay::createFrame(const std::string & frame) -{ - FrameInfo * info = new FrameInfo(this); - frames_.insert(std::make_pair(frame, info) ); - - info->name_ = frame; - info->last_update_ = tf2::get_now(); - info->axes_ = new Axes(scene_manager_, axes_node_, 0.2f, 0.02f); - info->axes_->getSceneNode()->setVisible(show_axes_property_->getBool() ); - info->selection_handler_.reset(new FrameSelectionHandler(info, this, context_)); - info->selection_handler_->addTrackedObjects(info->axes_->getSceneNode() ); - - info->name_text_ = new MovableText(frame, "Liberation Sans", 0.1f); - info->name_text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_BELOW); - info->name_node_ = names_node_->createChildSceneNode(); - info->name_node_->attachObject(info->name_text_); - info->name_node_->setVisible(show_names_property_->getBool() ); - - info->parent_arrow_ = new Arrow(scene_manager_, arrows_node_, 1.0f, 0.01f, 1.0f, 0.08f); - info->parent_arrow_->getSceneNode()->setVisible(false); - info->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR); - info->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR); - - info->enabled_property_ = new BoolProperty(QString::fromStdString( - info->name_), true, "Enable or disable this individual frame.", - frames_category_, SLOT(updateVisibilityFromFrame()), info); - - info->parent_property_ = new StringProperty("Parent", "", - "Parent of this frame. (Not editable)", - info->enabled_property_); - info->parent_property_->setReadOnly(true); - - info->position_property_ = new VectorProperty("Position", Ogre::Vector3::ZERO, - "Position of this frame, in the current Fixed Frame. (Not editable)", - info->enabled_property_); - info->position_property_->setReadOnly(true); - - info->orientation_property_ = new QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY, - "Orientation of this frame, in the current Fixed Frame. (Not editable)", - info->enabled_property_); - info->orientation_property_->setReadOnly(true); - - info->rel_position_property_ = new VectorProperty("Relative Position", Ogre::Vector3::ZERO, - "Position of this frame, relative to it's parent frame. (Not editable)", - info->enabled_property_); - info->rel_position_property_->setReadOnly(true); - - info->rel_orientation_property_ = new QuaternionProperty("Relative Orientation", - Ogre::Quaternion::IDENTITY, - "Orientation of this frame, relative to it's parent frame. (Not editable)", - info->enabled_property_); - info->rel_orientation_property_->setReadOnly(true); - - // If the current frame was specified as disabled in the config file - // then its enabled state must be updated accordingly - if (frame_config_enabled_state_.count(frame) > 0 && !frame_config_enabled_state_[frame]) { - info->enabled_property_->setBool(false); - } - - updateFrame(info); - - return info; -} - -Ogre::ColourValue lerpColor( - const Ogre::ColourValue & start, - const Ogre::ColourValue & end, float t) -{ - return start * t + end * (1 - t); -} - -void TFDisplay::updateFrame(FrameInfo * frame) -{ - std::shared_ptr tf_buffer = context_->getFrameManager()->getTFBufferPtr(); - - // Check last received time so we can grey out/fade out frames that have stopped being published - tf2::TimePoint latest_time; - // TODO(wjwwood): figure out where the `/` is coming from and remove it - // also consider warning the user in the GUI about this... - std::string stripped_fixed_frame = fixed_frame_.toStdString(); - if (stripped_fixed_frame[0] == '/') { - stripped_fixed_frame = stripped_fixed_frame.substr(1); - } - try { - tf_buffer->_getLatestCommonTime( - tf_buffer->_validateFrameId("get_latest_common_time", stripped_fixed_frame), - tf_buffer->_validateFrameId("get_latest_common_time", frame->name_), - latest_time, - 0); - } catch (const tf2::LookupException & e) { - RVIZ_COMMON_LOG_DEBUG_STREAM( - "Error transforming frame '" << frame->parent_.c_str() << - "' (parent of '" << frame->name_.c_str() << - "') to frame '" << qPrintable(fixed_frame_) << "': " << e.what()); - return; - } - - if (( latest_time != frame->last_time_to_fixed_ ) || - ( latest_time == tf2::TimePointZero )) - { - frame->last_update_ = tf2::get_now(); - frame->last_time_to_fixed_ = latest_time; - } - - // Fade from color -> grey, then grey -> fully transparent - double age = tf2::durationToSec(tf2::get_now() - frame->last_update_); - double frame_timeout = frame_timeout_property_->getFloat(); - double one_third_timeout = frame_timeout * 0.3333333f; - if (age > frame_timeout) { - frame->parent_arrow_->getSceneNode()->setVisible(false); - frame->axes_->getSceneNode()->setVisible(false); - frame->name_node_->setVisible(false); - return; - } else if (age > one_third_timeout) { - Ogre::ColourValue grey(0.7f, 0.7f, 0.7f, 1.0f); - - if (age > one_third_timeout * 2) { - double a = std::max(0.0, (frame_timeout - age) / one_third_timeout); - Ogre::ColourValue c = Ogre::ColourValue(grey.r, grey.g, grey.b, a); - - frame->axes_->setXColor(c); - frame->axes_->setYColor(c); - frame->axes_->setZColor(c); - frame->name_text_->setColor(c); - frame->parent_arrow_->setColor(c.r, c.g, c.b, c.a); - } else { - double t = std::max(0.0, (one_third_timeout * 2 - age) / one_third_timeout); - frame->axes_->setXColor(lerpColor(frame->axes_->getDefaultXColor(), grey, t)); - frame->axes_->setYColor(lerpColor(frame->axes_->getDefaultYColor(), grey, t)); - frame->axes_->setZColor(lerpColor(frame->axes_->getDefaultZColor(), grey, t)); - frame->name_text_->setColor(lerpColor(Ogre::ColourValue::White, grey, t)); - frame->parent_arrow_->setShaftColor(lerpColor(ARROW_SHAFT_COLOR, grey, t)); - frame->parent_arrow_->setHeadColor(lerpColor(ARROW_HEAD_COLOR, grey, t)); - } - } else { - frame->axes_->setToDefaultColors(); - frame->name_text_->setColor(Ogre::ColourValue::White); - frame->parent_arrow_->setHeadColor(ARROW_HEAD_COLOR); - frame->parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR); - } - - setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK"); - - Ogre::Vector3 position; - position.x = 0; - position.y = 0; - position.z = 0; - Ogre::Quaternion orientation; - orientation.w = 1.0; - orientation.x = 0; - orientation.y = 0; - orientation.z = 0; - if (!context_->getFrameManager()->getTransform(frame->name_, position, orientation)) { - std::stringstream ss; - ss << "No transform from [" << frame->name_ << "] to frame [" << fixed_frame_.toStdString() << - "]"; - setStatusStd(StatusProperty::Warn, frame->name_, ss.str()); - // RVIZ_COMMON_LOG_DEBUG_STREAM( - // "Error transforming frame '" << frame->name_.c_str() << - // "' to frame '" << qPrintable( fixed_frame_ ) << "'"); - frame->name_node_->setVisible(false); - frame->axes_->getSceneNode()->setVisible(false); - frame->parent_arrow_->getSceneNode()->setVisible(false); - return; - } - - frame->selection_handler_->setPosition(position); - frame->selection_handler_->setOrientation(orientation); - - bool frame_enabled = frame->enabled_property_->getBool(); - - frame->axes_->setPosition(position); - frame->axes_->setOrientation(orientation); - frame->axes_->getSceneNode()->setVisible(show_axes_property_->getBool() && frame_enabled); - float scale = scale_property_->getFloat(); - frame->axes_->setScale(Ogre::Vector3(scale, scale, scale)); - - frame->name_node_->setPosition(position); - frame->name_node_->setVisible(show_names_property_->getBool() && frame_enabled); - frame->name_node_->setScale(scale, scale, scale); - - frame->position_property_->setVector(position); - frame->orientation_property_->setQuaternion(orientation); - - std::string old_parent = frame->parent_; - frame->parent_.clear(); - bool has_parent = tf_buffer->_getParent(frame->name_, tf2::TimePointZero, frame->parent_); - if (has_parent) { - // If this frame has no tree property or the parent has changed, - if (!frame->tree_property_ || old_parent != frame->parent_) { - // Look up the new parent. - M_FrameInfo::iterator parent_it = frames_.find(frame->parent_); - if (parent_it != frames_.end() ) { - FrameInfo * parent = parent_it->second; - - // If the parent has a tree property, make a new tree property for this frame. - if (parent->tree_property_) { - if (!frame->tree_property_) { - frame->tree_property_ = new Property(QString::fromStdString(frame->name_), - QVariant(), "", parent->tree_property_); - } else { - frame->tree_property_->setParent(parent->tree_property_); - frame->tree_property_->setName(QString::fromStdString(frame->name_)); - frame->tree_property_->setValue(QVariant()); - frame->tree_property_->setDescription(""); - } - } - } - } - - geometry_msgs::msg::TransformStamped transform; - transform.transform.translation.x = 0; - transform.transform.translation.y = 0; - transform.transform.translation.z = 0; - transform.transform.rotation.w = 1.0; - transform.transform.rotation.x = 0; - transform.transform.rotation.y = 0; - transform.transform.rotation.z = 0; - try { - transform = tf_buffer->lookupTransform( - frame->parent_, - frame->name_, - tf2::TimePointZero); - } catch (const tf2::LookupException & e) { - RVIZ_COMMON_LOG_DEBUG_STREAM( - "Error1 transforming frame '" << frame->parent_.c_str() << - "' (parent of '" << frame->name_.c_str() << - "') to frame '" << qPrintable(fixed_frame_) << "': " << e.what()); - } catch (const tf2::TransformException & e) { - RVIZ_COMMON_LOG_DEBUG_STREAM( - "Error2 transforming frame '" << frame->parent_.c_str() << - "' (parent of '" << frame->name_.c_str() << - "') to frame '" << qPrintable(fixed_frame_) << "': " << e.what()); - } - - // get the position/orientation relative to the parent frame - Ogre::Vector3 relative_position( - transform.transform.translation.x, - transform.transform.translation.y, - transform.transform.translation.z - ); - Ogre::Quaternion relative_orientation( - transform.transform.rotation.w, - transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z); - frame->rel_position_property_->setVector(relative_position); - frame->rel_orientation_property_->setQuaternion(relative_orientation); - - if (show_arrows_property_->getBool() ) { - Ogre::Vector3 parent_position; - parent_position.x = 0; - parent_position.y = 0; - parent_position.z = 0; - Ogre::Quaternion parent_orientation; - parent_orientation.w = 1.0; - parent_orientation.x = 0.0; - parent_orientation.y = 0.0; - parent_orientation.z = 0.0; - if (!context_->getFrameManager()->getTransform( - frame->parent_, parent_position, parent_orientation)) - { - RVIZ_COMMON_LOG_DEBUG_STREAM( - "Error3 transforming frame '" << frame->parent_.c_str() << - "' (parent of '" << frame->name_.c_str() << - "') to frame '" << qPrintable(fixed_frame_) << "'"); - } else { - Ogre::Vector3 direction = parent_position - position; - float distance = direction.length(); - direction.normalise(); - - Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(direction); - - if (!orient.isNaN()) { - frame->distance_to_parent_ = distance; - float head_length = (distance < 0.1f * scale) ? (0.1f * scale * distance) : 0.1f * scale; - float shaft_length = distance - head_length; - // aleeper: This was changed from 0.02 and 0.08 to 0.01 and 0.04 - // to match proper radius handling in arrow.cpp - frame->parent_arrow_->set(shaft_length, 0.01f * scale, head_length, 0.04f * scale); - - if (distance > 0.001f) { - frame->parent_arrow_->getSceneNode()->setVisible( - show_arrows_property_->getBool() && frame_enabled); - } else { - frame->parent_arrow_->getSceneNode()->setVisible(false); - } - - frame->parent_arrow_->setPosition(position); - frame->parent_arrow_->setOrientation(orient); - } - } - } else { - frame->parent_arrow_->getSceneNode()->setVisible(false); - } - } else { - if (!frame->tree_property_ || old_parent != frame->parent_) { - if (!frame->tree_property_) { - frame->tree_property_ = new Property(QString::fromStdString(frame->name_), - QVariant(), "", tree_category_); - } else { - frame->tree_property_->setName(QString::fromStdString(frame->name_)); - frame->tree_property_->setValue(QVariant()); - frame->tree_property_->setDescription(""); - frame->tree_property_->setParent(tree_category_); - } - } - - frame->parent_arrow_->getSceneNode()->setVisible(false); - } - - frame->parent_property_->setStdString(frame->parent_); - frame->selection_handler_->setParentName(frame->parent_); -} - -void TFDisplay::deleteFrame(FrameInfo * frame, bool delete_properties) -{ - M_FrameInfo::iterator it = frames_.find(frame->name_); - assert(it != frames_.end()); - - frames_.erase(it); - - delete frame->axes_; - context_->getSelectionManager()->removeObject(frame->axes_coll_); - delete frame->parent_arrow_; - delete frame->name_text_; - scene_manager_->destroySceneNode(frame->name_node_->getName() ); - if (delete_properties) { - delete frame->enabled_property_; - delete frame->tree_property_; - } - delete frame; -} - -void TFDisplay::fixedFrameChanged() -{ - update_timer_ = update_rate_property_->getFloat(); -} - -void TFDisplay::reset() -{ - Display::reset(); - clear(); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////// -// FrameInfo - -FrameInfo::FrameInfo(TFDisplay * display) -: display_(display), - axes_(NULL), - axes_coll_(0), - parent_arrow_(NULL), - name_text_(NULL), - distance_to_parent_(0.0f), - arrow_orientation_(Ogre::Quaternion::IDENTITY), - tree_property_(NULL) -{} - -void FrameInfo::updateVisibilityFromFrame() -{ - bool enabled = enabled_property_->getBool(); - selection_handler_->setEnabled(enabled); - setEnabled(enabled); -} - -void FrameInfo::updateVisibilityFromSelection() -{ - bool enabled = selection_handler_->getEnabled(); - enabled_property_->setBool(enabled); - setEnabled(enabled); -} - -void FrameInfo::setEnabled(bool enabled) -{ - if (name_node_) { - name_node_->setVisible(display_->show_names_property_->getBool() && enabled); - } - - if (axes_) { - axes_->getSceneNode()->setVisible(display_->show_axes_property_->getBool() && enabled); - } - - if (parent_arrow_) { - if (distance_to_parent_ > 0.001f) { - parent_arrow_->getSceneNode()->setVisible( - display_->show_arrows_property_->getBool() && enabled); - } else { - parent_arrow_->getSceneNode()->setVisible(false); - } - } - - if (display_->all_enabled_property_->getBool() && !enabled) { - display_->changing_single_frame_enabled_state_ = true; - display_->all_enabled_property_->setBool(false); - display_->changing_single_frame_enabled_state_ = false; - } - - // Update the configuration that stores the enabled state of all frames - display_->frame_config_enabled_state_[this->name_] = enabled; - - display_->context_->queueRender(); -} - -} // namespace rviz_common - -// #include -// PLUGINLIB_EXPORT_CLASS( rviz::TFDisplay, rviz::Display ) diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 5b2ef61c1..17c41ce39 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -38,6 +38,8 @@ set(rviz_default_plugins_headers_to_moc src/rviz_default_plugins/displays/pointcloud/point_cloud2_display.hpp src/rviz_default_plugins/displays/pointcloud/point_cloud_transformer.hpp src/rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp + src/rviz_default_plugins/displays/tf/frame_info.hpp + src/rviz_default_plugins/displays/tf/tf_display.hpp ) set(rviz_default_plugins_source_files @@ -55,6 +57,9 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.cpp src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp src/rviz_default_plugins/displays/pointcloud/point_cloud2_display.cpp + src/rviz_default_plugins/displays/tf/frame_info.cpp + src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp + src/rviz_default_plugins/displays/tf/tf_display.cpp src/rviz_default_plugins/tools/move/move_tool.cpp src/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.cpp ) diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index 94c2d2cef..7bcb1cc16 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -45,6 +45,16 @@ sensor_msgs/PointCloud2 + + + Displays the TF transform hierarchy. <a href="http://www.ros.org/wiki/rviz/DisplayTypes/TF">More Information</a>. + + tf2_msgs/TFMessage + + +#include + +#include "rviz_rendering/arrow.hpp" +#include "rviz_rendering/axes.hpp" +#include "rviz_rendering/movable_text.hpp" +#include "rviz_common/display_context.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/properties/quaternion_property.hpp" +#include "frame_selection_handler.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +FrameInfo::FrameInfo(TFDisplay * display) +: display_(display), + axes_(nullptr), + axes_coll_(0), + parent_arrow_(nullptr), + name_text_(nullptr), + distance_to_parent_(0.0f), + arrow_orientation_(Ogre::Quaternion::IDENTITY), + tree_property_(nullptr) +{} + +const Ogre::ColourValue FrameInfo::ARROW_HEAD_COLOR(1.0f, 0.1f, 0.6f, 1.0f); +const Ogre::ColourValue FrameInfo::ARROW_SHAFT_COLOR(0.8f, 0.8f, 0.3f, 1.0f); + + +void FrameInfo::updateVisibilityFromFrame() +{ + bool enabled = enabled_property_->getBool(); + selection_handler_->setEnabled(enabled); + setEnabled(enabled); +} + +void FrameInfo::updateVisibilityFromSelection() +{ + bool enabled = selection_handler_->getEnabled(); + enabled_property_->setBool(enabled); + setEnabled(enabled); +} + +void FrameInfo::setEnabled(bool enabled) +{ + if (name_node_) { + setNamesVisible(display_->show_names_property_->getBool()); + } + + if (axes_) { + setAxesVisible(display_->show_axes_property_->getBool()); + } + + if (parent_arrow_) { + setParentArrowVisible(display_->show_arrows_property_->getBool()); + } + + if (display_->all_enabled_property_->getBool() && !enabled) { + display_->changing_single_frame_enabled_state_ = true; + display_->all_enabled_property_->setBool(false); + display_->changing_single_frame_enabled_state_ = false; + } + + // Update the configuration that stores the enabled state of all frames + display_->frame_config_enabled_state_[this->name_] = enabled; + + display_->context_->queueRender(); +} + +void FrameInfo::updatePositionAndOrientation( + const Ogre::Vector3 & position, const Ogre::Quaternion & orientation, float scale) +{ + selection_handler_->setPosition(position); + selection_handler_->setOrientation(orientation); + axes_->setPosition(position); + axes_->setOrientation(orientation); + axes_->setScale(Ogre::Vector3(scale, scale, scale)); + + name_node_->setPosition(position); + name_node_->setScale(scale, scale, scale); + + position_property_->setVector(position); + orientation_property_->setQuaternion(orientation); +} + +void FrameInfo::updateTreeProperty(rviz_common::properties::Property * parent) +{ + if (!tree_property_) { + tree_property_ = new rviz_common::properties::Property( + QString::fromStdString(name_), QVariant(), "", parent); + } else { + tree_property_->setParent(parent); + tree_property_->setName(QString::fromStdString(name_)); + tree_property_->setValue(QVariant()); + tree_property_->setDescription(""); + } +} + +void FrameInfo::setVisible(bool show_frame) +{ + setNamesVisible(show_frame); + setAxesVisible(show_frame); + setParentArrowVisible(show_frame); +} + +void FrameInfo::setNamesVisible(bool show_names) +{ + bool frame_enabled = enabled_property_->getBool(); + name_node_->setVisible(show_names && frame_enabled); +} + +void FrameInfo::setAxesVisible(bool show_axes) +{ + bool frame_enabled = enabled_property_->getBool(); + axes_->getSceneNode()->setVisible(show_axes && frame_enabled); +} + +void FrameInfo::setParentArrowVisible(bool show_parent_arrow) +{ + bool frame_enabled = enabled_property_->getBool(); + if (distance_to_parent_ > 0.001f) { + parent_arrow_->getSceneNode()->setVisible(show_parent_arrow && frame_enabled); + } else { + parent_arrow_->getSceneNode()->setVisible(false); + } +} + +void FrameInfo::setLastUpdate(const tf2::TimePoint & latest_time) +{ + if ((latest_time != last_time_to_fixed_) || (latest_time == tf2::TimePointZero)) { + last_update_ = tf2::get_now(); + last_time_to_fixed_ = latest_time; + } +} + +Ogre::ColourValue lerpColor(const Ogre::ColourValue & start, const Ogre::ColourValue & end, float t) +{ + return start * t + end * (1 - t); +} + +/// Fade from color -> grey, then grey -> fully transparent +void FrameInfo::updateColorForAge(double age, double frame_timeout) const +{ + double one_third_timeout = frame_timeout * 0.3333333f; + if (age > one_third_timeout) { + Ogre::ColourValue grey(0.7f, 0.7f, 0.7f, 1.0f); + + if (age > one_third_timeout * 2) { + double a = std::max(0.0, (frame_timeout - age) / one_third_timeout); + Ogre::ColourValue c = Ogre::ColourValue(grey.r, grey.g, grey.b, a); + + axes_->setXColor(c); + axes_->setYColor(c); + axes_->setZColor(c); + name_text_->setColor(c); + parent_arrow_->setColor(c.r, c.g, c.b, c.a); + } else { + double t = std::max(0.0, (one_third_timeout * 2 - age) / one_third_timeout); + axes_->setXColor(lerpColor(axes_->getDefaultXColor(), grey, t)); + axes_->setYColor(lerpColor(axes_->getDefaultYColor(), grey, t)); + axes_->setZColor(lerpColor(axes_->getDefaultZColor(), grey, t)); + name_text_->setColor(lerpColor(Ogre::ColourValue::White, grey, t)); + parent_arrow_->setShaftColor(lerpColor(ARROW_SHAFT_COLOR, grey, t)); + parent_arrow_->setHeadColor(lerpColor(ARROW_HEAD_COLOR, grey, t)); + } + } else { + axes_->setToDefaultColors(); + name_text_->setColor(Ogre::ColourValue::White); + parent_arrow_->setHeadColor(ARROW_HEAD_COLOR); + parent_arrow_->setShaftColor(ARROW_SHAFT_COLOR); + } +} + +void FrameInfo::updateParentArrow( + const Ogre::Vector3 & position, + const Ogre::Vector3 & parent_position, + const float scale) +{ + Ogre::Vector3 direction = parent_position - position; + float distance = direction.length(); + direction.normalise(); + + Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(direction); + + if (!orient.isNaN()) { + distance_to_parent_ = distance; + float head_length = (distance < 0.1f * scale) ? (0.1f * scale * distance) : 0.1f * scale; + float shaft_length = distance - head_length; + // aleeper: This was changed from 0.02 and 0.08 to 0.01 and 0.04 + // to match proper radius handling in arrow.cpp + parent_arrow_->set(shaft_length, 0.01f * scale, head_length, 0.04f * scale); + + parent_arrow_->setPosition(position); + parent_arrow_->setOrientation(orient); + } +} + +} // namespace displays +} // namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.hpp new file mode 100644 index 000000000..5a7f9147a --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.hpp @@ -0,0 +1,127 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__FRAME_INFO_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__FRAME_INFO_HPP_ + +#include + +#include "tf2/time.h" + +#include "tf_display.hpp" + +namespace rviz_common +{ + +namespace properties +{ +class Property; +class BoolProperty; +class StringProperty; +class VectorProperty; +class QuaternionProperty; +} // namespace properties +} // namespace rviz_common + +namespace rviz_default_plugins +{ +namespace displays +{ + +/** @brief Internal class needed only by TFDisplay. */ +class FrameInfo : public QObject +{ + Q_OBJECT + +public: + explicit FrameInfo(TFDisplay * display); + + static const Ogre::ColourValue ARROW_HEAD_COLOR; + static const Ogre::ColourValue ARROW_SHAFT_COLOR; + + /** @brief Set this frame to be visible or invisible. */ + void setEnabled(bool enabled); + + void updatePositionAndOrientation( + const Ogre::Vector3 & position, const Ogre::Quaternion & orientation, float scale); + + void setVisible(bool show_frame); + void setNamesVisible(bool show_names); + void setAxesVisible(bool show_axes); + void setParentArrowVisible(bool show_parent_arrow); + void setLastUpdate(const tf2::TimePoint & latest_time); + + void updateTreeProperty(rviz_common::properties::Property * parent); + void updateColorForAge(double age, double frame_timeout) const; + void updateParentArrow( + const Ogre::Vector3 & position, + const Ogre::Vector3 & parent_position, + float scale); + +public Q_SLOTS: + /** @brief Update whether the frame is visible or not, based on the enabled_property_ + * in this FrameInfo. */ + void updateVisibilityFromFrame(); + + /** @brief Update whether the frame is visible or not, based on the enabled_property_ + * in the selection handler. */ + void updateVisibilityFromSelection(); + +public: + TFDisplay * display_; + std::string name_; + std::string parent_; + rviz_rendering::Axes * axes_; + rviz_common::selection::CollObjectHandle axes_coll_; + FrameSelectionHandlerPtr selection_handler_; + rviz_rendering::Arrow * parent_arrow_; + rviz_rendering::MovableText * name_text_; + Ogre::SceneNode * name_node_; + + float distance_to_parent_; + Ogre::Quaternion arrow_orientation_; + + tf2::TimePoint last_update_; + tf2::TimePoint last_time_to_fixed_; + + rviz_common::properties::VectorProperty * rel_position_property_; + rviz_common::properties::QuaternionProperty * rel_orientation_property_; + rviz_common::properties::VectorProperty * position_property_; + rviz_common::properties::QuaternionProperty * orientation_property_; + rviz_common::properties::StringProperty * parent_property_; + rviz_common::properties::BoolProperty * enabled_property_; + + rviz_common::properties::Property * tree_property_; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__FRAME_INFO_HPP_ diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp new file mode 100644 index 000000000..2275a926f --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp @@ -0,0 +1,143 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "frame_selection_handler.hpp" + +#include + +#include "rviz_common/frame_manager.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/quaternion_property.hpp" +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/properties/vector_property.hpp" + +using rviz_common::selection::SelectionHandler; +using rviz_common::properties::BoolProperty; +using rviz_common::properties::FloatProperty; +using rviz_common::properties::StatusProperty; +using rviz_common::properties::StringProperty; +using rviz_common::properties::Property; +using rviz_common::properties::QuaternionProperty; +using rviz_common::properties::VectorProperty; +using rviz_common::selection::Picked; + +namespace rviz_default_plugins +{ +namespace displays +{ + +FrameSelectionHandler::FrameSelectionHandler( + FrameInfo * frame, + TFDisplay * display, + rviz_common::DisplayContext * context) +: SelectionHandler(context), + frame_(frame), + display_(display), + category_property_(nullptr), + enabled_property_(nullptr), + parent_property_(nullptr), + position_property_(nullptr), + orientation_property_(nullptr) +{ +} + +void FrameSelectionHandler::createProperties(const Picked & obj, Property * parent_property) +{ + (void) obj; + (void) display_; + category_property_ = new Property("Frame " + QString::fromStdString(frame_->name_), + QVariant(), "", parent_property); + + enabled_property_ = + new BoolProperty("Enabled", true, "", category_property_, SLOT( + updateVisibilityFromSelection()), frame_); + + parent_property_ = new StringProperty("Parent", "", "", category_property_); + parent_property_->setReadOnly(true); + + position_property_ = new VectorProperty("Position", Ogre::Vector3::ZERO, "", category_property_); + position_property_->setReadOnly(true); + + orientation_property_ = new QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY, "", + category_property_); + orientation_property_->setReadOnly(true); +} + +void FrameSelectionHandler::destroyProperties(const Picked & obj, Property * parent_property) +{ + (void) obj; + (void) parent_property; + delete category_property_; // This deletes its children as well. + category_property_ = nullptr; + enabled_property_ = nullptr; + parent_property_ = nullptr; + position_property_ = nullptr; + orientation_property_ = nullptr; +} + +bool FrameSelectionHandler::getEnabled() +{ + if (enabled_property_) { + return enabled_property_->getBool(); + } + return false; // should never happen, but don't want to crash if it does. +} + +void FrameSelectionHandler::setEnabled(bool enabled) +{ + if (enabled_property_) { + enabled_property_->setBool(enabled); + } +} + +void FrameSelectionHandler::setParentName(std::string parent_name) +{ + if (parent_property_) { + parent_property_->setStdString(parent_name); + } +} + +void FrameSelectionHandler::setPosition(const Ogre::Vector3 & position) +{ + if (position_property_) { + position_property_->setVector(position); + } +} + +void FrameSelectionHandler::setOrientation(const Ogre::Quaternion & orientation) +{ + if (orientation_property_) { + orientation_property_->setQuaternion(orientation); + } +} + +} // namespace displays + +} // namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.hpp new file mode 100644 index 000000000..09f049de3 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_selection_handler.hpp @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__FRAME_SELECTION_HANDLER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__FRAME_SELECTION_HANDLER_HPP_ + +#include + +#include "rviz_common/selection/selection_handler.hpp" +#include "tf_display.hpp" +#include "frame_info.hpp" + +namespace rviz_common +{ + +namespace properties +{ +class Property; +class BoolProperty; +class StringProperty; +class VectorProperty; +class QuaternionProperty; +} // namespace properties +} // namespace rviz_common + +namespace rviz_default_plugins +{ + +namespace displays +{ + +class FrameSelectionHandler : public rviz_common::selection::SelectionHandler +{ +public: + FrameSelectionHandler( + FrameInfo * frame, + TFDisplay * display, + rviz_common::DisplayContext * context); + + ~FrameSelectionHandler() override = default; + + void createProperties( + const rviz_common::selection::Picked & obj, + rviz_common::properties::Property * parent_property) override; + + void destroyProperties( + const rviz_common::selection::Picked & obj, + rviz_common::properties::Property * parent_property) override; + + bool getEnabled(); + + void setEnabled(bool enabled); + + void setParentName(std::string parent_name); + + void setPosition(const Ogre::Vector3 & position); + + void setOrientation(const Ogre::Quaternion & orientation); + +private: + FrameInfo * frame_; + TFDisplay * display_; + rviz_common::properties::Property * category_property_; + rviz_common::properties::BoolProperty * enabled_property_; + rviz_common::properties::StringProperty * parent_property_; + rviz_common::properties::VectorProperty * position_property_; + rviz_common::properties::QuaternionProperty * orientation_property_; +}; + +} // namespace displays + +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__FRAME_SELECTION_HANDLER_HPP_ diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp new file mode 100644 index 000000000..eb549ccdb --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.cpp @@ -0,0 +1,603 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations, GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "tf_display.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +#endif + +#include +#include + +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#include "tf2_ros/transform_listener.h" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/frame_manager.hpp" +#include "rviz_rendering/arrow.hpp" +#include "rviz_rendering/axes.hpp" +#include "rviz_rendering/movable_text.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/quaternion_property.hpp" +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/selection/forwards.hpp" +#include "rviz_common/selection/selection_manager.hpp" +#include "rviz_common/uniform_string_stream.hpp" +#include "frame_info.hpp" +#include "frame_selection_handler.hpp" + +using rviz_common::selection::SelectionHandler; +using rviz_common::properties::BoolProperty; +using rviz_common::properties::FloatProperty; +using rviz_common::properties::StatusProperty; +using rviz_common::properties::StringProperty; +using rviz_common::properties::Property; +using rviz_common::properties::QuaternionProperty; +using rviz_common::properties::VectorProperty; +using rviz_common::selection::Picked; +using rviz_rendering::Axes; +using rviz_rendering::Arrow; +using rviz_rendering::MovableText; + +namespace rviz_default_plugins +{ + +namespace displays +{ + +TFDisplay::TFDisplay() +: update_timer_(0.0f), + changing_single_frame_enabled_state_(false) +{ + show_names_property_ = new BoolProperty( + "Show Names", + false, + "Whether or not names should be shown next to the frames.", + this, + SLOT(updateShowNames())); + + show_axes_property_ = new BoolProperty( + "Show Axes", + true, + "Whether or not the axes of each frame should be shown.", + this, + SLOT(updateShowAxes())); + + show_arrows_property_ = new BoolProperty( + "Show Arrows", + true, + "Whether or not arrows from child to parent should be shown.", + this, + SLOT(updateShowArrows())); + + scale_property_ = new FloatProperty( + "Marker Scale", + 1, + "Scaling factor for all names, axes and arrows.", + this); + + update_rate_property_ = new FloatProperty( + "Update Interval", + 0, + "The interval, in seconds, at which to update the frame transforms. " + "0 means to do so every update cycle.", + this); + update_rate_property_->setMin(0); + + frame_timeout_property_ = new FloatProperty( + "Frame Timeout", + 15, + "The length of time, in seconds, before a frame that has not been updated is considered" + " \"dead\". For 1/3 of this time the frame will appear correct, for the second 1/3rd it will" + " fade to gray, and then it will fade out completely.", + this); + frame_timeout_property_->setMin(1); + + frames_category_ = new Property("Frames", QVariant(), "The list of all frames.", this); + + all_enabled_property_ = new BoolProperty( + "All Enabled", + true, + "Whether all the frames should be enabled or not.", + frames_category_, + SLOT(allEnabledChanged()), + this); + + tree_category_ = new Property( + "Tree", + QVariant(), + "A tree-view of the frames, showing the parent/child relationships.", + this); +} + +TFDisplay::~TFDisplay() +{ + if (initialized()) { + root_node_->removeAndDestroyAllChildren(); + scene_manager_->destroySceneNode(root_node_->getName()); + } +} + +void TFDisplay::onInitialize() +{ + frame_config_enabled_state_.clear(); + + root_node_ = scene_node_->createChildSceneNode(); + + names_node_ = root_node_->createChildSceneNode(); + arrows_node_ = root_node_->createChildSceneNode(); + axes_node_ = root_node_->createChildSceneNode(); +} + +void TFDisplay::load(const rviz_common::Config & config) +{ + rviz_common::Display::load(config); + + // Load the enabled state for all frames specified in the config, and store + // the values in a map so that the enabled state can be properly set once + // the frame is created + rviz_common::Config c = config.mapGetChild("Frames"); + for (auto iter = c.mapIterator(); iter.isValid(); iter.advance()) { + QString key = iter.currentKey(); + if (key != "All Enabled") { + const rviz_common::Config & child = iter.currentChild(); + bool enabled = child.mapGetChild("Value").getValue().toBool(); + + frame_config_enabled_state_[key.toStdString()] = enabled; + } + } +} + +void TFDisplay::clear() +{ + tree_category_->removeChildren(); + + // Clear the frames category, except for the "All enabled" property, which is first. + frames_category_->removeChildren(1); + + S_FrameInfo to_delete; + for (auto & frame : frames_) { + to_delete.insert(frame.second); + } + + for (auto & frame : to_delete) { + deleteFrame(frame, false); + } + + frames_.clear(); + + update_timer_ = 0.0f; + + clearStatuses(); +} + +void TFDisplay::onEnable() +{ + root_node_->setVisible(true); + + names_node_->setVisible(show_names_property_->getBool()); + arrows_node_->setVisible(show_arrows_property_->getBool()); + axes_node_->setVisible(show_axes_property_->getBool()); +} + +void TFDisplay::onDisable() +{ + root_node_->setVisible(false); + clear(); +} + +void TFDisplay::updateShowNames() +{ + names_node_->setVisible(show_names_property_->getBool()); + + for (auto & frame : frames_) { + frame.second->updateVisibilityFromFrame(); + } +} + +void TFDisplay::updateShowAxes() +{ + axes_node_->setVisible(show_axes_property_->getBool()); + + for (auto & frame : frames_) { + frame.second->updateVisibilityFromFrame(); + } +} + +void TFDisplay::updateShowArrows() +{ + arrows_node_->setVisible(show_arrows_property_->getBool()); + + for (auto & frame : frames_) { + frame.second->updateVisibilityFromFrame(); + } +} + +void TFDisplay::allEnabledChanged() +{ + if (changing_single_frame_enabled_state_) { + return; + } + bool enabled = all_enabled_property_->getBool(); + + for (auto & frame : frames_) { + frame.second->enabled_property_->setBool(enabled); + } +} + +void TFDisplay::update(float wall_dt, float ros_dt) +{ + (void) ros_dt; + update_timer_ += wall_dt; + float update_rate = update_rate_property_->getFloat(); + if (update_rate < 0.0001f || update_timer_ > update_rate) { + updateFrames(); + + update_timer_ = 0.0f; + } +} + +void TFDisplay::updateFrames() +{ + typedef std::vector V_string; + V_string frames; + context_->getFrameManager()->getTFBufferPtr()->_getFrameStrings(frames); + std::sort(frames.begin(), frames.end()); + + S_FrameInfo current_frames = createOrUpdateFrames(frames); + deleteObsoleteFrames(current_frames); + + context_->queueRender(); +} + +S_FrameInfo TFDisplay::createOrUpdateFrames(const std::vector & frames) +{ + S_FrameInfo current_frames; + for (auto & frame : frames) { + if (frame.empty()) { + continue; + } + + FrameInfo * info = getFrameInfo(frame); + if (!info) { + info = createFrame(frame); + } else { + updateFrame(info); + } + + current_frames.insert(info); + } + return current_frames; +} + +FrameInfo * TFDisplay::getFrameInfo(const std::string & frame) +{ + auto it = frames_.find(frame); + if (it == frames_.end()) { + return nullptr; + } + + return it->second; +} + +void TFDisplay::deleteObsoleteFrames(S_FrameInfo & current_frames) +{ + S_FrameInfo to_delete; + for (auto & frame : frames_) { + if (current_frames.find(frame.second) == current_frames.end()) { + to_delete.insert(frame.second); + } + } + + for (auto & frame : to_delete) { + deleteFrame(frame, true); + } +} + +FrameInfo * TFDisplay::createFrame(const std::string & frame) +{ + auto info = new FrameInfo(this); + frames_.insert(std::make_pair(frame, info)); + + info->name_ = frame; + info->last_update_ = tf2::get_now(); + info->axes_ = new Axes(scene_manager_, axes_node_, 0.2f, 0.02f); + info->axes_->getSceneNode()->setVisible(show_axes_property_->getBool()); + info->selection_handler_.reset(new FrameSelectionHandler(info, this, context_)); + info->selection_handler_->addTrackedObjects(info->axes_->getSceneNode()); + + info->name_text_ = new MovableText(frame, "Liberation Sans", 0.1f); + info->name_text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_BELOW); + info->name_node_ = names_node_->createChildSceneNode(); + info->name_node_->attachObject(info->name_text_); + info->name_node_->setVisible(show_names_property_->getBool()); + + info->parent_arrow_ = new Arrow(scene_manager_, arrows_node_, 1.0f, 0.01f, 1.0f, 0.08f); + info->parent_arrow_->getSceneNode()->setVisible(false); + info->parent_arrow_->setHeadColor(FrameInfo::ARROW_HEAD_COLOR); + info->parent_arrow_->setShaftColor(FrameInfo::ARROW_SHAFT_COLOR); + + info->enabled_property_ = new BoolProperty( + QString::fromStdString(info->name_), + true, + "Enable or disable this individual frame.", + frames_category_, + SLOT(updateVisibilityFromFrame()), + info); + + info->parent_property_ = new StringProperty( + "Parent", "", + "Parent of this frame. (Not editable)", + info->enabled_property_); + info->parent_property_->setReadOnly(true); + + info->position_property_ = new VectorProperty( + "Position", Ogre::Vector3::ZERO, + "Position of this frame, in the current Fixed Frame. (Not editable)", + info->enabled_property_); + info->position_property_->setReadOnly(true); + + info->orientation_property_ = new QuaternionProperty( + "Orientation", Ogre::Quaternion::IDENTITY, + "Orientation of this frame, in the current Fixed Frame. (Not editable)", + info->enabled_property_); + info->orientation_property_->setReadOnly(true); + + info->rel_position_property_ = new VectorProperty( + "Relative Position", Ogre::Vector3::ZERO, + "Position of this frame, relative to it's parent frame. (Not editable)", + info->enabled_property_); + info->rel_position_property_->setReadOnly(true); + + info->rel_orientation_property_ = new QuaternionProperty( + "Relative Orientation", + Ogre::Quaternion::IDENTITY, + "Orientation of this frame, relative to it's parent frame. (Not editable)", + info->enabled_property_); + info->rel_orientation_property_->setReadOnly(true); + + // If the current frame was specified as disabled in the config file + // then its enabled state must be updated accordingly + if (frame_config_enabled_state_.count(frame) > 0 && !frame_config_enabled_state_[frame]) { + info->enabled_property_->setBool(false); + } + + updateFrame(info); + + return info; +} + +void TFDisplay::updateFrame(FrameInfo * frame) +{ + std::shared_ptr tf_buffer = context_->getFrameManager()->getTFBufferPtr(); + + // Check last received time so we can grey out/fade out frames that have stopped being published + tf2::TimePoint latest_time; + + std::string stripped_fixed_frame = fixed_frame_.toStdString(); + if (stripped_fixed_frame[0] == '/') { + stripped_fixed_frame = stripped_fixed_frame.substr(1); + } + try { + tf_buffer->_getLatestCommonTime( + tf_buffer->_validateFrameId("get_latest_common_time", stripped_fixed_frame), + tf_buffer->_validateFrameId("get_latest_common_time", frame->name_), + latest_time, + nullptr); + } catch (const tf2::LookupException & e) { + logTransformationException(stripped_fixed_frame, frame->name_, e.what()); + return; + } + + frame->setLastUpdate(latest_time); + + double age = tf2::durationToSec(tf2::get_now() - frame->last_update_); + double frame_timeout = frame_timeout_property_->getFloat(); + if (age > frame_timeout) { + frame->setVisible(false); + return; + } + frame->updateColorForAge(age, frame_timeout); + + setStatusStd(StatusProperty::Ok, frame->name_, "Transform OK"); + + Ogre::Vector3 position(0, 0, 0); + Ogre::Quaternion orientation(1.0, 0.0, 0.0, 0.0); + if (!context_->getFrameManager()->getTransform(frame->name_, position, orientation)) { + rviz_common::UniformStringStream ss; + ss << "No transform from [" << frame->name_ << "] to [" << fixed_frame_.toStdString() << "]"; + setStatusStd(StatusProperty::Warn, frame->name_, ss.str()); + frame->setVisible(false); + return; + } + + frame->updatePositionAndOrientation(position, orientation, scale_property_->getFloat()); + frame->setNamesVisible(show_names_property_->getBool()); + frame->setAxesVisible(show_axes_property_->getBool()); + + std::string old_parent = frame->parent_; + frame->parent_.clear(); + bool has_parent = tf_buffer->_getParent(frame->name_, tf2::TimePointZero, frame->parent_); + if (has_parent) { + if (hasNoTreePropertyOrParentChanged(frame, old_parent)) { + updateParentTreeProperty(frame); + } + + updateRelativePositionAndOrientation(frame, tf_buffer); + + if (show_arrows_property_->getBool()) { + updateParentArrowIfTransformExists(frame, position); + } else { + frame->setParentArrowVisible(false); + } + } else { + if (hasNoTreePropertyOrParentChanged(frame, old_parent)) { + frame->updateTreeProperty(tree_category_); + } + + frame->setParentArrowVisible(false); + } + + frame->parent_property_->setStdString(frame->parent_); + frame->selection_handler_->setParentName(frame->parent_); +} + +void TFDisplay::updateParentTreeProperty(FrameInfo * frame) const +{ + // Look up the new parent. + auto parent_it = frames_.find(frame->parent_); + if (parent_it != frames_.end()) { + FrameInfo * parent = parent_it->second; + + // If the parent has a tree property, make a new tree property for this frame. + if (parent->tree_property_) { + frame->updateTreeProperty(parent->tree_property_); + } + } +} + +/// If this frame has no tree property or the parent has changed, +bool TFDisplay::hasNoTreePropertyOrParentChanged( + const FrameInfo * frame, + const std::string & old_parent) const +{ + return !frame->tree_property_ || old_parent != frame->parent_; +} + +void TFDisplay::logTransformationException( + const std::string & parent_frame, + const std::string & child_frame, + const std::string & message) const +{ + RVIZ_COMMON_LOG_DEBUG_STREAM( + "Error transforming from frame '" << parent_frame.c_str() << + "' to frame '" << child_frame.c_str() << + "' with fixed frame '" << qPrintable(fixed_frame_) << "': " << message); +} + +/// set the position/orientation relative to the parent frame +void TFDisplay::updateRelativePositionAndOrientation( + const FrameInfo * frame, + std::shared_ptr tf_buffer) const +{ + geometry_msgs::msg::TransformStamped transform; + transform.transform.translation = geometry_msgs::msg::Vector3(); + transform.transform.rotation = geometry_msgs::msg::Quaternion(); + + try { + transform = tf_buffer->lookupTransform(frame->parent_, frame->name_, tf2::TimePointZero); + } catch (const tf2::LookupException & e) { + logTransformationException(frame->parent_, frame->name_, e.what()); + } catch (const tf2::TransformException & e) { + logTransformationException(frame->parent_, frame->name_, e.what()); + } + + Ogre::Vector3 relative_position( + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z + ); + Ogre::Quaternion relative_orientation( + transform.transform.rotation.w, + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z); + frame->rel_position_property_->setVector(relative_position); + frame->rel_orientation_property_->setQuaternion(relative_orientation); +} + +void TFDisplay::updateParentArrowIfTransformExists( + FrameInfo * frame, + const Ogre::Vector3 & position) const +{ + Ogre::Vector3 parent_position(0, 0, 0); + Ogre::Quaternion parent_orientation(1.0f, 0.0f, 0.0f, 0.0f); + if (!context_->getFrameManager()->getTransform( + frame->parent_, parent_position, parent_orientation)) + { + logTransformationException(frame->parent_, frame->name_); + } else { + frame->updateParentArrow(position, parent_position, scale_property_->getFloat()); + frame->setParentArrowVisible(show_arrows_property_->getBool()); + } +} + + +void TFDisplay::deleteFrame(FrameInfo * frame, bool delete_properties) +{ + auto it = frames_.find(frame->name_); + assert(it != frames_.end()); + + frames_.erase(it); + + delete frame->axes_; + context_->getSelectionManager()->removeObject(frame->axes_coll_); + delete frame->parent_arrow_; + delete frame->name_text_; + scene_manager_->destroySceneNode(frame->name_node_->getName()); + if (delete_properties) { + delete frame->enabled_property_; + delete frame->tree_property_; + } + delete frame; +} + +void TFDisplay::fixedFrameChanged() +{ + update_timer_ = update_rate_property_->getFloat(); +} + +void TFDisplay::reset() +{ + rviz_common::Display::reset(); + clear(); +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::TFDisplay, rviz_common::Display) diff --git a/rviz_common/src/rviz_common/temp/default_plugins/displays/tf_display.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.hpp similarity index 57% rename from rviz_common/src/rviz_common/temp/default_plugins/displays/tf_display.hpp rename to rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.hpp index 901996816..a25d0834a 100644 --- a/rviz_common/src/rviz_common/temp/default_plugins/displays/tf_display.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/tf_display.hpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,17 +28,21 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RVIZ_COMMON__TEMP__DEFAULT_PLUGINS__DISPLAYS__TF_DISPLAY_HPP_ -#define RVIZ_COMMON__TEMP__DEFAULT_PLUGINS__DISPLAYS__TF_DISPLAY_HPP_ +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__TF_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__TF_DISPLAY_HPP_ #include #include #include #include +#include #include #include +#include "geometry_msgs/msg/transform_stamped__struct.hpp" +#include "tf2/exceptions.h" +#include "tf2/buffer_core.h" #include "tf2/time.h" #include "rviz_common/selection/forwards.hpp" @@ -65,29 +70,38 @@ class FloatProperty; class QuaternionProperty; class StringProperty; class VectorProperty; -} +} // namespace properties +} // namespace rviz_common + +namespace rviz_default_plugins +{ +namespace displays +{ class FrameInfo; + class FrameSelectionHandler; + typedef std::shared_ptr FrameSelectionHandlerPtr; +typedef std::set S_FrameInfo; /** @brief Displays a visual representation of the TF hierarchy. */ -class TFDisplay : public Display +class TFDisplay : public rviz_common::Display { Q_OBJECT public: TFDisplay(); - virtual ~TFDisplay(); - virtual void update(float wall_dt, float ros_dt); + ~TFDisplay() override; + + void update(float wall_dt, float ros_dt) override; protected: - // Overrides from Display - virtual void onInitialize(); - virtual void load(const Config & config); - virtual void fixedFrameChanged(); - virtual void reset(); + void onInitialize() override; + void load(const rviz_common::Config & config) override; + void fixedFrameChanged() override; + void reset() override; private Q_SLOTS: void updateShowAxes(); @@ -100,14 +114,11 @@ private Q_SLOTS: FrameInfo * createFrame(const std::string & frame); void updateFrame(FrameInfo * frame); void deleteFrame(FrameInfo * frame, bool delete_properties); - FrameInfo * getFrameInfo(const std::string & frame); - void clear(); - // overrides from Display - virtual void onEnable(); - virtual void onDisable(); + void onEnable() override; + void onDisable() override; Ogre::SceneNode * root_node_; Ogre::SceneNode * names_node_; @@ -122,69 +133,41 @@ private Q_SLOTS: float update_timer_; - BoolProperty * show_names_property_; - BoolProperty * show_arrows_property_; - BoolProperty * show_axes_property_; - properties::FloatProperty * update_rate_property_; - properties::FloatProperty * frame_timeout_property_; - BoolProperty * all_enabled_property_; + rviz_common::properties::BoolProperty * show_names_property_; + rviz_common::properties::BoolProperty * show_arrows_property_; + rviz_common::properties::BoolProperty * show_axes_property_; + rviz_common::properties::FloatProperty * update_rate_property_; + rviz_common::properties::FloatProperty * frame_timeout_property_; + rviz_common::properties::BoolProperty * all_enabled_property_; - properties::FloatProperty * scale_property_; + rviz_common::properties::FloatProperty * scale_property_; - Property * frames_category_; - Property * tree_category_; + rviz_common::properties::Property * frames_category_; + rviz_common::properties::Property * tree_category_; bool changing_single_frame_enabled_state_; - friend class FrameInfo; -}; -/** @brief Internal class needed only by TFDisplay. */ -class FrameInfo : public QObject -{ - Q_OBJECT + void updateRelativePositionAndOrientation( + const FrameInfo * frame, std::shared_ptr tf_buffer) const; -public: - explicit FrameInfo(TFDisplay * display); + void logTransformationException( + const std::string & parent_frame, + const std::string & child_frame, + const std::string & message = "") const; - /** @brief Set this frame to be visible or invisible. */ - void setEnabled(bool enabled); + void updateParentArrowIfTransformExists(FrameInfo * frame, const Ogre::Vector3 & position) const; -public Q_SLOTS: - /** @brief Update whether the frame is visible or not, based on the enabled_property_ - * in this FrameInfo. */ - void updateVisibilityFromFrame(); + bool hasNoTreePropertyOrParentChanged( + const FrameInfo * frame, const std::string & old_parent) const; + void updateParentTreeProperty(FrameInfo * frame) const; - /** @brief Update whether the frame is visible or not, based on the enabled_property_ - * in the selection handler. */ - void updateVisibilityFromSelection(); + void deleteObsoleteFrames(std::set & current_frames); + S_FrameInfo createOrUpdateFrames(const std::vector & frames); -public: - TFDisplay * display_; - std::string name_; - std::string parent_; - rviz_rendering::Axes * axes_; - selection::CollObjectHandle axes_coll_; - FrameSelectionHandlerPtr selection_handler_; - rviz_rendering::Arrow * parent_arrow_; - rviz_rendering::MovableText * name_text_; - Ogre::SceneNode * name_node_; - - float distance_to_parent_; - Ogre::Quaternion arrow_orientation_; - - tf2::TimePoint last_update_; - tf2::TimePoint last_time_to_fixed_; - - properties::VectorProperty * rel_position_property_; - properties::QuaternionProperty * rel_orientation_property_; - properties::VectorProperty * position_property_; - properties::QuaternionProperty * orientation_property_; - properties::StringProperty * parent_property_; - properties::BoolProperty * enabled_property_; - - properties::Property * tree_property_; + friend class FrameInfo; }; -} // end namespace rviz_common +} // namespace displays +} // namespace rviz_default_plugins -#endif // RVIZ_COMMON__TEMP__DEFAULT_PLUGINS__DISPLAYS__TF_DISPLAY_HPP_ +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__TF__TF_DISPLAY_HPP_ diff --git a/rviz_rendering/CMakeLists.txt b/rviz_rendering/CMakeLists.txt index 8634037d8..b2f79ecdf 100644 --- a/rviz_rendering/CMakeLists.txt +++ b/rviz_rendering/CMakeLists.txt @@ -210,6 +210,18 @@ if(BUILD_TESTING) # Qt5::Widgets # explicitly do this for include directories (not necessary for external use) # ) # endif() + + # TODO(wjwwood): reenable this test when it can be run on CI + # ament_add_gtest(movable_text_test_target test/movable_text_test.cpp + # APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_INSTALL_PREFIX} PATH=${CMAKE_INSTALL_PREFIX}/opt/rviz_ogre_vendor/bin) + # if(TARGET movable_text_test_target) + # target_link_libraries(movable_text_test_target + # rviz_ogre_vendor::OgreMain + # rviz_ogre_vendor::OgreOverlay + # rviz_rendering + # Qt5::Widgets + # ) + # endif() endif() list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS diff --git a/rviz_rendering/include/rviz_rendering/movable_text.hpp b/rviz_rendering/include/rviz_rendering/movable_text.hpp index 51acf3dba..c2c38649a 100644 --- a/rviz_rendering/include/rviz_rendering/movable_text.hpp +++ b/rviz_rendering/include/rviz_rendering/movable_text.hpp @@ -1,6 +1,7 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -28,15 +29,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -// TODO(wjwwood): revist style of this file. - // Adapted from: http://www.ogre3d.org/wiki/index.php/MovableText // now: http://www.ogre3d.org/tikiwiki/tiki-index.php?page=MovableText // Original authors: /* * File: MovableText.h * - * description: This create create a billboarding object that display a text. + * description: This creates a billboarding object that displays a text. * * @author 2003 by cTh see gavocanov@rambler.ru * @update 2006 by barraq see nospam@barraquand.com @@ -54,6 +53,7 @@ #include #include #include +#include #include #ifndef _WIN32 @@ -72,10 +72,8 @@ class RenderQueue; namespace rviz_rendering { -class RVIZ_RENDERING_LOCAL MovableText : public Ogre::MovableObject, public Ogre::Renderable +class MovableText : public Ogre::SimpleRenderable { - /******************************** MovableText data ****************************/ - public: enum RVIZ_RENDERING_PUBLIC HorizontalAlignment { @@ -86,169 +84,187 @@ class RVIZ_RENDERING_LOCAL MovableText : public Ogre::MovableObject, public Ogre V_BELOW, V_ABOVE, V_CENTER }; -protected: - Ogre::String mFontName; - Ogre::String mType; - Ogre::String mName; - Ogre::String mCaption; - HorizontalAlignment mHorizontalAlignment; - VerticalAlignment mVerticalAlignment; - - Ogre::ColourValue mColor; - Ogre::RenderOperation mRenderOp; - Ogre::AxisAlignedBox mAABB; - Ogre::LightList mLList; - - Ogre::Real mCharHeight; - Ogre::Real mLineSpacing; - Ogre::Real mSpaceWidth; - - bool mNeedUpdate; - bool mUpdateColors; - bool mOnTop; - - Ogre::Real mTimeUntilNextToggle; - Ogre::Real mRadius; - - Ogre::Vector3 mGlobalTranslation; - Ogre::Vector3 mLocalTranslation; - - Ogre::Camera * mpCam; - Ogre::RenderWindow * mpWin; - Ogre::Font * mpFont; - Ogre::MaterialPtr mpMaterial; - Ogre::MaterialPtr mpBackgroundMaterial; - - /******************************** public methods ******************************/ - public: RVIZ_RENDERING_PUBLIC - MovableText( + explicit MovableText( const Ogre::String & caption, const Ogre::String & fontName = "Liberation Sans", Ogre::Real charHeight = 1.0, const Ogre::ColourValue & color = Ogre::ColourValue::White); RVIZ_RENDERING_PUBLIC - virtual ~MovableText(); + ~MovableText() override; -#if (OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 6) RVIZ_RENDERING_PUBLIC - virtual void visitRenderables(Ogre::Renderable::Visitor * visitor, bool debugRenderables = false); -#endif + void setFontName(const Ogre::String & font_name); - // Set settings - RVIZ_RENDERING_PUBLIC - void setFontName(const Ogre::String & fontName); RVIZ_RENDERING_PUBLIC void setCaption(const Ogre::String & caption); + RVIZ_RENDERING_PUBLIC void setColor(const Ogre::ColourValue & color); + RVIZ_RENDERING_PUBLIC void setCharacterHeight(Ogre::Real height); + RVIZ_RENDERING_PUBLIC void setLineSpacing(Ogre::Real height); + RVIZ_RENDERING_PUBLIC void setSpaceWidth(Ogre::Real width); + RVIZ_RENDERING_PUBLIC void setTextAlignment( - const HorizontalAlignment & horizontalAlignment, - const VerticalAlignment & verticalAlignment); + const HorizontalAlignment & horizontal_alignment, + const VerticalAlignment & vertical_alignment); + RVIZ_RENDERING_PUBLIC - void setGlobalTranslation(Ogre::Vector3 trans); + void setGlobalTranslation(Ogre::Vector3 translation); + RVIZ_RENDERING_PUBLIC - void setLocalTranslation(Ogre::Vector3 trans); + void setLocalTranslation(Ogre::Vector3 translation); + RVIZ_RENDERING_PUBLIC void showOnTop(bool show = true); - // Get settings + RVIZ_RENDERING_PUBLIC const Ogre::String & getFontName() const { - return mFontName; + return font_name_; } + + RVIZ_RENDERING_PUBLIC const Ogre::String & getCaption() const { - return mCaption; + return caption_; } + + RVIZ_RENDERING_PUBLIC const Ogre::ColourValue & getColor() const { - return mColor; + return color_; } + RVIZ_RENDERING_PUBLIC Ogre::Real getCharacterHeight() const { - return mCharHeight; + return char_height_; } + + RVIZ_RENDERING_PUBLIC Ogre::Real getSpaceWidth() const { - return mSpaceWidth; + return space_width_; } + + RVIZ_RENDERING_PUBLIC Ogre::Vector3 getGlobalTranslation() const { - return mGlobalTranslation; + return global_translation_; } + + RVIZ_RENDERING_PUBLIC Ogre::Vector3 getLocalTranslation() const { - return mLocalTranslation; + return local_translation_; } + + RVIZ_RENDERING_PUBLIC bool getShowOnTop() const { - return mOnTop; + return on_top_; } - Ogre::AxisAlignedBox GetAABB(void) + + RVIZ_RENDERING_PUBLIC + const Ogre::AxisAlignedBox & getBoundingBox() const override { - return mAABB; + return mBox; } - const Ogre::MaterialPtr & getMaterial(void) const + RVIZ_RENDERING_PUBLIC + Ogre::Real getBoundingRadius() const override { - assert(mpMaterial); - return mpMaterial; + return radius_; } + RVIZ_RENDERING_PUBLIC + const Ogre::MaterialPtr & getMaterial() const override + { + assert(material_); + return material_; + } - /******************************** protected methods and overload **************/ + RVIZ_RENDERING_PUBLIC + void + visitRenderables(Ogre::Renderable::Visitor * visitor, bool debug_renderables) override; -protected: - // from MovableText, create the object - void _setupGeometry(); - void _updateColors(); + RVIZ_RENDERING_PUBLIC + void update(); - // from Ogre::MovableObject - void getWorldTransforms(Ogre::Matrix4 * xform) const; - Ogre::Real getBoundingRadius(void) const - { - return mRadius; - } - Ogre::Real getSquaredViewDepth(const Ogre::Camera * cam) const +protected: + void setupGeometry(); + unsigned int calculateVertexCount() const; + void setupRenderOperation(); + Ogre::HardwareVertexBufferSharedPtr setupHardwareBuffers() const; + void calculateTotalDimensionsForPositioning(float & total_height, float & total_width) const; + float getVerticalStartFromVerticalAlignment(float total_height) const; + float getLineStartFromHorizontalAlignment(float total_width) const; + void fillVertexBuffer( + Ogre::HardwareVertexBufferSharedPtr & position_and_texture_buffer, + float top, float starting_left); + + void updateColors(); + + void getWorldTransforms(Ogre::Matrix4 * xform) const override; + Ogre::Real getSquaredViewDepth(const Ogre::Camera * cam) const override { (void) cam; return 0; } - const Ogre::Quaternion & getWorldOrientation(void) const; - const Ogre::Vector3 & getWorldPosition(void) const; - const Ogre::AxisAlignedBox & getBoundingBox(void) const - { - return mAABB; - } - const Ogre::String & getName(void) const - { - return mName; - } - const Ogre::String & getMovableType(void) const + const Ogre::Quaternion & getWorldOrientation() const; + const Ogre::Vector3 & getWorldPosition() const; + const Ogre::String & getMovableType() const override { static Ogre::String movType = "MovableText"; return movType; } - void _notifyCurrentCamera(Ogre::Camera * cam); - void _updateRenderQueue(Ogre::RenderQueue * queue); + void _notifyCurrentCamera(Ogre::Camera * camera) override; + void _updateRenderQueue(Ogre::RenderQueue * queue) override; - // from renderable - void getRenderOperation(Ogre::RenderOperation & op); - const Ogre::LightList & getLights(void) const + const Ogre::LightList & getLights() const override { - return mLList; + return light_list_; } + + void fillColorBuffer(Ogre::RGBA color) const; + + void getRenderOperation(Ogre::RenderOperation & op) override; + +private: + Ogre::String font_name_; + Ogre::String name_; + Ogre::String caption_; + HorizontalAlignment horizontal_alignment_; + VerticalAlignment vertical_alignment_; + + Ogre::ColourValue color_; + Ogre::Real radius_; + + Ogre::Real char_height_; + Ogre::Real line_spacing_; + Ogre::Real space_width_; + + bool needs_update_; + bool needs_color_update_; + bool on_top_; + + Ogre::Vector3 global_translation_; + Ogre::Vector3 local_translation_; + + Ogre::Font * font_; + Ogre::MaterialPtr material_; + + Ogre::LightList light_list_; }; } // namespace rviz_rendering diff --git a/rviz_rendering/src/rviz_rendering/movable_text.cpp b/rviz_rendering/src/rviz_rendering/movable_text.cpp index 5964793db..6ef59eb42 100644 --- a/rviz_rendering/src/rviz_rendering/movable_text.cpp +++ b/rviz_rendering/src/rviz_rendering/movable_text.cpp @@ -1,6 +1,7 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -28,15 +29,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -// TODO(wjwwood): revist style of this file. - // Adapted from: http://www.ogre3d.org/wiki/index.php/MovableText // now: http://www.ogre3d.org/tikiwiki/tiki-index.php?page=MovableText // Original authors: /* * File: MovableText.cpp * - * description: This create create a billboarding object that display a text. + * description: This creates a billboarding object that displays a text. * * @author 2003 by cTh see gavocanov@rambler.ru * @update 2006 by barraq see nospam@barraquand.com @@ -44,7 +43,10 @@ #include "rviz_rendering/movable_text.hpp" +#include +#include #include +#include #ifndef _WIN32 # pragma GCC diagnostic push @@ -64,17 +66,12 @@ #include #include -#include -#include - #ifndef _WIN32 # pragma GCC diagnostic pop #else # pragma warning(pop) #endif -using namespace Ogre; // NOLINT - #define POS_TEX_BINDING 0 #define COLOUR_BINDING 1 @@ -83,538 +80,496 @@ using namespace Ogre; // NOLINT namespace rviz_rendering { +const float effective_char_height_factor = 2.0f; + MovableText::MovableText( - const String & caption, const String & fontName, Real charHeight, - const ColourValue & color) -: mFontName(fontName), - mType("MovableText"), - mCaption(caption), - mHorizontalAlignment(H_LEFT), - mVerticalAlignment(V_BELOW), - mColor(color), - mCharHeight(charHeight), - mLineSpacing(0.01f), - mSpaceWidth(0), - mUpdateColors(true), - mOnTop(false), - mTimeUntilNextToggle(0), - mGlobalTranslation(0.0f), - mLocalTranslation(0.0f), - mpCam(NULL), - mpWin(NULL), - mpFont(NULL) + const Ogre::String & caption, + const Ogre::String & fontName, + Ogre::Real charHeight, + const Ogre::ColourValue & color) +: font_name_(fontName), + caption_(caption), + horizontal_alignment_(H_LEFT), + vertical_alignment_(V_BELOW), + color_(color), + char_height_(charHeight), + line_spacing_(0.01f), + space_width_(0), + needs_color_update_(true), + on_top_(false), + global_translation_(0.0f), + local_translation_(0.0f), + font_(nullptr) { static int count = 0; std::stringstream ss; ss << "MovableText" << count++; - mName = ss.str(); + name_ = ss.str(); - mRenderOp.vertexData = NULL; - this->setFontName(mFontName); - this->_setupGeometry(); + mRenderOp.vertexData = nullptr; + this->setFontName(font_name_); + // Set a reasonable default space width + space_width_ = font_->getGlyphAspectRatio('A') * char_height_ * effective_char_height_factor; + this->setupGeometry(); } MovableText::~MovableText() { - if (mRenderOp.vertexData) { - delete mRenderOp.vertexData; - } - // May cause crashing... check this and comment if it does - if (mpMaterial) { - MaterialManager::getSingletonPtr()->remove(mpMaterial->getName(), MATERIAL_GROUP); + delete mRenderOp.vertexData; + if (material_) { + Ogre::MaterialManager::getSingletonPtr()->remove(material_->getName(), MATERIAL_GROUP); } } -void MovableText::setFontName(const String & fontName) +void MovableText::setFontName(const Ogre::String & font_name) { - if (Ogre::MaterialManager::getSingletonPtr()->resourceExists(mName + "Material", - MATERIAL_GROUP)) + if (Ogre::MaterialManager::getSingletonPtr() + ->resourceExists(name_ + "Material", MATERIAL_GROUP)) { - Ogre::MaterialManager::getSingleton().remove(mName + "Material", MATERIAL_GROUP); + Ogre::MaterialManager::getSingleton().remove(name_ + "Material", MATERIAL_GROUP); } - if (mFontName != fontName || !mpMaterial || !mpFont) { - mFontName = fontName; - mpFont = - reinterpret_cast(FontManager::getSingleton().getByName(mFontName, - MATERIAL_GROUP).get()); - if (!mpFont) { - throw Exception(Exception::ERR_ITEM_NOT_FOUND, "Could not find font " + - fontName, "MovableText::setFontName"); + if (font_name_ != font_name || !material_ || !font_) { + font_name_ = font_name; + font_ = Ogre::FontManager::getSingleton().getByName(font_name_, MATERIAL_GROUP).get(); + if (!font_) { + throw Ogre::Exception(Ogre::Exception::ERR_ITEM_NOT_FOUND, "Could not find font " + + font_name, "MovableText::setFontName"); } - mpFont->load(); - if (mpMaterial) { - MaterialManager::getSingletonPtr()->remove(mpMaterial->getName(), MATERIAL_GROUP); - mpMaterial.reset(); + font_->load(); + if (material_) { + Ogre::MaterialManager::getSingletonPtr()->remove(material_->getName(), MATERIAL_GROUP); + material_.reset(); } - mpMaterial = mpFont->getMaterial()->clone(mName + "Material"); - if (!mpMaterial->isLoaded()) { - mpMaterial->load(); + material_ = font_->getMaterial()->clone(name_ + "Material"); + if (!material_->isLoaded()) { + material_->load(); } - mpMaterial->setDepthCheckEnabled(!mOnTop); - mpMaterial->setDepthBias(1.0, 1.0); - mpMaterial->setDepthWriteEnabled(mOnTop); - mpMaterial->setLightingEnabled(false); - mNeedUpdate = true; + material_->setDepthCheckEnabled(!on_top_); + material_->setDepthBias(1.0, 1.0); + material_->setDepthWriteEnabled(on_top_); + material_->setLightingEnabled(false); + needs_update_ = true; } } -void MovableText::setCaption(const String & caption) +void MovableText::setCaption(const Ogre::String & caption) { - if (caption != mCaption) { - mCaption = caption; - mNeedUpdate = true; + if (caption != caption_) { + caption_ = caption; + needs_update_ = true; } } -void MovableText::setColor(const ColourValue & color) +void MovableText::setColor(const Ogre::ColourValue & color) { - if (color != mColor) { - mColor = color; - mUpdateColors = true; + if (color != color_) { + color_ = color; + needs_color_update_ = true; } } -void MovableText::setCharacterHeight(Real height) +void MovableText::setCharacterHeight(Ogre::Real height) { - if (height != mCharHeight) { - mCharHeight = height; - mNeedUpdate = true; + if (height != char_height_) { + char_height_ = height; + needs_update_ = true; } } -void MovableText::setLineSpacing(Real height) +void MovableText::setLineSpacing(Ogre::Real height) { - if (height != mLineSpacing) { - mLineSpacing = height; - mNeedUpdate = true; + if (height != line_spacing_) { + line_spacing_ = height; + needs_update_ = true; } } -void MovableText::setSpaceWidth(Real width) +void MovableText::setSpaceWidth(Ogre::Real width) { - if (width != mSpaceWidth) { - mSpaceWidth = width; - mNeedUpdate = true; + if (width != space_width_) { + space_width_ = width; + needs_update_ = true; } } void MovableText::setTextAlignment( - const HorizontalAlignment & horizontalAlignment, - const VerticalAlignment & verticalAlignment) + const HorizontalAlignment & horizontal_alignment, + const VerticalAlignment & vertical_alignment) { - if (mHorizontalAlignment != horizontalAlignment) { - mHorizontalAlignment = horizontalAlignment; - mNeedUpdate = true; + if (horizontal_alignment_ != horizontal_alignment) { + horizontal_alignment_ = horizontal_alignment; + needs_update_ = true; } - if (mVerticalAlignment != verticalAlignment) { - mVerticalAlignment = verticalAlignment; - mNeedUpdate = true; + if (vertical_alignment_ != vertical_alignment) { + vertical_alignment_ = vertical_alignment; + needs_update_ = true; } } -void MovableText::setGlobalTranslation(Vector3 trans) +void MovableText::setGlobalTranslation(Ogre::Vector3 translation) { - mGlobalTranslation = trans; + global_translation_ = translation; } -void MovableText::setLocalTranslation(Vector3 trans) +void MovableText::setLocalTranslation(Ogre::Vector3 translation) { - mLocalTranslation = trans; + local_translation_ = translation; } void MovableText::showOnTop(bool show) { - if (mOnTop != show && mpMaterial) { - mOnTop = show; - mpMaterial->setDepthBias(1.0, 1.0); - mpMaterial->setDepthCheckEnabled(!mOnTop); - mpMaterial->setDepthWriteEnabled(mOnTop); + if (on_top_ != show && material_) { + on_top_ = show; + material_->setDepthBias(1.0, 1.0); + material_->setDepthCheckEnabled(!on_top_); + material_->setDepthWriteEnabled(on_top_); } } -void MovableText::_setupGeometry() +struct TextBuffer { - assert(mpFont); - assert(mpMaterial); + float * buffer_; + Ogre::Vector3 min_; + Ogre::Vector3 max_; + Ogre::Real max_squared_radius_; + float top_; + float left_; + Ogre::Font::UVRect text_coords_; + + explicit TextBuffer(float * buffer) + : buffer_(buffer), + min_(std::numeric_limits::max()), + max_(std::numeric_limits::min()), + max_squared_radius_(0), + top_(0), + left_(0) + { + } - unsigned int vertexCount = 0; + void addTopLeft() + { + addPositionToBuffer(0, 0); + addTextureToBuffer(text_coords_.left, text_coords_.top); + } - // count letters to determine how many vertices are needed - std::string::iterator i = mCaption.begin(); - std::string::iterator iend = mCaption.end(); - for (; i != iend; ++i) { - if ((*i != ' ') && (*i != '\n')) { - vertexCount += 6; - } + void addBottomLeft(float char_height) + { + addPositionToBuffer(0, 2.0f * char_height); + addTextureToBuffer(text_coords_.left, text_coords_.bottom); } - if (mRenderOp.vertexData) { - delete mRenderOp.vertexData; - mRenderOp.vertexData = NULL; - mUpdateColors = true; + void addTopRight(float char_width) + { + addPositionToBuffer(2.0f * char_width, 0); + addTextureToBuffer(text_coords_.right, text_coords_.top); } - if (mCaption.empty()) { + void addBottomRight(float char_widht, float char_height) + { + addPositionToBuffer(2.0f * char_widht, 2.0f * char_height); + addTextureToBuffer(text_coords_.right, text_coords_.bottom); + } + +private: + void addPositionToBuffer(float plus_left, float minus_top) + { + Ogre::Vector3 current_position = Ogre::Vector3(left_ + plus_left, top_ - minus_top, 0.0); + * buffer_++ = current_position.x; + * buffer_++ = current_position.y; + * buffer_++ = current_position.z; + + min_.makeFloor(current_position); + max_.makeCeil(current_position); + max_squared_radius_ = std::max(max_squared_radius_, current_position.squaredLength()); + } + + void addTextureToBuffer(float texture_x, float texture_y) + { + * buffer_++ = texture_x; + * buffer_++ = texture_y; + } +}; + +void MovableText::setupGeometry() +{ + assert(font_); + assert(material_); + + if (caption_.empty()) { return; } - if (!mRenderOp.vertexData) { - mRenderOp.vertexData = new VertexData(); + setupRenderOperation(); + Ogre::HardwareVertexBufferSharedPtr position_and_texture_buffer = setupHardwareBuffers(); + + float total_height; + float total_width; + calculateTotalDimensionsForPositioning(total_height, total_width); + + float starting_left = getLineStartFromHorizontalAlignment(total_width); + float starting_top = getVerticalStartFromVerticalAlignment(total_height); + + fillVertexBuffer(position_and_texture_buffer, starting_top, starting_left); + + if (needs_color_update_) { + this->updateColors(); + } + + needs_update_ = false; +} + +void MovableText::setupRenderOperation() +{ + unsigned int vertex_count = calculateVertexCount(); + + if (mRenderOp.vertexData) { + delete mRenderOp.vertexData; + mRenderOp.vertexData = nullptr; + needs_color_update_ = true; } - mRenderOp.indexData = 0; + mRenderOp.vertexData = new Ogre::VertexData(); + mRenderOp.indexData = nullptr; mRenderOp.vertexData->vertexStart = 0; - mRenderOp.vertexData->vertexCount = vertexCount; - mRenderOp.operationType = RenderOperation::OT_TRIANGLE_LIST; + mRenderOp.vertexData->vertexCount = vertex_count; + mRenderOp.operationType = Ogre::RenderOperation::OT_TRIANGLE_LIST; mRenderOp.useIndexes = false; +} - VertexDeclaration * decl = mRenderOp.vertexData->vertexDeclaration; - VertexBufferBinding * bind = mRenderOp.vertexData->vertexBufferBinding; +unsigned int MovableText::calculateVertexCount() const +{ + unsigned int vertex_count = 0; + for (auto & character : caption_) { + if ((character != ' ') && (character != '\n')) { + vertex_count += 6; + } + } + return vertex_count; +} + +Ogre::HardwareVertexBufferSharedPtr MovableText::setupHardwareBuffers() const +{ + Ogre::VertexDeclaration * declaration = mRenderOp.vertexData->vertexDeclaration; + Ogre::VertexBufferBinding * bind = mRenderOp.vertexData->vertexBufferBinding; size_t offset = 0; - // create/bind positions/tex.ccord. buffer - if (!decl->findElementBySemantic(VES_POSITION)) { - decl->addElement(POS_TEX_BINDING, offset, VET_FLOAT3, VES_POSITION); + // create/bind positions/texture coordinates buffer + if (!declaration->findElementBySemantic(Ogre::VES_POSITION)) { + declaration->addElement(POS_TEX_BINDING, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION); } - offset += VertexElement::getTypeSize(VET_FLOAT3); + offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3); - if (!decl->findElementBySemantic(VES_TEXTURE_COORDINATES)) { - decl->addElement(POS_TEX_BINDING, offset, Ogre::VET_FLOAT2, + if (!declaration->findElementBySemantic(Ogre::VES_TEXTURE_COORDINATES)) { + declaration->addElement(POS_TEX_BINDING, offset, Ogre::VET_FLOAT2, Ogre::VES_TEXTURE_COORDINATES, 0); } - HardwareVertexBufferSharedPtr ptbuf = - HardwareBufferManager::getSingleton().createVertexBuffer( - decl->getVertexSize(POS_TEX_BINDING), + Ogre::HardwareVertexBufferSharedPtr position_and_texture_buffer = + Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( + declaration->getVertexSize(POS_TEX_BINDING), mRenderOp.vertexData->vertexCount, - HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY); - bind->setBinding(POS_TEX_BINDING, ptbuf); + Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY); + bind->setBinding(POS_TEX_BINDING, position_and_texture_buffer); // Colours - store these in a separate buffer because they change less often - if (!decl->findElementBySemantic(VES_DIFFUSE)) { - decl->addElement(COLOUR_BINDING, 0, VET_COLOUR, VES_DIFFUSE); + if (!declaration->findElementBySemantic(Ogre::VES_DIFFUSE)) { + declaration->addElement(COLOUR_BINDING, 0, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE); } - HardwareVertexBufferSharedPtr cbuf = - HardwareBufferManager::getSingleton().createVertexBuffer( - decl->getVertexSize(COLOUR_BINDING), + Ogre::HardwareVertexBufferSharedPtr color_buffer = + Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( + declaration->getVertexSize(COLOUR_BINDING), mRenderOp.vertexData->vertexCount, - HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY); - bind->setBinding(COLOUR_BINDING, cbuf); - - float * pPCBuff = - static_cast(ptbuf->lock(HardwareBuffer::HBL_DISCARD)); + Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY); + bind->setBinding(COLOUR_BINDING, color_buffer); + return position_and_texture_buffer; +} - Real spaceWidth = mSpaceWidth; - // Derive space width from a capital A - if (spaceWidth == 0) { - spaceWidth = mpFont->getGlyphAspectRatio('A') * mCharHeight * 2.0f; - } +void +MovableText::calculateTotalDimensionsForPositioning(float & total_height, float & total_width) const +{ + Ogre::Real effective_char_height = char_height_ * effective_char_height_factor; - float total_height = mCharHeight; - float total_width = 0.0f; + total_height = effective_char_height; + total_width = 0.0f; float current_width = 0.0f; - i = mCaption.begin(); - iend = mCaption.end(); - for (; i != iend; ++i) { - if (*i == '\n') { - total_height += mCharHeight + 0.01f; - - if (current_width > total_width) { - total_width = current_width; - current_width = 0.0f; - } + for (auto & character : caption_) { + if (character == '\n') { + total_height += effective_char_height + line_spacing_; + total_width = current_width > total_width ? current_width : total_width; + } else if (character == ' ') { + current_width += space_width_; } else { - current_width += mpFont->getGlyphAspectRatio(*i) * mCharHeight * 2.0f; + current_width += font_->getGlyphAspectRatio(character) * effective_char_height; } } + total_width = current_width > total_width ? current_width : total_width; +} - if (current_width > total_width) { - total_width = current_width; - } - - float top = 0.0f; - switch (mVerticalAlignment) { - case MovableText::V_ABOVE: - top = total_height * 2; - break; - case MovableText::V_CENTER: - top = 0.5f * total_height * 2; - break; - case MovableText::V_BELOW: - top = 0.0f; - break; - } - - float starting_left = 0.0f; - switch (mHorizontalAlignment) { - case MovableText::H_LEFT: - starting_left = 0.0f; - break; - case MovableText::H_CENTER: - starting_left = -total_width / 2.0f; - break; - } - - float left = starting_left; - - bool newLine = true; - Real len = 0.0f; - // for calculation of AABB - Ogre::Vector3 min(9999999.0f), max(-9999999.0f), currPos(0.0f); - Ogre::Real maxSquaredRadius = -99999999.0f; - float largestWidth = 0.0f; - for (i = mCaption.begin(); i != iend; ++i) { - if (newLine) { - len = 0.0f; - for (String::iterator j = i; j != iend && *j != '\n'; j++) { - if (*j == ' ') { - len += spaceWidth; - } else { - len += mpFont->getGlyphAspectRatio(*j) * mCharHeight * 2.0f; - } - } - newLine = false; - } +float MovableText::getVerticalStartFromVerticalAlignment(float total_height) const +{ + switch (vertical_alignment_) { + case V_ABOVE: + return total_height; + case V_CENTER: + return 0.5f * total_height; + case V_BELOW: + return 0.0f; + default: + throw std::runtime_error("unexpected vertical alignment"); + } +} - if (*i == '\n') { - left = starting_left; - top -= mCharHeight * 2.0f; - newLine = true; +float MovableText::getLineStartFromHorizontalAlignment(float total_width) const +{ + switch (horizontal_alignment_) { + case H_LEFT: + return 0.0f; + case H_CENTER: + return -0.5f * total_width; + default: + throw std::runtime_error("unexpected horizontal alignment"); + } +} + +void MovableText::fillVertexBuffer( + Ogre::HardwareVertexBufferSharedPtr & position_and_texture_buffer, float top, float starting_left) +{ + Ogre::Real effective_char_height = char_height_ * effective_char_height_factor; + + auto hardware_buffer = + static_cast(position_and_texture_buffer->lock(Ogre::HardwareBuffer::HBL_DISCARD)); + + auto buffer = TextBuffer(hardware_buffer); + buffer.left_ = starting_left; + buffer.top_ = top; + + for (auto & character : caption_) { + if (character == '\n') { + buffer.left_ = starting_left; + buffer.top_ -= effective_char_height + line_spacing_; continue; } - if (*i == ' ') { - // Just leave a gap, no tris - left += spaceWidth; + if (character == ' ') { + buffer.left_ += space_width_; continue; } - Real horiz_height = mpFont->getGlyphAspectRatio(*i); - Real u1, u2, v1, v2; - Ogre::Font::UVRect utmp; - utmp = mpFont->getGlyphTexCoords(*i); - u1 = utmp.left; - u2 = utmp.right; - v1 = utmp.top; - v2 = utmp.bottom; - - // each vert is (x, y, z, u, v) - //------------------------------------------------------------------------------------- - // First tri - // - // Upper left - currPos = Ogre::Vector3(left, top, 0.0); - - *pPCBuff++ = currPos.x; - *pPCBuff++ = currPos.y; - *pPCBuff++ = currPos.z; - *pPCBuff++ = u1; - *pPCBuff++ = v1; - - // Deal with bounds - - - min.makeFloor(currPos); - max.makeCeil(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - top -= mCharHeight * 2.0f; - - // Bottom left - currPos = Ogre::Vector3(left, top, 0.0f); - *pPCBuff++ = currPos.x; - *pPCBuff++ = currPos.y; - *pPCBuff++ = currPos.z; - *pPCBuff++ = u1; - *pPCBuff++ = v2; - - // Deal with bounds - min.makeFloor(currPos); - max.makeCeil(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - top += mCharHeight * 2.0f; - left += horiz_height * mCharHeight * 2.0f; - - // Top right - currPos = Ogre::Vector3(left, top, 0.0f); - *pPCBuff++ = currPos.x; - *pPCBuff++ = currPos.y; - *pPCBuff++ = currPos.z; - *pPCBuff++ = u2; - *pPCBuff++ = v1; - //------------------------------------------------------------------------------------- - - // Deal with bounds - min.makeFloor(currPos); - max.makeCeil(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - //------------------------------------------------------------------------------------- - // Second tri - // - // Top right (again) - currPos = Ogre::Vector3(left, top, 0.0f); - *pPCBuff++ = currPos.x; - *pPCBuff++ = currPos.y; - *pPCBuff++ = currPos.z; - *pPCBuff++ = u2; - *pPCBuff++ = v1; - - min.makeFloor(currPos); - max.makeCeil(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - top -= mCharHeight * 2.0f; - left -= horiz_height * mCharHeight * 2.0f; - - // Bottom left (again) - currPos = Ogre::Vector3(left, top, 0.0f); - *pPCBuff++ = currPos.x; - *pPCBuff++ = currPos.y; - *pPCBuff++ = currPos.z; - *pPCBuff++ = u1; - *pPCBuff++ = v2; - - min.makeFloor(currPos); - max.makeCeil(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - left += horiz_height * mCharHeight * 2.0f; - - // Bottom right - currPos = Ogre::Vector3(left, top, 0.0f); - *pPCBuff++ = currPos.x; - *pPCBuff++ = currPos.y; - *pPCBuff++ = currPos.z; - *pPCBuff++ = u2; - *pPCBuff++ = v2; - //------------------------------------------------------------------------------------- - min.makeFloor(currPos); - max.makeCeil(currPos); - maxSquaredRadius = std::max(maxSquaredRadius, currPos.squaredLength()); - - // Go back up with top - top += mCharHeight * 2.0f; - - float currentWidth = (left + 1) / 2 - 0; - if (currentWidth > largestWidth) { - largestWidth = currentWidth; - } - } + Ogre::Real char_aspect_ratio = font_->getGlyphAspectRatio(character); + buffer.text_coords_ = font_->getGlyphTexCoords(character); + float char_width = char_aspect_ratio * char_height_; - // Unlock vertex buffer - ptbuf->unlock(); + buffer.addTopLeft(); + buffer.addBottomLeft(char_height_); + buffer.addTopRight(char_width); - // update AABB/Sphere radius - mAABB = Ogre::AxisAlignedBox(min, max); - mRadius = Ogre::Math::Sqrt(maxSquaredRadius); + buffer.addTopRight(char_width); + buffer.addBottomLeft(char_height_); + buffer.addBottomRight(char_width, char_height_); - if (mUpdateColors) { - this->_updateColors(); + buffer.left_ += char_aspect_ratio * effective_char_height; } - mNeedUpdate = false; + position_and_texture_buffer->unlock(); + + mBox = Ogre::AxisAlignedBox(buffer.min_, buffer.max_); + radius_ = Ogre::Math::Sqrt(buffer.max_squared_radius_); } -void MovableText::_updateColors(void) +void MovableText::updateColors() { - assert(mpFont); - assert(mpMaterial); + assert(font_); + assert(material_); + + Ogre::RGBA color; + Ogre::Root::getSingleton().convertColourValue(color_, &color); + fillColorBuffer(color); + needs_color_update_ = false; +} - // Convert to system-specific - RGBA color; - Root::getSingleton().convertColourValue(mColor, &color); - HardwareVertexBufferSharedPtr vbuf = +void MovableText::fillColorBuffer(Ogre::RGBA color) const +{ + Ogre::HardwareVertexBufferSharedPtr hardware_buffer = mRenderOp.vertexData->vertexBufferBinding->getBuffer(COLOUR_BINDING); - RGBA * pDest = static_cast(vbuf->lock(HardwareBuffer::HBL_DISCARD)); + auto color_buffer = static_cast( + hardware_buffer->lock(Ogre::HardwareBuffer::HBL_DISCARD)); for (int i = 0; i < static_cast(mRenderOp.vertexData->vertexCount); ++i) { - *pDest++ = color; + *color_buffer++ = color; } - vbuf->unlock(); - mUpdateColors = false; + hardware_buffer->unlock(); } -const Quaternion & MovableText::getWorldOrientation(void) const +const Ogre::Quaternion & MovableText::getWorldOrientation() const { - assert(mpCam); - return const_cast(mpCam->getDerivedOrientation()); + assert(mCamera); + return const_cast(mCamera->getDerivedOrientation()); } -#if ( (OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 6) || OGRE_VERSION_MAJOR >= 2 ) -void MovableText::visitRenderables(Ogre::Renderable::Visitor * visitor, bool debugRenderables) +void MovableText::visitRenderables(Ogre::Renderable::Visitor * visitor, bool debug_renderables) { - (void) debugRenderables; - visitor->visit(this, 0, false); + visitor->visit(this, 0, debug_renderables); } -#endif -const Vector3 & MovableText::getWorldPosition(void) const +const Ogre::Vector3 & MovableText::getWorldPosition() const { assert(mParentNode); return mParentNode->_getDerivedPosition(); } -void MovableText::getWorldTransforms(Matrix4 * xform) const +void MovableText::getWorldTransforms(Ogre::Matrix4 * xform) const { - if (this->isVisible() && mpCam) { - Matrix3 rot3x3, scale3x3 = Matrix3::IDENTITY; + if (this->isVisible() && mCamera) { + Ogre::Matrix3 rot3x3, scale3x3 = Ogre::Matrix3::IDENTITY; - // store rotation in a matrix - mpCam->getDerivedOrientation().ToRotationMatrix(rot3x3); + mCamera->getDerivedOrientation().ToRotationMatrix(rot3x3); - // parent node position - Vector3 ppos = mParentNode->_getDerivedPosition() + Vector3::UNIT_Y * - mGlobalTranslation; - ppos += rot3x3 * mLocalTranslation; + Ogre::Vector3 parent_position = mParentNode->_getDerivedPosition() + + Ogre::Vector3::UNIT_Y * global_translation_; + parent_position += rot3x3 * local_translation_; - // apply scale scale3x3[0][0] = mParentNode->_getDerivedScale().x / 2; scale3x3[1][1] = mParentNode->_getDerivedScale().y / 2; scale3x3[2][2] = mParentNode->_getDerivedScale().z / 2; - // apply all transforms to xform *xform = (rot3x3 * scale3x3); - xform->setTrans(ppos); + xform->setTrans(parent_position); } } -void MovableText::getRenderOperation(RenderOperation & op) +void MovableText::getRenderOperation(Ogre::RenderOperation & op) { - if (this->isVisible()) { - if (mNeedUpdate) { - this->_setupGeometry(); - } - if (mUpdateColors) { - this->_updateColors(); - } + if (isVisible()) { + update(); op = mRenderOp; } } -void MovableText::_notifyCurrentCamera(Camera * cam) +void MovableText::update() { - mpCam = cam; + if (needs_update_) { + setupGeometry(); + } + if (needs_color_update_) { + updateColors(); + } } -void MovableText::_updateRenderQueue(RenderQueue * queue) +void MovableText::_notifyCurrentCamera(Ogre::Camera * camera) { - if (this->isVisible()) { - if (mNeedUpdate) { - this->_setupGeometry(); - } - if (mUpdateColors) { - this->_updateColors(); - } + mCamera = camera; +} +void MovableText::_updateRenderQueue(Ogre::RenderQueue * queue) +{ + if (isVisible()) { + update(); queue->addRenderable(this, mRenderQueueID, OGRE_RENDERABLE_DEFAULT_PRIORITY); - // queue->addRenderable(this, mRenderQueueID, RENDER_QUEUE_SKIES_LATE); } } diff --git a/rviz_rendering/test/movable_text_test.cpp b/rviz_rendering/test/movable_text_test.cpp new file mode 100644 index 000000000..4883d7bb0 --- /dev/null +++ b/rviz_rendering/test/movable_text_test.cpp @@ -0,0 +1,215 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include "test/rviz_rendering/ogre_testing_environment.hpp" +#include "rviz_rendering/movable_text.hpp" + +class MovableTextTestFixture : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + testing_environment_ = std::make_shared(); + testing_environment_->setUpOgreTestEnvironment(); + } + + static std::shared_ptr testing_environment_; +}; + +std::shared_ptr +MovableTextTestFixture::testing_environment_ = nullptr; + +float getCharWidth(std::shared_ptr movable_text, char character) +{ + auto font = + Ogre::FontManager::getSingleton().getByName("Liberation Sans", "rviz_rendering").get(); + font->load(); + float char_height = movable_text->getCharacterHeight(); + float additional_space = 2.0f; + return font->getGlyphAspectRatio(character) * char_height * additional_space; +} + +void assertVector3Equality(Ogre::Vector3 actual, Ogre::Vector3 expected) +{ + ASSERT_NEAR(actual.x, expected.x, 0.0001); + ASSERT_NEAR(actual.y, expected.y, 0.0001); + ASSERT_NEAR(actual.z, expected.z, 0.0001); +} +void assertBoundingBoxEquality(Ogre::AxisAlignedBox actual, Ogre::AxisAlignedBox expected) +{ + assertVector3Equality(actual.getMaximum(), expected.getMaximum()); + assertVector3Equality(actual.getMinimum(), expected.getMinimum()); +} + +TEST_F(MovableTextTestFixture, setCaption_changes_displayed_text) { + auto movable_text = std::make_shared("TestCaption"); + + movable_text->setCaption("NewCaption"); + + ASSERT_EQ(movable_text->getCaption(), "NewCaption"); +} + +TEST_F(MovableTextTestFixture, bounding_box_for_one_character_text_depends_on_glyp_ratio) { + auto movable_text = std::make_shared("W"); + + float char_width = getCharWidth(movable_text, 'W'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, -2, 0), Ogre::Vector3(char_width, 0, 0))); +} + +TEST_F(MovableTextTestFixture, bounding_box_space_is_given_by_bounding_box_for_A) { + auto movable_text = std::make_shared("A A"); + + float char_width = getCharWidth(movable_text, 'A'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, -2, 0), Ogre::Vector3(3 * char_width, 0, 0))); +} + +TEST_F(MovableTextTestFixture, new_line_creates_a_new_line_making_bounding_box_larger) { + auto movable_text = std::make_shared("A\nA"); + + float char_width = getCharWidth(movable_text, 'A'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, -4 - 0.01f, 0), Ogre::Vector3(char_width, 0, 0))); +} + +TEST_F(MovableTextTestFixture, larger_char_height_makes_characters_wider) { + auto movable_text = std::make_shared("A", "Liberation Sans", 2.0); + + float char_width = getCharWidth(movable_text, 'A'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, -2 * 2.0f, 0), Ogre::Vector3(char_width, 0, 0))); +} + +TEST_F(MovableTextTestFixture, horizontal_alignment_center_centers_x_coordinate) { + auto movable_text = std::make_shared("W"); + + movable_text->setTextAlignment(rviz_rendering::MovableText::HorizontalAlignment::H_CENTER, + rviz_rendering::MovableText::VerticalAlignment::V_BELOW); + movable_text->update(); + + float char_width = getCharWidth(movable_text, 'W'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(-char_width / 2, -2, 0), + Ogre::Vector3(char_width / 2, 0, 0))); +} + +TEST_F(MovableTextTestFixture, vertical_alignment_center_centers_y_coordinate) { + auto movable_text = std::make_shared("W"); + + movable_text->setTextAlignment(rviz_rendering::MovableText::HorizontalAlignment::H_LEFT, + rviz_rendering::MovableText::VerticalAlignment::V_CENTER); + movable_text->update(); + + float char_width = getCharWidth(movable_text, 'W'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, -1, 0), Ogre::Vector3(char_width, 1, 0))); +} + +TEST_F(MovableTextTestFixture, vertical_alignment_above_puts_y_coordinate_above_zero) { + auto movable_text = std::make_shared("W"); + + movable_text->setTextAlignment(rviz_rendering::MovableText::HorizontalAlignment::H_LEFT, + rviz_rendering::MovableText::VerticalAlignment::V_ABOVE); + movable_text->update(); + + float char_width = getCharWidth(movable_text, 'W'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, 0, 0), Ogre::Vector3(char_width, 2, 0))); +} + +TEST_F(MovableTextTestFixture, vertical_alignment_above_puts_y_coordinate_above) { + auto movable_text = std::make_shared("W\nW"); + + movable_text->setTextAlignment(rviz_rendering::MovableText::HorizontalAlignment::H_LEFT, + rviz_rendering::MovableText::VerticalAlignment::V_ABOVE); + movable_text->update(); + + float char_width = getCharWidth(movable_text, 'W'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, 0, 0), Ogre::Vector3(char_width, 4.01, 0))); +} + +TEST_F(MovableTextTestFixture, setSpaceWidth_changes_width_of_space_character) { + auto movable_text = std::make_shared("A A"); + + float new_space_width = 0.5f; + movable_text->setSpaceWidth(new_space_width); + movable_text->update(); + + float char_width = getCharWidth(movable_text, 'A'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, -2, 0), + Ogre::Vector3(2 * char_width + new_space_width, 0, 0))); +} + +TEST_F(MovableTextTestFixture, setLineSpacing_changes_space_between_lines) { + auto movable_text = std::make_shared("A\nA"); + + float new_line_spacing = 0.5f; + movable_text->setLineSpacing(new_line_spacing); + movable_text->update(); + + float char_width = getCharWidth(movable_text, 'A'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(0, -2 * 2 - new_line_spacing, 0), + Ogre::Vector3(char_width, 0, 0))); +} + +TEST_F(MovableTextTestFixture, horizontal_alignment_works_correctly_with_spaces) { + auto movable_text = std::make_shared("A A"); + + movable_text->setTextAlignment(rviz_rendering::MovableText::HorizontalAlignment::H_CENTER, + rviz_rendering::MovableText::VerticalAlignment::V_BELOW); + movable_text->update(); + + float char_width = getCharWidth(movable_text, 'A'); + assertBoundingBoxEquality(movable_text->getBoundingBox(), + Ogre::AxisAlignedBox(Ogre::Vector3(-3 * char_width / 2, -2, 0), + Ogre::Vector3(3 * char_width / 2, 0, 0))); +} + +TEST_F(MovableTextTestFixture, getBoundingRadius_gets_squared_length_from_origin_to_box_corner) { + auto movable_text = std::make_shared("A A"); + + float char_width = getCharWidth(movable_text, 'A'); + Ogre::Vector3 farthest_point = Ogre::Vector3(3 * char_width, -2.0f, 0); + ASSERT_EQ(movable_text->getBoundingRadius(), Ogre::Math::Sqrt(farthest_point.squaredLength())); +}