pose_array_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <OgreManualObject.h>
00031 #include <OgreSceneManager.h>
00032 #include <OgreSceneNode.h>
00033 
00034 #include "rviz/display_context.h"
00035 #include "rviz/frame_manager.h"
00036 #include "rviz/properties/color_property.h"
00037 #include "rviz/properties/float_property.h"
00038 #include "rviz/properties/enum_property.h"
00039 #include "rviz/validate_floats.h"
00040 #include "rviz/ogre_helpers/arrow.h"
00041 #include "rviz/ogre_helpers/axes.h"
00042 
00043 #include "pose_array_display.h"
00044 
00045 namespace jsk_rviz_plugins
00046 {
00047 
00048 PoseArrayDisplay::PoseArrayDisplay()
00049   : manual_object_( NULL )
00050 {
00051   color_property_ = new rviz::ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrows.", this );
00052   length_property_ = new rviz::FloatProperty( "Arrow Length", 0.3, "Length of the arrows.", this );
00053   axes_length_property_ = new rviz::FloatProperty( "Axes Length", 1, "Length of each axis, in meters.",
00054                                              this, SLOT( updateAxisGeometry() ));
00055   axes_radius_property_ = new rviz::FloatProperty( "Axes Radius", 0.1, "Radius of each axis, in meters.",
00056                                              this, SLOT( updateAxisGeometry() ));
00057   shape_property_ = new rviz::EnumProperty( "Shape", "Arrow", "Shape to display the pose as.",
00058                                       this, SLOT( updateShapeChoice() ));
00059   shape_property_->addOption( "Arrow", Arrow );
00060   shape_property_->addOption( "Axes", Axes );
00061 }
00062 
00063 PoseArrayDisplay::~PoseArrayDisplay()
00064 {
00065   if ( initialized() )
00066   {
00067     scene_manager_->destroyManualObject( manual_object_ );
00068   }
00069 }
00070 
00071 void PoseArrayDisplay::onInitialize()
00072 {
00073   MFDClass::onInitialize();
00074   manual_object_ = scene_manager_->createManualObject();
00075   manual_object_->setDynamic( true );
00076   scene_node_->attachObject( manual_object_ );
00077 
00078   updateShapeChoice();
00079   updateShapeVisibility();
00080   updateAxisGeometry();
00081 }
00082 
00083 void PoseArrayDisplay::updateShapeChoice()
00084 {
00085     bool use_arrow = ( shape_property_->getOptionInt() == Arrow );
00086 
00087   color_property_->setHidden( !use_arrow );
00088   length_property_->setHidden( !use_arrow );
00089 
00090   axes_length_property_->setHidden( use_arrow );
00091   axes_radius_property_->setHidden( use_arrow );
00092 
00093   updateShapeVisibility();
00094 
00095   context_->queueRender();
00096 }
00097 
00098 void PoseArrayDisplay::updateShapeVisibility()
00099 {
00100 
00101   if( !pose_valid_ )
00102   {
00103     manual_object_->setVisible(false);
00104     for (int i = 0; i < coords_nodes_.size() ; i++)
00105       coords_nodes_[i]->setVisible(false);
00106   }
00107   else
00108   {
00109     bool use_arrow = (shape_property_->getOptionInt() == Arrow);
00110     for (int i = 0; i < coords_nodes_.size() ; i++)
00111       coords_nodes_[i]->setVisible(!use_arrow);
00112 
00113     manual_object_->setVisible(use_arrow);
00114   }
00115 }
00116 
00117 void PoseArrayDisplay::updateAxisGeometry()
00118 {
00119   for(size_t i = 0; i < coords_objects_.size() ; i++)
00120     coords_objects_[i]->set( axes_length_property_->getFloat(),
00121                              axes_radius_property_->getFloat() );
00122   context_->queueRender();
00123 }
00124 
00125 void PoseArrayDisplay::allocateCoords(int num)
00126 {
00127   if (num > coords_objects_.size()) {
00128     for (size_t i = coords_objects_.size(); i < num; i++) {
00129       Ogre::SceneNode* scene_node = scene_node_->createChildSceneNode();
00130       rviz::Axes* axes = new rviz::Axes( scene_manager_, scene_node,
00131                                         axes_length_property_->getFloat(),
00132                                         axes_radius_property_->getFloat());
00133       coords_nodes_.push_back(scene_node);
00134       coords_objects_.push_back(axes);
00135     }
00136   }
00137   else if (num < coords_objects_.size()) {
00138     for (int i = coords_objects_.size() - 1; num <= i; i--) {
00139       delete coords_objects_[i];
00140       scene_manager_->destroySceneNode(coords_nodes_[i]);
00141     }
00142     coords_objects_.resize(num);
00143     coords_nodes_.resize(num);
00144   }
00145 }
00146 
00147 
00148 bool validateFloats( const geometry_msgs::PoseArray& msg )
00149 {
00150   return rviz::validateFloats( msg.poses );
00151 }
00152 
00153 void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr& msg )
00154 {
00155   if( !validateFloats( *msg ))
00156   {
00157     setStatus( rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00158     return;
00159   }
00160 
00161   manual_object_->clear();
00162 
00163   Ogre::Vector3 position;
00164   Ogre::Quaternion orientation;
00165   if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00166   {
00167     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00168   }
00169 
00170   pose_valid_ = true;
00171   updateShapeVisibility();
00172 
00173   scene_node_->setPosition( position );
00174   scene_node_->setOrientation( orientation );
00175 
00176   manual_object_->clear();
00177 
00178   if(shape_property_->getOptionInt() == Arrow ) {
00179     for (int i = 0; i < coords_nodes_.size() ; i++)
00180       coords_nodes_[i]->setVisible(false);
00181     Ogre::ColourValue color = color_property_->getOgreColor();
00182     float length = length_property_->getFloat();
00183     size_t num_poses = msg->poses.size();
00184     manual_object_->estimateVertexCount( num_poses * 6 );
00185     manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
00186     for( size_t i=0; i < num_poses; ++i )
00187       {
00188         Ogre::Vector3 pos( msg->poses[i].position.x,
00189                            msg->poses[i].position.y,
00190                            msg->poses[i].position.z );
00191         Ogre::Quaternion orient( msg->poses[i].orientation.w,
00192                                  msg->poses[i].orientation.x,
00193                                  msg->poses[i].orientation.y,
00194                                  msg->poses[i].orientation.z );
00195         // orient here is not normalized, so the scale of the quaternion
00196         // will affect the scale of the arrow.
00197 
00198         Ogre::Vector3 vertices[6];
00199         vertices[0] = pos; // back of arrow
00200         vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 ); // tip of arrow
00201         vertices[2] = vertices[ 1 ];
00202         vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 );
00203         vertices[4] = vertices[ 1 ];
00204         vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 );
00205 
00206         for( int i = 0; i < 6; ++i )
00207           {
00208             manual_object_->position( vertices[i] );
00209             manual_object_->colour( color );
00210           }
00211       }
00212     manual_object_->end();
00213   }
00214   else{
00215     allocateCoords(msg->poses.size());
00216     for (int i = 0; i < msg->poses.size() ; i++){
00217       geometry_msgs::Pose pose = msg->poses[i];
00218       Ogre::SceneNode* scene_node = coords_nodes_[i];
00219       scene_node->setVisible(true);
00220 
00221       Ogre::Vector3 position( msg->poses[i].position.x,
00222                               msg->poses[i].position.y,
00223                               msg->poses[i].position.z );
00224       Ogre::Quaternion orientation( msg->poses[i].orientation.w,
00225                                     msg->poses[i].orientation.x,
00226                                     msg->poses[i].orientation.y,
00227                                     msg->poses[i].orientation.z );
00228       scene_node->setPosition(position);
00229       scene_node->setOrientation(orientation); // scene node is at frame pose
00230     }
00231   }
00232 
00233   context_->queueRender();
00234 }
00235 
00236 void PoseArrayDisplay::reset()
00237 {
00238   MFDClass::reset();
00239   if( manual_object_ )
00240   {
00241     manual_object_->clear();
00242   }
00243   if ( coords_objects_.size() > 0 ) {
00244     allocateCoords(0);
00245   }
00246   
00247 }
00248 
00249 } // namespace rviz
00250 
00251 #include <pluginlib/class_list_macros.h>
00252 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::PoseArrayDisplay, rviz::Display )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03