grid_cells_display.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 <boost/bind.hpp>
31 #include <OgreSceneNode.h>
32 #include <OgreSceneManager.h>
33 #include <OgreManualObject.h>
34 
35 #include "rviz/frame_manager.h"
42 #include "rviz/validate_floats.h"
43 #include "rviz/display_context.h"
44 
45 #include "grid_cells_display.h"
46 
47 namespace rviz
48 {
49 GridCellsDisplay::GridCellsDisplay() : Display(), messages_received_(0), last_frame_count_(uint64_t(-1))
50 {
51  color_property_ = new ColorProperty("Color", QColor(25, 255, 0), "Color of the grid cells.", this);
52 
53  alpha_property_ = new FloatProperty("Alpha", 1.0, "Amount of transparency to apply to the cells.",
54  this, SLOT(updateAlpha()));
57 
59  new RosTopicProperty("Topic", "",
60  QString::fromStdString(ros::message_traits::datatype<nav_msgs::GridCells>()),
61  "nav_msgs::GridCells topic to subscribe to.", this, SLOT(updateTopic()));
62 }
63 
65 {
66 // TODO(wjwwood): remove this and use tf2 interface instead
67 #ifndef _WIN32
68 #pragma GCC diagnostic push
69 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
70 #endif
71 
72  auto tf_client = context_->getTFClient();
73 
74 #ifndef _WIN32
75 #pragma GCC diagnostic pop
76 #endif
77 
78  tf_filter_ =
79  new tf::MessageFilter<nav_msgs::GridCells>(*tf_client, fixed_frame_.toStdString(), 10, update_nh_);
80  static int count = 0;
81  std::stringstream ss;
82  ss << "PolyLine" << count++;
83 
84  cloud_ = new PointCloud();
86  cloud_->setCommonDirection(Ogre::Vector3::UNIT_Z);
87  cloud_->setCommonUpVector(Ogre::Vector3::UNIT_Y);
88  scene_node_->attachObject(cloud_);
89  updateAlpha();
90 
93 // TODO(wjwwood): remove this and use tf2 interface instead
94 #ifndef _WIN32
95 #pragma GCC diagnostic push
96 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
97 #endif
98 
100 
101 #ifndef _WIN32
102 #pragma GCC diagnostic pop
103 #endif
104 }
105 
107 {
108  if (initialized())
109  {
110  unsubscribe();
111  clear();
112  scene_node_->detachObject(cloud_);
113  delete cloud_;
114  delete tf_filter_;
115  }
116 }
117 
119 {
120  cloud_->clear();
121 
122  messages_received_ = 0;
123  setStatus(StatusProperty::Warn, "Topic", "No messages received");
124 }
125 
127 {
128  unsubscribe();
129  subscribe();
131 }
132 
134 {
137 }
138 
140 {
141  if (!isEnabled())
142  {
143  return;
144  }
145 
146  try
147  {
149  setStatus(StatusProperty::Ok, "Topic", "OK");
150  }
151  catch (ros::Exception& e)
152  {
153  setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
154  }
155 }
156 
158 {
159  sub_.unsubscribe();
160 }
161 
163 {
164  subscribe();
165 }
166 
168 {
169  unsubscribe();
170  clear();
171 }
172 
174 {
175  clear();
176 
177  tf_filter_->setTargetFrame(fixed_frame_.toStdString());
178 }
179 
180 bool validateFloats(const nav_msgs::GridCells& msg)
181 {
182  bool valid = true;
183  valid = valid && validateFloats(msg.cell_width);
184  valid = valid && validateFloats(msg.cell_height);
185  valid = valid && validateFloats(msg.cells);
186  return valid;
187 }
188 
189 void GridCellsDisplay::incomingMessage(const nav_msgs::GridCells::ConstPtr& msg)
190 {
191  if (!msg)
192  {
193  return;
194  }
195 
197 
199  {
200  return;
201  }
203 
204  cloud_->clear();
205 
206  if (!validateFloats(*msg))
207  {
209  "Message contained invalid floating point values (nans or infs)");
210  return;
211  }
212 
213  setStatus(StatusProperty::Ok, "Topic", QString::number(messages_received_) + " messages received");
214 
215  Ogre::Vector3 position;
216  Ogre::Quaternion orientation;
217  if (!context_->getFrameManager()->getTransform(msg->header, position, orientation))
218  {
219  ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(),
220  qPrintable(fixed_frame_));
221  }
222 
223  scene_node_->setPosition(position);
224  scene_node_->setOrientation(orientation);
225 
226  if (msg->cell_width == 0)
227  {
228  setStatus(StatusProperty::Error, "Topic", "Cell width is zero, cells will be invisible.");
229  }
230  else if (msg->cell_height == 0)
231  {
232  setStatus(StatusProperty::Error, "Topic", "Cell height is zero, cells will be invisible.");
233  }
234 
235  cloud_->setDimensions(msg->cell_width, msg->cell_height, 0.0);
236 
237  Ogre::ColourValue color_int = qtToOgre(color_property_->getColor());
238  uint32_t num_points = msg->cells.size();
239 
240  typedef std::vector<PointCloud::Point> V_Point;
241  V_Point points;
242  points.resize(num_points);
243  for (uint32_t i = 0; i < num_points; i++)
244  {
245  PointCloud::Point& current_point = points[i];
246  current_point.position.x = msg->cells[i].x;
247  current_point.position.y = msg->cells[i].y;
248  current_point.position.z = msg->cells[i].z;
249  current_point.color = color_int;
250  }
251 
252  cloud_->clear();
253 
254  if (!points.empty())
255  {
256  cloud_->addPoints(&points.front(), points.size());
257  }
258 }
259 
261 {
262  Display::reset();
263  clear();
264 }
265 
266 void GridCellsDisplay::setTopic(const QString& topic, const QString& /*datatype*/)
267 {
268  topic_property_->setString(topic);
269 }
270 
271 } // namespace rviz
272 
void addPoints(Point *points, uint32_t num_points)
Add points to this point cloud.
void setMin(float min)
RosTopicProperty * topic_property_
virtual void setString(const QString &str)
void setMax(float max)
void connectInput(F &f)
message_filters::Subscriber< nav_msgs::GridCells > sub_
void setRenderMode(RenderMode mode)
Set what type of rendering primitives should be used, currently points, billboards and boxes are supp...
virtual tf::TransformListener * getTFClient() const =0
Convenience function: returns getFrameManager()->getTFClient().
virtual uint64_t getFrameCount() const =0
Return the current value of the frame count.
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
virtual QColor getColor() const
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:300
void setTopic(const QString &topic, const QString &datatype) override
Set the ROS topic to listen to for this display.
FloatProperty * alpha_property_
void incomingMessage(const nav_msgs::GridCells::ConstPtr &msg)
void onDisable() override
Derived classes override this to do the actual work of disabling themselves.
Property specialized to enforce floating point max/min.
std::string getTopicStd() const
tf::MessageFilter< nav_msgs::GridCells > * tf_filter_
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:295
Ogre::ColourValue qtToOgre(const QColor &c)
Definition: parse_color.cpp:83
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
void setCommonUpVector(const Ogre::Vector3 &vec)
See Ogre::BillboardSet::setCommonUpVector.
Displays a nav_msgs::GridCells message.
void onInitialize() override
Override this function to do subclass-specific initialization.
bool validateFloats(const sensor_msgs::CameraInfo &msg)
void reset() override
Called to tell the display to clear its state.
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:287
void registerFilterForTransformStatusCheck(tf::MessageFilter< M > *filter, Display *display)
Connect a tf::MessageFilter&#39;s callbacks to success and failure handler functions in this FrameManager...
ColorProperty * color_property_
Representation of a point, with x/y/z position and r/g/b color.
Definition: point_cloud.h:132
void setDimensions(float width, float height, float depth)
Set the dimensions of the billboards used to render each point.
Ogre::Vector3 position
Definition: point_cloud.h:139
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void setAlpha(float alpha, bool per_point_alpha=false)
void clear()
Clear all the points.
void onEnable() override
Derived classes override this to do the actual work of enabling themselves.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:268
Ogre::ColourValue color
Definition: point_cloud.h:140
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
A visual representation of a set of points.
Definition: point_cloud.h:107
virtual float getFloat() const
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
void setTargetFrame(const std::string &target_frame)
void setCommonDirection(const Ogre::Vector3 &vec)
See Ogre::BillboardSet::setCommonDirection.
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:275
void fixedFrameChanged() override
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:175
Connection registerCallback(const C &callback)
#define ROS_DEBUG(...)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Aug 15 2022 02:05:16