odometry_display.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, 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 "rviz/ogre_helpers/arrow.h"
#include "rviz/ogre_helpers/axes.h"
#include "rviz/properties/enum_property.h"
#include "rviz/properties/color_property.h"
#include "rviz/properties/float_property.h"
#include "rviz/properties/int_property.h"
#include "rviz/validate_floats.h"
#include "rviz/validate_quaternions.h"

#include <OgreSceneManager.h>
#include <OgreSceneNode.h>

#include "odometry_display.h"
#include "covariance_property.h"
#include "covariance_visual.h"

namespace rviz
{

OdometryDisplay::OdometryDisplay()
{

  position_tolerance_property_ = new FloatProperty( "Position Tolerance", .1,
                                                    "Distance, in meters from the last arrow dropped, "
                                                    "that will cause a new arrow to drop.",
                                                    this );
  position_tolerance_property_->setMin( 0 );

  angle_tolerance_property_ = new FloatProperty( "Angle Tolerance", .1,
                                                 "Angular distance from the last arrow dropped, "
                                                 "that will cause a new arrow to drop.",
                                                 this );
  angle_tolerance_property_->setMin( 0 );

  keep_property_ = new IntProperty( "Keep", 100,
                                    "Number of arrows to keep before removing the oldest.  0 means keep all of them.",
                                    this );
  keep_property_->setMin( 0 );

  shape_property_ = new EnumProperty( "Shape", "Arrow", "Shape to display the pose as.",
                                      this, SLOT( updateShapeChoice() ));
  shape_property_->addOption( "Arrow", ArrowShape );
  shape_property_->addOption( "Axes", AxesShape );

  color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ),
                                       "Color of the arrows.",
                                       shape_property_, SLOT( updateColorAndAlpha() ), this);

  alpha_property_ = new FloatProperty( "Alpha", 1, "Amount of transparency to apply to the arrow.",
                                       shape_property_, SLOT( updateColorAndAlpha() ), this);
  alpha_property_->setMin( 0 );
  alpha_property_->setMax( 1 );

  shaft_length_property_ = new FloatProperty( "Shaft Length", 1, "Length of the each arrow's shaft, in meters.",
                                              shape_property_, SLOT( updateArrowsGeometry() ), this);

  // aleeper: default changed from 0.1 to match change in arrow.cpp
  shaft_radius_property_ = new FloatProperty( "Shaft Radius", 0.05, "Radius of the each arrow's shaft, in meters.",
                                              shape_property_, SLOT( updateArrowsGeometry() ), this);

  head_length_property_ = new FloatProperty( "Head Length", 0.3, "Length of the each arrow's head, in meters.",
                                             shape_property_, SLOT( updateArrowsGeometry() ), this);

  // aleeper: default changed from 0.2 to match change in arrow.cpp
  head_radius_property_ = new FloatProperty( "Head Radius", 0.1, "Radius of the each arrow's head, in meters.",
                                             shape_property_, SLOT( updateArrowsGeometry() ), this);

  axes_length_property_ = new FloatProperty( "Axes Length", 1, "Length of each axis, in meters.",
                                             shape_property_, SLOT( updateAxisGeometry() ), this);

  axes_radius_property_ = new FloatProperty( "Axes Radius", 0.1, "Radius of each axis, in meters.",
                                             shape_property_, SLOT( updateAxisGeometry() ), this);

  covariance_property_ = new CovarianceProperty( "Covariance", true, "Whether or not the covariances of the messages should be shown.",
                                             this, SLOT( queueRender() ));

}

OdometryDisplay::~OdometryDisplay()
{
  if ( initialized() )
  {
    clear();
  }
}

void OdometryDisplay::onInitialize()
{
  MFDClass::onInitialize();
  updateShapeChoice();
}

void OdometryDisplay::onEnable()
{
  MFDClass::onEnable();
  updateShapeVisibility();
}

void OdometryDisplay::clear()
{
  D_Arrow::iterator it = arrows_.begin();
  D_Arrow::iterator end = arrows_.end();
  for ( ; it != end; ++it )
  {
    delete *it;
  }
  arrows_.clear();

  // covariances are stored in covariance_property_
  covariance_property_->clearVisual();

  D_Axes::iterator it_axes = axes_.begin();
  D_Axes::iterator end_axes = axes_.end();
  for ( ; it_axes != end_axes; ++it_axes )
  {
    delete *it_axes;
  }
  axes_.clear();

  if( last_used_message_ )
  {
    last_used_message_.reset();
  }
}

void OdometryDisplay::updateColorAndAlpha()
{
  QColor color = color_property_->getColor();
  float red   = color.redF();
  float green = color.greenF();
  float blue  = color.blueF();
  float alpha = alpha_property_->getFloat();

  D_Arrow::iterator it = arrows_.begin();
  D_Arrow::iterator end = arrows_.end();
  for( ; it != end; ++it )
  {
    Arrow* arrow = *it;
    arrow->setColor( red, green, blue, alpha );
  }
  context_->queueRender();
}

void OdometryDisplay::updateArrowsGeometry()
{
  D_Arrow::iterator it = arrows_.begin();
  D_Arrow::iterator end = arrows_.end();
  for ( ; it != end; ++it )
  {
    updateGeometry(*it);
  }
  context_->queueRender();
}

void OdometryDisplay::updateAxisGeometry()
{
  D_Axes::iterator it = axes_.begin();
  D_Axes::iterator end = axes_.end();
  for ( ; it != end; ++it )
  {
    updateGeometry(*it);
  }
  context_->queueRender();
}

void OdometryDisplay::updateGeometry( Axes* axes )
{
    axes->set( axes_length_property_->getFloat(),
              axes_radius_property_->getFloat() );
}

