range_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, 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 <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){ // Fixed distance ranger
00139     if(msg->range < 0 && !std::isfinite(msg->range)){ // NaNs and +Inf return false here: both of those should have 0.0 as the range
00140       displayed_range = msg->min_range; // -Inf, display the detectable range
00141     }
00142   }
00143   
00144   pose.position.x = displayed_range/2 - .008824 * displayed_range; // .008824 fudge factor measured, must be inaccuracy of cone model.
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 } // namespace rviz
00165 
00166 #include <pluginlib/class_list_macros.h>
00167 PLUGINLIB_EXPORT_CLASS( rviz::RangeDisplay, rviz::Display )


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