jlo_display_base.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, U. Klank
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  */
00031 
00032 #include "jlo_display_base.h"
00033 #include "rviz/properties/property.h"
00034 #include "rviz/properties/property_manager.h"
00035 
00036 #include <boost/bind.hpp>
00037 
00038 #include <algorithm>
00039 
00040 using namespace vision_srvs;
00041 namespace rviz_shows_cop
00042 {
00043 
00044 JloDisplayBase::JloDisplayBase(const std::string & name,
00045   rviz::VisualizationManager * manager)
00046   : Display(name, manager),
00047     keep_(1),
00048     show_text_(true),
00049     show_axis_(true),
00050     show_cov_(true),
00051     axis_length_(0.2),
00052     axis_thickness_(0.02),
00053     cov_length_(0.05),
00054     cov_thickness_(0.005)
00055 {
00056   scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00057   inited_jlo = false;
00058 }
00059 
00060 JloDisplayBase::~JloDisplayBase()
00061 {
00062   if(m_binited)
00063     clear();
00064 }
00065 
00066 void JloDisplayBase::clear()
00067 {
00068   if(jlo_nodes_.size() > 0)
00069   {
00070     scene_node_->removeAndDestroyAllChildren();
00071     jlo_nodes_.clear();
00072   }
00073 }
00074 
00075 void JloDisplayBase::setTopic(const std::string & topic)
00076 {
00077   topic_ = topic;
00078   propertyChanged(topic_property_);
00079   if(m_binited)
00080   {
00081     clear();
00082     causeRender();
00083   }
00084 }
00085 
00086 
00087 void JloDisplayBase::setKeep(uint32_t keep)
00088 {
00089   keep_ = keep;
00090   propertyChanged(keep_property_);
00091   while( jlo_nodes_.size() > keep_ )
00092     popJloSet();
00093   processMessage();
00094 }
00095 
00096 void JloDisplayBase::onEnable()
00097 {
00098   scene_node_->setVisible(true);
00099 }
00100 
00101 void JloDisplayBase::onDisable()
00102 {
00103   scene_node_->setVisible(false);
00104   if(m_binited)
00105     clear();
00106 }
00107 
00108 void JloDisplayBase::fixedFrameChanged()
00109 {
00110   if(m_binited)
00111     clear();
00112 }
00113 
00114 void JloDisplayBase::update(float wall_dt, float ros_dt)
00115 {
00116 }
00117 
00118 void JloDisplayBase::popJloSet()
00119 {
00120   while( jlo_nodes_.front().size() )
00121   {
00122     jlo_nodes_.front().front().first->removeAndDestroyAllChildren();
00123     delete jlo_nodes_.front().front().first;
00124     jlo_nodes_.front().pop_front();
00125   }
00126   jlo_nodes_.pop_front();
00127 }
00128 
00129 bool JloDisplayBase::GetJlo(unsigned long id, unsigned long parent_id,
00130     vision_msgs::partial_lo  &lo)
00131 {
00132   srvjlo msg;
00133   msg.request.command = "framequery";
00134   msg.request.query.parent_id = parent_id;
00135   msg.request.query.id = id;
00136   if(!inited_jlo || !jlo_client.isValid())
00137   {
00138     inited_jlo = true;
00139     jlo_client = update_nh_.serviceClient<srvjlo>("/located_object", true);
00140   }
00141   if (!jlo_client.call(msg))
00142   {
00143     printf("Error in ROSjloComm: Update of pose information not psossible!\n");
00144     return false;
00145   }
00146   else if (msg.response.error.length() > 0)
00147   {
00148     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00149     return false;
00150   }
00151   lo = msg.response.answer;
00152   return true;
00153 }
00154 
00155 unsigned  long JloDisplayBase::NameQueryJlo(std::string name)
00156 {
00157   srvjlo msg;
00158   msg.request.command = "namequery";
00159   if(name.length() < 2)
00160     return 1;
00161   msg.request.query.name = name;
00162 
00163   if(!inited_jlo || !jlo_client.isValid())
00164   {
00165     inited_jlo = true;
00166     jlo_client = update_nh_.serviceClient<srvjlo>("/located_object", true);
00167   }
00168   if (!jlo_client.call(msg))
00169   {
00170     printf("Error in ROSjloComm: Update of pose information not psossible!\n");
00171     return false;
00172   }
00173   else if (msg.response.error.length() > 0)
00174   {
00175     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00176     return 1;
00177   }
00178   return msg.response.answer.id;
00179 }
00180 
00181 
00182 
00183 void JloDisplayBase::reset()
00184 {
00185   if(m_binited)
00186     clear();
00187 }
00188 
00189 void JloDisplayBase::targetFrameChanged()
00190 {
00191 }
00192 
00193 void JloDisplayBase::setST( bool show_text )
00194 {
00195   show_text_ = show_text;
00196   propertyChanged(show_text_p_);
00197   processMessage();
00198 }
00199 
00200 void JloDisplayBase::setSA( bool show_axis )
00201 {
00202   show_axis_ = show_axis;
00203   propertyChanged(show_axis_p_);
00204   processMessage();
00205 }
00206 
00207 void JloDisplayBase::setSC( bool show_cov)
00208 {
00209   show_cov_ = show_cov;
00210   propertyChanged(show_cov_p_);
00211   processMessage();
00212 }
00213 void JloDisplayBase::setAL(double axis_length)
00214 {
00215   axis_length_ = axis_length;
00216   propertyChanged(axis_length_p_);
00217   processMessage();
00218 }
00219 void JloDisplayBase::setAT(double axis_thick)
00220 {
00221   axis_thickness_ = axis_thick;
00222   propertyChanged(axis_thickness_p_);
00223   processMessage();
00224 }
00225 void JloDisplayBase::setCL(double cov_length)
00226 {
00227   cov_length_ = cov_length;
00228   propertyChanged(cov_length_p_);
00229   processMessage();
00230 }
00231 void JloDisplayBase::setCT(double cov_thick)
00232 {
00233   cov_thickness_ = cov_thick;
00234   propertyChanged(cov_thickness_p_);
00235   processMessage();
00236 }
00237 
00238 void JloDisplayBase::createProperties()
00239 {
00240 
00241 
00242 
00243   keep_property_ = property_manager_->createProperty<rviz::IntProperty>( "Keep", property_prefix_, boost::bind( &JloDisplayBase::getKeep, this ),
00244                                                                     boost::bind( &JloDisplayBase::setKeep, this, _1 ), parent_category_, this );
00245 
00246 
00247 
00248   extended_props_p_ = property_manager_->createCategory("Appearance Properties", property_prefix_, parent_category_);
00249   show_text_p_ = property_manager_->createProperty<rviz::BoolProperty>( "Show Text", property_prefix_, boost::bind( &JloDisplayBase::getST, this ),
00250                                                                       boost::bind( &JloDisplayBase::setST, this, _1 ), extended_props_p_, this );
00251 
00252   show_axis_p_ = property_manager_->createProperty<rviz::BoolProperty>( "Show Axis", property_prefix_, boost::bind( &JloDisplayBase::getSA, this ),
00253                                                                       boost::bind( &JloDisplayBase::setSA, this, _1 ), extended_props_p_, this );
00254   axis_length_p_ = property_manager_->createProperty<rviz::FloatProperty>( "Axis Length", property_prefix_, boost::bind( &JloDisplayBase::getAL, this ),
00255                                                                       boost::bind( &JloDisplayBase::setAL, this, _1 ), extended_props_p_, this );
00256   axis_thickness_p_ = property_manager_->createProperty<rviz::FloatProperty>( "Axis Thickness", property_prefix_, boost::bind( &JloDisplayBase::getAT, this ),
00257                                                                       boost::bind( &JloDisplayBase::setAT, this, _1 ), extended_props_p_, this );
00258 
00259   show_cov_p_ = property_manager_->createProperty<rviz::BoolProperty>( "Show Covariance", property_prefix_, boost::bind( &JloDisplayBase::getSC, this ),
00260                                                                       boost::bind( &JloDisplayBase::setSC, this, _1 ), extended_props_p_, this );
00261 
00262   cov_length_p_ =  property_manager_->createProperty<rviz::FloatProperty>( "Covariance Length", property_prefix_, boost::bind( &JloDisplayBase::getCL, this ),
00263                                                                       boost::bind( &JloDisplayBase::setCL, this, _1 ), extended_props_p_, this );
00264   cov_thickness_p_ = property_manager_->createProperty<rviz::FloatProperty>( "Covariance Thickness", property_prefix_, boost::bind( &JloDisplayBase::getCT, this ),
00265                                                                       boost::bind( &JloDisplayBase::setCT, this, _1 ), extended_props_p_, this );
00266 }
00267 
00268 } // namespace mapping_rviz_plugin
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


cop_rviz_plugin
Author(s): U. Klank, Josh Faust
autogenerated on Thu May 23 2013 10:50:43