void OdometryDisplay::updateGeometry( Arrow* arrow )
{
    arrow->set( shaft_length_property_->getFloat(),
                shaft_radius_property_->getFloat(),
                head_length_property_->getFloat(),
                head_radius_property_->getFloat() );
}

void OdometryDisplay::updateShapeChoice()
{
  bool use_arrow = ( shape_property_->getOptionInt() == ArrowShape );

  color_property_->setHidden( !use_arrow );
  alpha_property_->setHidden( !use_arrow );
  shaft_length_property_->setHidden( !use_arrow );
  shaft_radius_property_->setHidden( !use_arrow );
  head_length_property_->setHidden( !use_arrow );
  head_radius_property_->setHidden( !use_arrow );

  axes_length_property_->setHidden( use_arrow );
  axes_radius_property_->setHidden( use_arrow );

  updateShapeVisibility();

  context_->queueRender();
}

void OdometryDisplay::updateShapeVisibility()
{
  bool use_arrow = (shape_property_->getOptionInt() == ArrowShape);

  D_Arrow::iterator it = arrows_.begin();
  D_Arrow::iterator end = arrows_.end();
  for ( ; it != end; ++it )
  {
    (*it)->getSceneNode()->setVisible( use_arrow );
  }

  D_Axes::iterator it_axes = axes_.begin();
  D_Axes::iterator end_axes = axes_.end();
  for ( ; it_axes != end_axes; ++it_axes )
  {
    (*it_axes)->getSceneNode()->setVisible( !use_arrow );
  }
}

bool validateFloats(const nav_msgs::Odometry& msg)
{
  bool valid = true;
  valid = valid && rviz::validateFloats( msg.pose.pose );
  valid = valid && rviz::validateFloats( msg.pose.covariance );
  valid = valid && rviz::validateFloats( msg.twist.twist );
  // valid = valid && rviz::validateFloats( msg.twist.covariance )
  return valid;
}

void OdometryDisplay::processMessage( const nav_msgs::Odometry::ConstPtr& message )
{
  typedef CovarianceProperty::CovarianceVisualPtr CovarianceVisualPtr;

  if( !validateFloats( *message ))
  {
    setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
    return;
  }

  if( !validateQuaternions( message->pose.pose ))
  {
    ROS_WARN_ONCE_NAMED( "quaternions", "Odometry '%s' contains unnormalized quaternions. "
                         "This warning will only be output once but may be true for others; "
                         "enable DEBUG messages for ros.rviz.quaternions to see more details.",
                         qPrintable( getName() ) );
    ROS_DEBUG_NAMED( "quaternions", "Odometry '%s' contains unnormalized quaternions.", 
                     qPrintable( getName() ) );
  }

  if( last_used_message_ )
  {
    Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);
    Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
    Eigen::Quaternionf last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z);
    Eigen::Quaternionf current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);

    if( (last_position - current_position).length() < position_tolerance_property_->getFloat() &&
        last_orientation.angularDistance(current_orientation) < angle_tolerance_property_->getFloat() )
    {
      return;
    }
  }

  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  if( !context_->getFrameManager()->transform( message->header, message->pose.pose, position, orientation ))
  {
    ROS_ERROR( "Error transforming odometry '%s' from frame '%s' to frame '%s'",
               qPrintable( getName() ), message->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
    return;
  }

  // If we arrive here, we're good. Continue...

  // Create a scene node, and attach the arrow and the covariance to it
  Axes* axes = new Axes( scene_manager_, scene_node_,
                         axes_length_property_->getFloat(),
                         axes_radius_property_->getFloat() );
  Arrow* arrow = new Arrow( scene_manager_, scene_node_,
                            shaft_length_property_->getFloat(),
                            shaft_radius_property_->getFloat(),
                            head_length_property_->getFloat(),
                            head_radius_property_->getFloat() );
  CovarianceVisualPtr cov = covariance_property_->createAndPushBackVisual(scene_manager_, scene_node_ );

  // Position the axes
  axes->setPosition( position );
  axes->setOrientation( orientation );

  // Position the arrow. Remember the arrow points in -Z direction, so rotate the orientation before display.
  arrow->setPosition( position );
  arrow->setOrientation( orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y ));

  // Position the frame where the covariance is attached covariance
  cov->setPosition( position );
  cov->setOrientation( orientation );

  // Set up arrow color
  QColor color = color_property_->getColor();
  float alpha = alpha_property_->getFloat();
  arrow->setColor( color.redF(), color.greenF(), color.blueF(), alpha);

  // Set up the covariance based on the message
  cov->setCovariance(message->pose);

  // Show/Hide things based on current properties
  bool use_arrow = (shape_property_->getOptionInt() == ArrowShape);
  arrow->getSceneNode()->setVisible( use_arrow );
  axes->getSceneNode()->setVisible( !use_arrow );

  // store everything
  axes_.push_back( axes );
  arrows_.push_back( arrow );

  last_used_message_ = message;
  context_->queueRender();
}

void OdometryDisplay::update( float wall_dt, float ros_dt )
{
  size_t keep = keep_property_->getInt();
  if( keep > 0 )
  {
    while( arrows_.size() > keep )
    {
      delete arrows_.front();
      arrows_.pop_front();

      // covariance visuals are stored into covariance_property_
      covariance_property_->popFrontVisual();

      delete axes_.front();
      axes_.pop_front();
    }
  }

  assert(arrows_.size() == axes_.size());
  assert(axes_.size() == covariance_property_->sizeVisual());

}

void OdometryDisplay::reset()
{
  MFDClass::reset();
  clear();
}

} // namespace rviz

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS( rviz::OdometryDisplay, rviz::Display )