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 <OgreSceneNode.h>
00031 #include <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 #include <limits>
00043
00044 namespace rviz
00045 {
00046 RangeDisplay::RangeDisplay()
00047 {
00048 color_property_ = new ColorProperty( "Color", Qt::white,
00049 "Color to draw the range.",
00050 this, SLOT( updateColorAndAlpha() ));
00051
00052 alpha_property_ = new FloatProperty( "Alpha", 0.5,
00053 "Amount of transparency to apply to the range.",
00054 this, SLOT( updateColorAndAlpha() ));
00055
00056 buffer_length_property_ = new IntProperty( "Buffer Length", 1,
00057 "Number of prior measurements to display.",
00058 this, SLOT( updateBufferLength() ));
00059 buffer_length_property_->setMin( 1 );
00060
00061 queue_size_property_ = new IntProperty( "Queue Size", 100,
00062 "Size of the tf message filter queue. It usually needs to be set at least as high as the number of sonar frames.",
00063 this, SLOT( updateQueueSize() ));
00064 }
00065
00066 void RangeDisplay::onInitialize()
00067 {
00068 MFDClass::onInitialize();
00069 updateBufferLength();
00070 updateColorAndAlpha();
00071 }
00072
00073 RangeDisplay::~RangeDisplay()
00074 {
00075 for( size_t i = 0; i < cones_.size(); i++ )
00076 {
00077 delete cones_[ i ];
00078 }
00079 }
00080
00081 void RangeDisplay::reset()
00082 {
00083 MFDClass::reset();
00084 updateBufferLength();
00085 }
00086
00087 void RangeDisplay::updateQueueSize()
00088 {
00089 tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00090 }
00091
00092 void RangeDisplay::updateColorAndAlpha()
00093 {
00094 Ogre::ColourValue oc = color_property_->getOgreColor();
00095 float alpha = alpha_property_->getFloat();
00096 for( size_t i = 0; i < cones_.size(); i++ )
00097 {
00098 cones_[i]->setColor( oc.r, oc.g, oc.b, alpha );
00099 }
00100 context_->queueRender();
00101 }
00102
00103 void RangeDisplay::updateBufferLength()
00104 {
00105 int buffer_length = buffer_length_property_->getInt();
00106 QColor color = color_property_->getColor();
00107
00108 for( size_t i = 0; i < cones_.size(); i++ )
00109 {
00110 delete cones_[i];
00111 }
00112 cones_.resize( buffer_length );
00113 for( size_t i = 0; i < cones_.size(); i++ )
00114 {
00115 Shape* cone = new Shape( Shape::Cone, context_->getSceneManager(), scene_node_ );
00116 cones_[ i ] = cone;
00117
00118 Ogre::Vector3 position;
00119 Ogre::Quaternion orientation;
00120 geometry_msgs::Pose pose;
00121 pose.orientation.w = 1;
00122 Ogre::Vector3 scale( 0, 0, 0 );
00123 cone->setScale( scale );
00124 cone->setColor( color.redF(), color.greenF(), color.blueF(), 0 );
00125 }
00126 }
00127
00128 void RangeDisplay::processMessage( const sensor_msgs::Range::ConstPtr& msg )
00129 {
00130 Shape* cone = cones_[ messages_received_ % buffer_length_property_->getInt() ];
00131
00132 Ogre::Vector3 position;
00133 Ogre::Quaternion orientation;
00134 geometry_msgs::Pose pose;
00135 float displayed_range = 0.0;
00136 if(msg->min_range <= msg->range && msg->range <= msg->max_range){
00137 displayed_range = msg->range;
00138 } else if(msg->min_range == msg->max_range){
00139 if(msg->range < 0 && !std::isfinite(msg->range)){
00140 displayed_range = msg->min_range;
00141 }
00142 }
00143
00144 pose.position.x = displayed_range/2 - .008824 * displayed_range;
00145 pose.orientation.z = 0.707;
00146 pose.orientation.w = 0.707;
00147 if( !context_->getFrameManager()->transform( msg->header.frame_id, msg->header.stamp, pose, position, orientation ))
00148 {
00149 ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00150 msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00151 }
00152
00153 cone->setPosition( position );
00154 cone->setOrientation( orientation );
00155
00156 double cone_width = 2.0 * displayed_range * tan( msg->field_of_view / 2.0 );
00157 Ogre::Vector3 scale( cone_width, displayed_range, cone_width );
00158 cone->setScale( scale );
00159
00160 QColor color = color_property_->getColor();
00161 cone->setColor( color.redF(), color.greenF(), color.blueF(), alpha_property_->getFloat() );
00162 }
00163
00164 }
00165
00166 #include <pluginlib/class_list_macros.h>
00167 PLUGINLIB_EXPORT_CLASS( rviz::RangeDisplay, rviz::Display )