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 <OGRE/OgreSceneNode.h>
00033 #include <OGRE/OgreSceneManager.h>
00034 #include <OGRE/OgreManualObject.h>
00035 #include <OGRE/OgreBillboardSet.h>
00036
00037 #include <tf/transform_listener.h>
00038
00039 #include "rviz/display_context.h"
00040 #include "rviz/frame_manager.h"
00041 #include "rviz/properties/color_property.h"
00042 #include "rviz/properties/float_property.h"
00043 #include "rviz/validate_floats.h"
00044
00045 #include "rviz/default_plugin/path_display.h"
00046
00047 namespace rviz
00048 {
00049
00050 PathDisplay::PathDisplay()
00051 : manual_object_( NULL )
00052 {
00053 color_property_ = new ColorProperty( "Color", QColor( 25, 255, 0 ),
00054 "Color to draw the path.", this );
00055
00056 alpha_property_ = new FloatProperty( "Alpha", 1.0,
00057 "Amount of transparency to apply to the path.", this );
00058 }
00059
00060 PathDisplay::~PathDisplay()
00061 {
00062 if( manual_object_ )
00063 {
00064 manual_object_->clear();
00065 scene_manager_->destroyManualObject( manual_object_ );
00066 }
00067 }
00068
00069 void PathDisplay::onInitialize()
00070 {
00071 MFDClass::onInitialize();
00072
00073 manual_object_ = scene_manager_->createManualObject();
00074 manual_object_->setDynamic( true );
00075 scene_node_->attachObject( manual_object_ );
00076 }
00077
00078 void PathDisplay::reset()
00079 {
00080 MFDClass::reset();
00081 manual_object_->clear();
00082 }
00083
00084 bool validateFloats( const nav_msgs::Path& msg )
00085 {
00086 bool valid = true;
00087 valid = valid && validateFloats( msg.poses );
00088 return valid;
00089 }
00090
00091 void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
00092 {
00093 if( !validateFloats( *msg ))
00094 {
00095 setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
00096 return;
00097 }
00098
00099 manual_object_->clear();
00100
00101 Ogre::Vector3 position;
00102 Ogre::Quaternion orientation;
00103 if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
00104 {
00105 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00106 }
00107
00108 scene_node_->setPosition( position );
00109 scene_node_->setOrientation( orientation );
00110
00111 manual_object_->clear();
00112
00113 Ogre::ColourValue color = color_property_->getOgreColor();
00114 color.a = alpha_property_->getFloat();
00115
00116 uint32_t num_points = msg->poses.size();
00117 manual_object_->estimateVertexCount( num_points );
00118 manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
00119 for( uint32_t i=0; i < num_points; ++i)
00120 {
00121 const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
00122 manual_object_->position( pos.x, pos.y, pos.z );
00123 manual_object_->colour( color );
00124 }
00125
00126 manual_object_->end();
00127 }
00128
00129 }
00130
00131 #include <pluginlib/class_list_macros.h>
00132 PLUGINLIB_DECLARE_CLASS( rviz, Path, rviz::PathDisplay, rviz::Display )