VoronoiGraphDisplay.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, 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 <OGRE/OgreSceneNode.h>
31 #include <OGRE/OgreSceneManager.h>
32 
33 #include <tf/transform_listener.h>
34 
39 #include <rviz/frame_manager.h>
40 
41 
44 
45 namespace tuw_multi_robot_rviz
46 {
47 
48 
49 // The constructor must have no arguments, so we can't give the
50 // constructor the parameters it needs to fully initialize.
52  color_path_property_ = new rviz::ColorProperty ( "Path Color", QColor ( 50, 51, 204 ),
53  "Color to draw the path.",
54  this, SLOT ( updatePathColor() ) );
55 
56  scale_point_property_ = new rviz::FloatProperty ( "Path Points Scale", 0.1,
57  "Scale of the path points.",
58  this, SLOT ( updatePointScale() ) );
61 
62 
63  history_length_property_ = new rviz::IntProperty ( "History Length", 1,
64  "Number of prior measurements to display.",
65  this, SLOT ( updateHistoryLength() ) );
67  history_length_property_->setMax ( 100000 );
68 }
69 
70 // After the top-level rviz::Display::initialize() does its own setup,
71 // it calls the subclass's onInitialize() function. This is where we
72 // instantiate all the workings of the class. We make sure to also
73 // call our immediate super-class's onInitialize() function, since it
74 // does important stuff setting up the message filter.
75 //
76 // Note that "MFDClass" is a typedef of
77 // ``MessageFilterDisplay<message type>``, to save typing that long
78 // templated class name every time you need to refer to the
79 // superclass.
83 }
84 
86 }
87 
88 // Clear the visuals by deleting their objects.
91  visuals_.clear();
92 }
93 
94 // Set the current color values for each visual.
96  Ogre::ColourValue color = color_path_property_->getOgreColor();
97  for ( auto& visualsI: visuals_ ) { visualsI->setPathColor ( color ); }
98 }
99 
100 // Set the number of past visuals to show.
102  visuals_.rset_capacity ( history_length_property_->getInt() );
103 }
104 
106 {
107  float scale = scale_point_property_->getFloat();
108  for ( auto& visualsI: visuals_ ) { visualsI->setPointScale ( scale ); }
109 }
110 
112 {
113  float scale = scale_path_property_->getFloat();
114  for ( auto& visualsI: visuals_ ) { visualsI->setPathScale ( scale ); }
115 }
116 
117 // This is our callback to handle an incoming message.
118 void VoronoiGraphDisplay::processMessage ( const tuw_multi_robot_msgs::Graph::ConstPtr& msg ) {
119  // Here we call the rviz::FrameManager to get the transform from the
120  // fixed frame to the frame in the header of this Imu message. If
121  // it fails, we can't do anything else so we return.
122  Ogre::Quaternion orientation;
123  Ogre::Vector3 position;
124  if ( !context_->getFrameManager()->getTransform ( msg->header.frame_id,
125  msg->header.stamp,
126  position, orientation ) ) {
127  ROS_DEBUG ( "Error transforming from frame '%s' to frame '%s'",
128  msg->header.frame_id.c_str(), qPrintable ( fixed_frame_ ) );
129  return;
130  }
131 
132  // We are keeping a circular buffer of visual pointers. This gets
133  // the next one, or creates and stores it if the buffer is not full
135  if ( visuals_.full() ) {
136  visual = visuals_.front();
137  } else {
138  visual.reset ( new VoronoiGraphVisual ( context_->getSceneManager(), scene_node_ ) );
139  }
140 
141  // Now set or update the contents of the chosen visual.
142  visual->setMessage ( msg );
143  visual->setFramePosition ( position );
144  visual->setFrameOrientation ( orientation );
145 
146  visual->setPathColor ( color_path_property_->getOgreColor() );
147 
148  // And send it to the end of the circular buffer
149  visuals_.push_back ( visual );
150 }
151 
152 
153 } // end namespace rviz_plugin_tutorials
154 
155 // Tell pluginlib about this class. It is important to do this in
156 // global scope, outside our package's namespace.
159 // END_TUTORIAL
void setMin(float min)
void processMessage(const tuw_multi_robot_msgs::Graph::ConstPtr &msg)
void setMax(float max)
Ogre::ColourValue getOgreColor() const
DisplayContext * context_
virtual int getInt() const
virtual float getFloat() const
void setMin(int min)
Ogre::SceneNode * scene_node_
void setMax(int max)
QString fixed_frame_
virtual FrameManager * getFrameManager() const =0
virtual Ogre::SceneManager * getSceneManager() const =0
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
boost::circular_buffer< boost::shared_ptr< VoronoiGraphVisual > > visuals_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_DEBUG(...)


tuw_multi_robot_rviz
Author(s): Benjamin Binder
autogenerated on Mon Jun 10 2019 15:42:40