selection_handler.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include "selection_handler.h"
31 
32 #include "properties/property.h"
33 #include "visualization_manager.h"
34 
35 #include <ros/assert.h>
36 
37 #include <OgreSceneNode.h>
38 #include <OgreSceneManager.h>
39 #include <OgreManualObject.h>
40 #include <OgreWireBoundingBox.h>
41 #include <OgreEntity.h>
42 #include <OgreSubEntity.h>
43 
45 
46 namespace rviz
47 {
48 
50  : context_( context )
51  , listener_( new Listener( this ))
52 {
55 }
56 
58 {
59  S_Movable::iterator it = tracked_objects_.begin();
60  S_Movable::iterator end = tracked_objects_.end();
61  for (; it != end; ++it)
62  {
63  Ogre::MovableObject* m = *it;
64  m->setListener(0);
65  }
66 
67  while (!boxes_.empty())
68  {
69  destroyBox(boxes_.begin()->first);
70  }
72 }
73 
75 {
76  M_HandleToBox::iterator it = boxes_.begin();
77  M_HandleToBox::iterator end = boxes_.end();
78  for (; it != end; ++it)
79  {
80  Ogre::WireBoundingBox* box = it->second.second;
81  box->setVisible(false);
82  }
83 }
84 
86 {
87  M_HandleToBox::iterator it = boxes_.begin();
88  M_HandleToBox::iterator end = boxes_.end();
89  for (; it != end; ++it)
90  {
91  Ogre::WireBoundingBox* box = it->second.second;
92  box->setVisible(true);
93  }
94 }
95 
96 
97 void SelectionHandler::addTrackedObjects( Ogre::SceneNode* node )
98 {
99  if (!node)
100  {
101  return;
102  }
103  // Loop over all objects attached to this node.
104  Ogre::SceneNode::ObjectIterator obj_it = node->getAttachedObjectIterator();
105  while( obj_it.hasMoreElements() )
106  {
107  Ogre::MovableObject* obj = obj_it.getNext();
108  addTrackedObject( obj );
109  }
110  // Loop over and recurse into all child nodes.
111  Ogre::SceneNode::ChildNodeIterator child_it = node->getChildIterator();
112  while( child_it.hasMoreElements() )
113  {
114  Ogre::SceneNode* child = dynamic_cast<Ogre::SceneNode*>( child_it.getNext() );
115  addTrackedObjects( child );
116  }
117 }
118 
119 void SelectionHandler::addTrackedObject(Ogre::MovableObject* object)
120 {
121  tracked_objects_.insert(object);
122  object->setListener(listener_.get());
123 
125 }
126 
127 void SelectionHandler::removeTrackedObject(Ogre::MovableObject* object)
128 {
129  tracked_objects_.erase(object);
130  object->setListener(0);
131 
133 }
134 
136 {
137  M_HandleToBox::iterator it = boxes_.begin();
138  M_HandleToBox::iterator end = boxes_.end();
139  for (; it != end; ++it)
140  {
141  V_AABB aabbs;
142  Picked p(it->first.first);
143  p.extra_handles.insert(it->first.second);
144  getAABBs(Picked(it->first.first), aabbs);
145 
146  if (!aabbs.empty())
147  {
148  Ogre::AxisAlignedBox combined;
149  V_AABB::iterator aabb_it = aabbs.begin();
150  V_AABB::iterator aabb_end = aabbs.end();
151  for (; aabb_it != aabb_end; ++aabb_it)
152  {
153  combined.merge(*aabb_it);
154  }
155 
156  createBox(std::make_pair(p.handle, it->first.second), combined, "RVIZ/Cyan");
157  }
158  }
159 }
160 
161 void SelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs)
162 {
163  S_Movable::iterator it = tracked_objects_.begin();
164  S_Movable::iterator end = tracked_objects_.end();
165  for (; it != end; ++it)
166  {
167  aabbs.push_back((*it)->getWorldBoundingBox());
168  }
169 }
170 
171 void SelectionHandler::destroyProperties( const Picked& obj, Property* parent_property )
172 {
173  for( int i = 0; i < properties_.size(); i++ )
174  {
175  delete properties_.at( i );
176  }
177  properties_.clear();
178 }
179 
180 void SelectionHandler::createBox(const std::pair<CollObjectHandle, uint64_t>& handles, const Ogre::AxisAlignedBox& aabb, const std::string& material_name)
181 {
182  Ogre::WireBoundingBox* box = 0;
183  Ogre::SceneNode* node = 0;
184 
185  M_HandleToBox::iterator it = boxes_.find(handles);
186  if (it == boxes_.end())
187  {
188  Ogre::SceneManager* scene_manager = context_->getSceneManager();
189  node = scene_manager->getRootSceneNode()->createChildSceneNode();
190  box = new Ogre::WireBoundingBox;
191 
192  bool inserted = boxes_.insert(std::make_pair(handles, std::make_pair(node, box))).second;
193  ROS_ASSERT(inserted);
194  }
195  else
196  {
197  node = it->second.first;
198  box = it->second.second;
199  }
200 
201  box->setMaterial(material_name);
202 
203  box->setupBoundingBox(aabb);
204  node->detachAllObjects();
205  node->attachObject(box);
206 }
207 
208 void SelectionHandler::destroyBox(const std::pair<CollObjectHandle, uint64_t>& handles)
209 {
210  M_HandleToBox::iterator it = boxes_.find(handles);
211  if (it != boxes_.end())
212  {
213  Ogre::SceneNode* node = it->second.first;
214  Ogre::WireBoundingBox* box = it->second.second;
215 
216  node->detachAllObjects();
217  node->getParentSceneNode()->removeAndDestroyChild(node->getName());
218 
219  delete box;
220 
221  boxes_.erase(it);
222  }
223 }
224 
226 {
227  ROS_DEBUG("Selected 0x%08x", obj.handle);
228 
229  V_AABB aabbs;
230  getAABBs(obj, aabbs);
231 
232  if (!aabbs.empty())
233  {
234  Ogre::AxisAlignedBox combined;
235  V_AABB::iterator it = aabbs.begin();
236  V_AABB::iterator end = aabbs.end();
237  for (; it != end; ++it)
238  {
239  combined.merge(*it);
240  }
241 
242  createBox(std::make_pair(obj.handle, 0ULL), combined, "RVIZ/Cyan");
243  }
244 }
245 
247 {
248  ROS_DEBUG("Deselected 0x%08x", obj.handle);
249 
250  destroyBox(std::make_pair(obj.handle, 0ULL));
251 }
252 
254 {
255  interactive_object_ = object;
256 }
257 
259 {
260  return interactive_object_;
261 }
262 
263 }
void removeTrackedObject(Ogre::MovableObject *object)
CollObjectHandle createHandle()
void destroyBox(const std::pair< CollObjectHandle, uint64_t > &handles)
Destroy the box associated with the given handle-int pair, if there is one.
virtual void updateTrackedBoxes()
void createBox(const std::pair< CollObjectHandle, uint64_t > &handles, const Ogre::AxisAlignedBox &aabb, const std::string &material_name)
Create or update a box for the given handle-int pair, with the box specified by aabb.
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
S_uint64 extra_handles
Definition: forwards.h:63
void removeObject(CollObjectHandle obj)
virtual InteractiveObjectWPtr getInteractiveObject()
Get the object to listen to mouse events and other interaction calls during use of the &#39;interact&#39; too...
virtual void preRenderPass(uint32_t pass)
void addObject(CollObjectHandle obj, SelectionHandler *handler)
CollObjectHandle handle
Definition: forwards.h:61
static void setPickHandle(CollObjectHandle handle, Ogre::SceneNode *node)
Pure-virtual base class for objects which give Display subclasses context in which to work...
std::vector< Ogre::AxisAlignedBox > V_AABB
DisplayContext * context_
void addTrackedObject(Ogre::MovableObject *object)
virtual void getAABBs(const Picked &obj, V_AABB &aabbs)
virtual SelectionManager * getSelectionManager() const =0
Return a pointer to the SelectionManager.
SelectionHandler(DisplayContext *context)
virtual void setInteractiveObject(InteractiveObjectWPtr object)
Set an object to listen to mouse events and other interaction calls during use of the &#39;interact&#39; tool...
InteractiveObjectWPtr interactive_object_
void addTrackedObjects(Ogre::SceneNode *node)
virtual void onSelect(const Picked &obj)
boost::weak_ptr< InteractiveObject > InteractiveObjectWPtr
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
virtual void destroyProperties(const Picked &obj, Property *parent_property)
Destroy all properties for the given picked object(s).
CollObjectHandle pick_handle_
virtual void postRenderPass(uint32_t pass)
QList< Property * > properties_
virtual void onDeselect(const Picked &obj)
#define ROS_ASSERT(cond)
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51