selection_handler.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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 "selection_handler.h"
00031 
00032 #include "properties/property.h"
00033 #include "visualization_manager.h"
00034 
00035 #include <ros/assert.h>
00036 
00037 #include <OgreSceneNode.h>
00038 #include <OgreSceneManager.h>
00039 #include <OgreManualObject.h>
00040 #include <OgreWireBoundingBox.h>
00041 #include <OgreEntity.h>
00042 #include <OgreSubEntity.h>
00043 
00044 #include "rviz/selection/selection_manager.h"
00045 
00046 namespace rviz
00047 {
00048 
00049 SelectionHandler::SelectionHandler( DisplayContext* context )
00050   : context_( context )
00051   , listener_( new Listener( this ))
00052 {
00053   pick_handle_ = context_->getSelectionManager()->createHandle();
00054   context_->getSelectionManager()->addObject( pick_handle_, this );
00055 }
00056 
00057 SelectionHandler::~SelectionHandler()
00058 {
00059   S_Movable::iterator it = tracked_objects_.begin();
00060   S_Movable::iterator end = tracked_objects_.end();
00061   for (; it != end; ++it)
00062   {
00063     Ogre::MovableObject* m = *it;
00064     m->setListener(0);
00065   }
00066 
00067   while (!boxes_.empty())
00068   {
00069     destroyBox(boxes_.begin()->first);
00070   }
00071   context_->getSelectionManager()->removeObject( pick_handle_ );
00072 }
00073 
00074 void SelectionHandler::preRenderPass(uint32_t pass)
00075 {
00076   M_HandleToBox::iterator it = boxes_.begin();
00077   M_HandleToBox::iterator end = boxes_.end();
00078   for (; it != end; ++it)
00079   {
00080     Ogre::WireBoundingBox* box = it->second.second;
00081     box->setVisible(false);
00082   }
00083 }
00084 
00085 void SelectionHandler::postRenderPass(uint32_t pass)
00086 {
00087   M_HandleToBox::iterator it = boxes_.begin();
00088   M_HandleToBox::iterator end = boxes_.end();
00089   for (; it != end; ++it)
00090   {
00091     Ogre::WireBoundingBox* box = it->second.second;
00092     box->setVisible(true);
00093   }
00094 }
00095 
00096 
00097 void SelectionHandler::addTrackedObjects( Ogre::SceneNode* node )
00098 {
00099   if (!node)
00100   {
00101     return;
00102   }
00103   // Loop over all objects attached to this node.
00104   Ogre::SceneNode::ObjectIterator obj_it = node->getAttachedObjectIterator();
00105   while( obj_it.hasMoreElements() )
00106   {
00107     Ogre::MovableObject* obj = obj_it.getNext();
00108     addTrackedObject( obj );
00109   }
00110   // Loop over and recurse into all child nodes.
00111   Ogre::SceneNode::ChildNodeIterator child_it = node->getChildIterator();
00112   while( child_it.hasMoreElements() )
00113   {
00114     Ogre::SceneNode* child = dynamic_cast<Ogre::SceneNode*>( child_it.getNext() );
00115     addTrackedObjects( child );
00116   }
00117 }
00118 
00119 void SelectionHandler::addTrackedObject(Ogre::MovableObject* object)
00120 {
00121   tracked_objects_.insert(object);
00122   object->setListener(listener_.get());
00123 
00124   SelectionManager::setPickHandle( pick_handle_, object );
00125 }
00126 
00127 void SelectionHandler::removeTrackedObject(Ogre::MovableObject* object)
00128 {
00129   tracked_objects_.erase(object);
00130   object->setListener(0);
00131 
00132   updateTrackedBoxes();
00133 }
00134 
00135 void SelectionHandler::updateTrackedBoxes()
00136 {
00137   M_HandleToBox::iterator it = boxes_.begin();
00138   M_HandleToBox::iterator end = boxes_.end();
00139   for (; it != end; ++it)
00140   {
00141     V_AABB aabbs;
00142     Picked p(it->first.first);
00143     p.extra_handles.insert(it->first.second);
00144     getAABBs(Picked(it->first.first), aabbs);
00145 
00146     if (!aabbs.empty())
00147     {
00148       Ogre::AxisAlignedBox combined;
00149       V_AABB::iterator aabb_it = aabbs.begin();
00150       V_AABB::iterator aabb_end = aabbs.end();
00151       for (; aabb_it != aabb_end; ++aabb_it)
00152       {
00153         combined.merge(*aabb_it);
00154       }
00155 
00156       createBox(std::make_pair(p.handle, it->first.second), combined, "RVIZ/Cyan");
00157     }
00158   }
00159 }
00160 
00161 void SelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs)
00162 {
00163   S_Movable::iterator it = tracked_objects_.begin();
00164   S_Movable::iterator end = tracked_objects_.end();
00165   for (; it != end; ++it)
00166   {
00167     aabbs.push_back((*it)->getWorldBoundingBox());
00168   }
00169 }
00170 
00171 void SelectionHandler::destroyProperties( const Picked& obj, Property* parent_property )
00172 {
00173   for( int i = 0; i < properties_.size(); i++ )
00174   {
00175     delete properties_.at( i );
00176   }
00177   properties_.clear();
00178 }
00179 
00180 void SelectionHandler::createBox(const std::pair<CollObjectHandle, uint64_t>& handles, const Ogre::AxisAlignedBox& aabb, const std::string& material_name)
00181 {
00182   Ogre::WireBoundingBox* box = 0;
00183   Ogre::SceneNode* node = 0;
00184 
00185   M_HandleToBox::iterator it = boxes_.find(handles);
00186   if (it == boxes_.end())
00187   {
00188     Ogre::SceneManager* scene_manager = context_->getSceneManager();
00189     node = scene_manager->getRootSceneNode()->createChildSceneNode();
00190     box = new Ogre::WireBoundingBox;
00191 
00192     bool inserted = boxes_.insert(std::make_pair(handles, std::make_pair(node, box))).second;
00193     ROS_ASSERT(inserted);
00194   }
00195   else
00196   {
00197     node = it->second.first;
00198     box = it->second.second;
00199   }
00200 
00201   box->setMaterial(material_name);
00202 
00203   box->setupBoundingBox(aabb);
00204   node->detachAllObjects();
00205   node->attachObject(box);
00206 }
00207 
00208 void SelectionHandler::destroyBox(const std::pair<CollObjectHandle, uint64_t>& handles)
00209 {
00210   M_HandleToBox::iterator it = boxes_.find(handles);
00211   if (it != boxes_.end())
00212   {
00213     Ogre::SceneNode* node = it->second.first;
00214     Ogre::WireBoundingBox* box = it->second.second;
00215 
00216     node->detachAllObjects();
00217     node->getParentSceneNode()->removeAndDestroyChild(node->getName());
00218 
00219     delete box;
00220 
00221     boxes_.erase(it);
00222   }
00223 }
00224 
00225 void SelectionHandler::onSelect(const Picked& obj)
00226 {
00227   ROS_DEBUG("Selected 0x%08x", obj.handle);
00228 
00229   V_AABB aabbs;
00230   getAABBs(obj, aabbs);
00231 
00232   if (!aabbs.empty())
00233   {
00234     Ogre::AxisAlignedBox combined;
00235     V_AABB::iterator it = aabbs.begin();
00236     V_AABB::iterator end = aabbs.end();
00237     for (; it != end; ++it)
00238     {
00239       combined.merge(*it);
00240     }
00241 
00242     createBox(std::make_pair(obj.handle, 0ULL), combined, "RVIZ/Cyan");
00243   }
00244 }
00245 
00246 void SelectionHandler::onDeselect(const Picked& obj)
00247 {
00248   ROS_DEBUG("Deselected 0x%08x", obj.handle);
00249 
00250   destroyBox(std::make_pair(obj.handle, 0ULL));
00251 }
00252 
00253 void SelectionHandler::setInteractiveObject( InteractiveObjectWPtr object )
00254 {
00255   interactive_object_ = object;
00256 }
00257 
00258 InteractiveObjectWPtr SelectionHandler::getInteractiveObject()
00259 {
00260   return interactive_object_;
00261 }
00262 
00263 }


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