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