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 <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  #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 
00062 void RangeDisplay::onInitialize()
00063 {
00064   MFDClass::onInitialize();
00065   updateBufferLength();
00066   updateColorAndAlpha();
00067 }
00068 
00069 RangeDisplay::~RangeDisplay()
00070 {
00071   for( size_t i = 0; i < cones_.size(); i++ )
00072   {
00073     delete cones_[ i ];
00074   }
00075 }
00076 
00077 void RangeDisplay::reset()
00078 {
00079   MFDClass::reset();
00080   updateBufferLength();
00081 }
00082 
00083 void RangeDisplay::updateColorAndAlpha()
00084 {
00085   Ogre::ColourValue oc = color_property_->getOgreColor();
00086   float alpha = alpha_property_->getFloat();
00087   for( size_t i = 0; i < cones_.size(); i++ )
00088   {
00089     cones_[i]->setColor( oc.r, oc.g, oc.b, alpha );
00090   }
00091   context_->queueRender();
00092 }
00093 
00094 void RangeDisplay::updateBufferLength()
00095 {
00096   int buffer_length = buffer_length_property_->getInt();
00097   QColor color = color_property_->getColor();
00098 
00099   for( size_t i = 0; i < cones_.size(); i++ )
00100   {
00101     delete cones_[i];
00102   }
00103   cones_.resize( buffer_length );
00104   for( size_t i = 0; i < cones_.size(); i++ )
00105   {
00106     Shape* cone = new Shape( Shape::Cone, context_->getSceneManager(), scene_node_ );
00107     cones_[ i ] = cone;    
00108 
00109     Ogre::Vector3 position;
00110     Ogre::Quaternion orientation;
00111     geometry_msgs::Pose pose;
00112     pose.orientation.w = 1;
00113     Ogre::Vector3 scale( 0, 0, 0 );
00114     cone->setScale( scale );
00115     cone->setColor( color.redF(), color.greenF(), color.blueF(), 0 );
00116   }
00117 }
00118 
00119 void RangeDisplay::processMessage( const sensor_msgs::Range::ConstPtr& msg )
00120 {
00121   Shape* cone = cones_[ messages_received_ % buffer_length_property_->getInt() ];
00122 
00123   Ogre::Vector3 position;
00124   Ogre::Quaternion orientation;
00125   geometry_msgs::Pose pose;
00126   float displayed_range = 0.0;
00127   if(msg->min_range <= msg->range && msg->range <= msg->max_range){
00128     displayed_range = msg->range;
00129   } else if(msg->min_range == msg->max_range){ // Fixed distance ranger
00130     if(msg->range < 0 && !std::isfinite(msg->range)){ // NaNs and +Inf return false here: both of those should have 0.0 as the range
00131       displayed_range = msg->min_range; // -Inf, display the detectable range
00132     }
00133   }
00134   
00135   pose.position.x = displayed_range/2 - .008824 * displayed_range; // .008824 fudge factor measured, must be inaccuracy of cone model.
00136   pose.orientation.z = 0.707;
00137   pose.orientation.w = 0.707;
00138   if( !context_->getFrameManager()->transform( msg->header.frame_id, msg->header.stamp, pose, position, orientation ))
00139   {
00140     ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00141                msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00142   }
00143 
00144   cone->setPosition( position );
00145   cone->setOrientation( orientation );
00146 
00147   double cone_width = 2.0 * displayed_range * tan( msg->field_of_view / 2.0 );
00148   Ogre::Vector3 scale( cone_width, displayed_range, cone_width );
00149   cone->setScale( scale );
00150 
00151   QColor color = color_property_->getColor();
00152   cone->setColor( color.redF(), color.greenF(), color.blueF(), alpha_property_->getFloat() );
00153 }
00154 
00155 } // namespace rviz
00156 
00157 #include <pluginlib/class_list_macros.h>
00158 PLUGINLIB_EXPORT_CLASS( rviz::RangeDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:35