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