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 "selection_handler.h"
00031
00032 #include "properties/property.h"
00033 #include "visualization_manager.h"
00034
00035 #include <ros/assert.h>
00036
00037 #include <OGRE/OgreSceneNode.h>
00038 #include <OGRE/OgreSceneManager.h>
00039 #include <OGRE/OgreManualObject.h>
00040 #include <OGRE/OgreWireBoundingBox.h>
00041 #include <OGRE/OgreEntity.h>
00042 #include <OGRE/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
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
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 }