Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00143
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 }
00163
00164 #include <pluginlib/class_list_macros.h>
00165 PLUGINLIB_EXPORT_CLASS( rviz::PathDisplay, rviz::Display )