marker_base.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, 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 "marker_base.h"
32 #include "rviz/display_context.h"
35 #include "rviz/frame_manager.h"
36 
37 #include <OgreSceneNode.h>
38 #include <OgreSceneManager.h>
39 #include <OgreEntity.h>
40 #include <OgreSubEntity.h>
41 #include <OgreSharedPtr.h>
42 
43 #include <tf/tf.h>
44 #include <tf/transform_listener.h>
45 
46 namespace rviz
47 {
48 
49 MarkerBase::MarkerBase( MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node )
50  : owner_( owner )
51  , context_( context )
52  , scene_node_( parent_node->createChildSceneNode() )
53 {}
54 
56 {
57  context_->getSceneManager()->destroySceneNode(scene_node_);
58 }
59 
60 void MarkerBase::setMessage(const Marker& message)
61 {
62  // copy and save to shared pointer
63  MarkerConstPtr message_ptr( new Marker(message) );
64  setMessage( message_ptr );
65 }
66 
68 {
70  message_ = message;
71 
72  expiration_ = ros::Time::now() + message->lifetime;
73 
74  onNewMessage(old, message);
75 }
76 
78 {
79  ROS_ASSERT(message_ && message_->frame_locked);
81 }
82 
84 {
85  return ros::Time::now() >= expiration_;
86 }
87 
88 bool MarkerBase::transform(const MarkerConstPtr& message, Ogre::Vector3& pos, Ogre::Quaternion& orient, Ogre::Vector3& scale)
89 {
90  ros::Time stamp = message->header.stamp;
91  if (message->frame_locked)
92  {
93  stamp = ros::Time();
94  }
95 
96  if (!context_->getFrameManager()->transform(message->header.frame_id, stamp, message->pose, pos, orient))
97  {
98  std::string error;
99  context_->getFrameManager()->transformHasProblems(message->header.frame_id, message->header.stamp, error);
100  if ( owner_ )
101  {
103  }
104  return false;
105  }
106 
107  scale = Ogre::Vector3(message->scale.x, message->scale.y, message->scale.z);
108 
109  return true;
110 }
111 
113 {
114  if( handler_ )
115  {
116  handler_->setInteractiveObject( control );
117  }
118 }
119 
120 void MarkerBase::setPosition( const Ogre::Vector3& position )
121 {
122  scene_node_->setPosition( position );
123 }
124 
125 void MarkerBase::setOrientation( const Ogre::Quaternion& orientation )
126 {
127  scene_node_->setOrientation( orientation );
128 }
129 
130 const Ogre::Vector3& MarkerBase::getPosition()
131 {
132  return scene_node_->getPosition();
133 }
134 
135 const Ogre::Quaternion& MarkerBase::getOrientation()
136 {
137  return scene_node_->getOrientation();
138 }
139 
140 void MarkerBase::extractMaterials( Ogre::Entity *entity, S_MaterialPtr &materials )
141 {
142  uint32_t num_sub_entities = entity->getNumSubEntities();
143  for (uint32_t i = 0; i < num_sub_entities; ++i)
144  {
145  Ogre::SubEntity* sub = entity->getSubEntity(i);
146  Ogre::MaterialPtr material = sub->getMaterial();
147  materials.insert( material );
148  }
149 }
150 
151 
152 
153 } // namespace rviz
void setMessage(const Marker &message)
Definition: marker_base.cpp:60
Ogre::SceneNode * scene_node_
Definition: marker_base.h:104
bool transform(const MarkerConstPtr &message, Ogre::Vector3 &pos, Ogre::Quaternion &orient, Ogre::Vector3 &scale)
Definition: marker_base.cpp:88
const Ogre::Quaternion & getOrientation()
bool transformHasProblems(const std::string &frame, ros::Time time, std::string &error)
Check to see if a transform is known between a given frame and the fixed frame.
virtual ~MarkerBase()
Definition: marker_base.cpp:55
void setMarkerStatus(MarkerID id, StatusLevel level, const std::string &text)
const Ogre::Vector3 & getPosition()
virtual void setPosition(const Ogre::Vector3 &position)
void updateFrameLocked()
Definition: marker_base.cpp:77
MarkerDisplay * owner_
Definition: marker_base.h:101
void extractMaterials(Ogre::Entity *entity, S_MaterialPtr &materials)
Pure-virtual base class for objects which give Display subclasses context in which to work...
boost::shared_ptr< MarkerSelectionHandler > handler_
Definition: marker_base.h:110
ros::Time expiration_
Definition: marker_base.h:108
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Transform a pose from a frame into the fixed frame.
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
DisplayContext * context_
Definition: marker_base.h:102
MarkerConstPtr message_
Definition: marker_base.h:106
MarkerID getID()
Definition: marker_base.h:77
visualization_msgs::Marker::ConstPtr MarkerConstPtr
Definition: marker_base.h:63
boost::weak_ptr< InteractiveObject > InteractiveObjectWPtr
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
void setInteractiveObject(InteractiveObjectWPtr object)
Associate an InteractiveObject with this MarkerBase.
std::set< Ogre::MaterialPtr > S_MaterialPtr
Definition: marker_base.h:57
virtual void setOrientation(const Ogre::Quaternion &orientation)
static Time now()
#define ROS_ASSERT(cond)
MarkerBase(MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node)
Definition: marker_base.cpp:49
Displays "markers" sent in by other ROS nodes on the "visualization_marker" topic.
virtual void onNewMessage(const MarkerConstPtr &old_message, const MarkerConstPtr &new_message)=0
visualization_msgs::Marker Marker
Definition: marker_base.h:62


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