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 <OGRE/OgreSceneNode.h>
00031 #include <OGRE/OgreSceneManager.h>
00032
00033 #include "rviz/display_context.h"
00034 #include "rviz/frame_manager.h"
00035 #include "rviz/ogre_helpers/shape.h"
00036 #include "rviz/properties/color_property.h"
00037 #include "rviz/properties/float_property.h"
00038 #include "rviz/properties/int_property.h"
00039 #include "rviz/properties/parse_color.h"
00040
00041 #include "range_display.h"
00042
00043 namespace rviz
00044 {
00045 RangeDisplay::RangeDisplay()
00046 {
00047 color_property_ = new ColorProperty( "Color", Qt::white,
00048 "Color to draw the range.",
00049 this, SLOT( updateColorAndAlpha() ));
00050
00051 alpha_property_ = new FloatProperty( "Alpha", 0.5,
00052 "Amount of transparency to apply to the range.",
00053 this, SLOT( updateColorAndAlpha() ));
00054
00055 buffer_length_property_ = new IntProperty( "Buffer Length", 1,
00056 "Number of prior measurements to display.",
00057 this, SLOT( updateBufferLength() ));
00058 buffer_length_property_->setMin( 1 );
00059 }
00060
00061 void RangeDisplay::onInitialize()
00062 {
00063 MFDClass::onInitialize();
00064 updateBufferLength();
00065 updateColorAndAlpha();
00066 }
00067
00068 RangeDisplay::~RangeDisplay()
00069 {
00070 for( size_t i = 0; i < cones_.size(); i++ )
00071 {
00072 delete cones_[ i ];
00073 }
00074 }
00075
00076 void RangeDisplay::reset()
00077 {
00078 MFDClass::reset();
00079 updateBufferLength();
00080 }
00081
00082 void RangeDisplay::updateColorAndAlpha()
00083 {
00084 Ogre::ColourValue oc = color_property_->getOgreColor();
00085 float alpha = alpha_property_->getFloat();
00086 for( size_t i = 0; i < cones_.size(); i++ )
00087 {
00088 cones_[i]->setColor( oc.r, oc.g, oc.b, alpha );
00089 }
00090 context_->queueRender();
00091 }
00092
00093 void RangeDisplay::updateBufferLength()
00094 {
00095 int buffer_length = buffer_length_property_->getInt();
00096 QColor color = color_property_->getColor();
00097
00098 for( size_t i = 0; i < cones_.size(); i++ )
00099 {
00100 delete cones_[i];
00101 }
00102 cones_.resize( buffer_length );
00103 for( size_t i = 0; i < cones_.size(); i++ )
00104 {
00105 Shape* cone = new Shape( Shape::Cone, context_->getSceneManager(), scene_node_ );
00106 cones_[ i ] = cone;
00107
00108 Ogre::Vector3 position;
00109 Ogre::Quaternion orientation;
00110 geometry_msgs::Pose pose;
00111 pose.orientation.w = 1;
00112 Ogre::Vector3 scale( 0, 0, 0 );
00113 cone->setScale( scale );
00114 cone->setColor( color.redF(), color.greenF(), color.blueF(), 0 );
00115 }
00116 }
00117
00118 void RangeDisplay::processMessage( const sensor_msgs::Range::ConstPtr& msg )
00119 {
00120 Shape* cone = cones_[ messages_received_ % buffer_length_property_->getInt() ];
00121
00122 Ogre::Vector3 position;
00123 Ogre::Quaternion orientation;
00124 geometry_msgs::Pose pose;
00125 pose.position.x = msg->range/2 - .008824 * msg->range;
00126 pose.orientation.z = 0.707;
00127 pose.orientation.w = 0.707;
00128 if( !context_->getFrameManager()->transform( msg->header.frame_id, msg->header.stamp, pose, position, orientation ))
00129 {
00130 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00131 msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00132 }
00133
00134 cone->setPosition( position );
00135 cone->setOrientation( orientation );
00136
00137 double cone_width = 2.0 * msg->range * tan( msg->field_of_view / 2.0 );
00138 Ogre::Vector3 scale( cone_width, msg->range, cone_width );
00139 cone->setScale( scale );
00140
00141 QColor color = color_property_->getColor();
00142 cone->setColor( color.redF(), color.greenF(), color.blueF(), alpha_property_->getFloat() );
00143 }
00144
00145 }
00146
00147 #include <pluginlib/class_list_macros.h>
00148 PLUGINLIB_DECLARE_CLASS( rviz, Range, rviz::RangeDisplay, rviz::Display )