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 namespace rviz
00045 {
00046
00047 SelectionHandler::SelectionHandler()
00048 : manager_(0)
00049 , listener_(new Listener(this))
00050 {
00051 }
00052
00053 SelectionHandler::~SelectionHandler()
00054 {
00055 S_Movable::iterator it = tracked_objects_.begin();
00056 S_Movable::iterator end = tracked_objects_.end();
00057 for (; it != end; ++it)
00058 {
00059 Ogre::MovableObject* m = *it;
00060 m->setListener(0);
00061 }
00062
00063 while (!boxes_.empty())
00064 {
00065 destroyBox(boxes_.begin()->first);
00066 }
00067 }
00068
00069 void SelectionHandler::initialize(VisualizationManager* manager)
00070 {
00071 manager_ = manager;
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 void SelectionHandler::addTrackedObject(Ogre::MovableObject* object)
00097 {
00098 tracked_objects_.insert(object);
00099 object->setListener(listener_.get());
00100 }
00101
00102 void SelectionHandler::removeTrackedObject(Ogre::MovableObject* object)
00103 {
00104 tracked_objects_.erase(object);
00105 object->setListener(0);
00106
00107 updateTrackedBoxes();
00108 }
00109
00110 void SelectionHandler::updateTrackedBoxes()
00111 {
00112 M_HandleToBox::iterator it = boxes_.begin();
00113 M_HandleToBox::iterator end = boxes_.end();
00114 for (; it != end; ++it)
00115 {
00116 V_AABB aabbs;
00117 Picked p(it->first.first);
00118 p.extra_handles.insert(it->first.second);
00119 getAABBs(Picked(it->first.first), aabbs);
00120
00121 if (!aabbs.empty())
00122 {
00123 Ogre::AxisAlignedBox combined;
00124 V_AABB::iterator aabb_it = aabbs.begin();
00125 V_AABB::iterator aabb_end = aabbs.end();
00126 for (; aabb_it != aabb_end; ++aabb_it)
00127 {
00128 combined.merge(*aabb_it);
00129 }
00130
00131 createBox(std::make_pair(p.handle, it->first.second), combined, "RVIZ/Cyan");
00132 }
00133 }
00134 }
00135
00136 void SelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs)
00137 {
00138 S_Movable::iterator it = tracked_objects_.begin();
00139 S_Movable::iterator end = tracked_objects_.end();
00140 for (; it != end; ++it)
00141 {
00142 aabbs.push_back((*it)->getWorldBoundingBox());
00143 }
00144 }
00145
00146 void SelectionHandler::destroyProperties( const Picked& obj, Property* parent_property )
00147 {
00148 for( int i = 0; i < properties_.size(); i++ )
00149 {
00150 delete properties_.at( i );
00151 }
00152 properties_.clear();
00153 }
00154
00155 void SelectionHandler::createBox(const std::pair<CollObjectHandle, uint64_t>& handles, const Ogre::AxisAlignedBox& aabb, const std::string& material_name)
00156 {
00157 Ogre::WireBoundingBox* box = 0;
00158 Ogre::SceneNode* node = 0;
00159
00160 M_HandleToBox::iterator it = boxes_.find(handles);
00161 if (it == boxes_.end())
00162 {
00163 Ogre::SceneManager* scene_manager = manager_->getSceneManager();
00164 node = scene_manager->getRootSceneNode()->createChildSceneNode();
00165 box = new Ogre::WireBoundingBox;
00166
00167 bool inserted = boxes_.insert(std::make_pair(handles, std::make_pair(node, box))).second;
00168 ROS_ASSERT(inserted);
00169 }
00170 else
00171 {
00172 node = it->second.first;
00173 box = it->second.second;
00174 }
00175
00176 box->setMaterial(material_name);
00177
00178 box->setupBoundingBox(aabb);
00179 node->detachAllObjects();
00180 node->attachObject(box);
00181 }
00182
00183 void SelectionHandler::destroyBox(const std::pair<CollObjectHandle, uint64_t>& handles)
00184 {
00185 M_HandleToBox::iterator it = boxes_.find(handles);
00186 if (it != boxes_.end())
00187 {
00188 Ogre::SceneNode* node = it->second.first;
00189 Ogre::WireBoundingBox* box = it->second.second;
00190
00191 node->detachAllObjects();
00192 node->getParentSceneNode()->removeAndDestroyChild(node->getName());
00193
00194 delete box;
00195
00196 boxes_.erase(it);
00197 }
00198 }
00199
00200 void SelectionHandler::onSelect(const Picked& obj)
00201 {
00202 ROS_DEBUG("Selected 0x%08x", obj.handle);
00203
00204 V_AABB aabbs;
00205 getAABBs(obj, aabbs);
00206
00207 if (!aabbs.empty())
00208 {
00209 Ogre::AxisAlignedBox combined;
00210 V_AABB::iterator it = aabbs.begin();
00211 V_AABB::iterator end = aabbs.end();
00212 for (; it != end; ++it)
00213 {
00214 combined.merge(*it);
00215 }
00216
00217 createBox(std::make_pair(obj.handle, 0ULL), combined, "RVIZ/Cyan");
00218 }
00219 }
00220
00221 void SelectionHandler::onDeselect(const Picked& obj)
00222 {
00223 ROS_DEBUG("Deselected 0x%08x", obj.handle);
00224
00225 destroyBox(std::make_pair(obj.handle, 0ULL));
00226 }
00227
00228 void SelectionHandler::setInteractiveObject( InteractiveObjectWPtr object )
00229 {
00230 interactive_object_ = object;
00231 }
00232
00233 InteractiveObjectWPtr SelectionHandler::getInteractiveObject()
00234 {
00235 return interactive_object_;
00236 }
00237
00238 }