path_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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 <boost/bind.hpp>
00031 
00032 #include <OgreSceneNode.h>
00033 #include <OgreSceneManager.h>
00034 #include <OgreManualObject.h>
00035 #include <OgreBillboardSet.h>
00036 #include <OgreMatrix4.h>
00037 
00038 #include <tf/transform_listener.h>
00039 
00040 #include "rviz/display_context.h"
00041 #include "rviz/frame_manager.h"
00042 #include "rviz/properties/color_property.h"
00043 #include "rviz/properties/float_property.h"
00044 #include "rviz/properties/int_property.h"
00045 #include "rviz/validate_floats.h"
00046 
00047 #include "rviz/default_plugin/path_display.h"
00048 
00049 namespace rviz
00050 {
00051 
00052 PathDisplay::PathDisplay()
00053 {
00054   color_property_ = new ColorProperty( "Color", QColor( 25, 255, 0 ),
00055                                        "Color to draw the path.", this );
00056 
00057   alpha_property_ = new FloatProperty( "Alpha", 1.0,
00058                                        "Amount of transparency to apply to the path.", this );
00059 
00060   buffer_length_property_ = new IntProperty( "Buffer Length", 1,
00061                                              "Number of paths to display.",
00062                                              this, SLOT( updateBufferLength() ));
00063   buffer_length_property_->setMin( 1 );
00064 }
00065 
00066 PathDisplay::~PathDisplay()
00067 {
00068   destroyObjects();
00069 }
00070 
00071 void PathDisplay::onInitialize()
00072 {
00073   MFDClass::onInitialize();
00074   updateBufferLength();
00075 }
00076 
00077 void PathDisplay::reset()
00078 {
00079   MFDClass::reset();
00080   updateBufferLength();
00081 }
00082 
00083 void PathDisplay::destroyObjects()
00084 {
00085   for( size_t i = 0; i < manual_objects_.size(); i++ )
00086   {
00087     Ogre::ManualObject* manual_object = manual_objects_[ i ];
00088     if( manual_object )
00089     {
00090       manual_object->clear();
00091       scene_manager_->destroyManualObject( manual_object );
00092     }
00093   }
00094 }
00095 
00096 void PathDisplay::updateBufferLength()
00097 {
00098   destroyObjects();
00099 
00100   int buffer_length = buffer_length_property_->getInt();
00101   QColor color = color_property_->getColor();
00102 
00103   manual_objects_.resize( buffer_length );
00104   for( size_t i = 0; i < manual_objects_.size(); i++ )
00105   {
00106     Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
00107     manual_object->setDynamic( true );
00108     scene_node_->attachObject( manual_object );
00109 
00110     manual_objects_[ i ] = manual_object;
00111   }
00112 }
00113 
00114 bool validateFloats( const nav_msgs::Path& msg )
00115 {
00116   bool valid = true;
00117   valid = valid && validateFloats( msg.poses );
00118   return valid;
00119 }
00120 
00121 void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
00122 {
00123   Ogre::ManualObject* manual_object = manual_objects_[ messages_received_ % buffer_length_property_->getInt() ];
00124   manual_object->clear();
00125 
00126   if( !validateFloats( *msg ))
00127   {
00128     setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00129     return;
00130   }
00131 
00132   Ogre::Vector3 position;
00133   Ogre::Quaternion orientation;
00134   if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00135   {
00136     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00137   }
00138 
00139   Ogre::Matrix4 transform( orientation );
00140   transform.setTrans( position );
00141 
00142 //  scene_node_->setPosition( position );
00143 //  scene_node_->setOrientation( orientation );
00144 
00145   Ogre::ColourValue color = color_property_->getOgreColor();
00146   color.a = alpha_property_->getFloat();
00147 
00148   uint32_t num_points = msg->poses.size();
00149   manual_object->estimateVertexCount( num_points );
00150   manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
00151   for( uint32_t i=0; i < num_points; ++i)
00152   {
00153     const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
00154     Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
00155     manual_object->position( xpos.x, xpos.y, xpos.z );
00156     manual_object->colour( color );
00157   }
00158 
00159   manual_object->end();
00160 }
00161 
00162 } // namespace rviz
00163 
00164 #include <pluginlib/class_list_macros.h>
00165 PLUGINLIB_EXPORT_CLASS( rviz::PathDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27