points_marker.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 "points_marker.h"
32 #include "rviz/display_context.h"
35 
37 
38 #include <OgreVector3.h>
39 #include <OgreQuaternion.h>
40 #include <OgreSceneNode.h>
41 #include <OgreSceneManager.h>
42 
43 namespace rviz
44 {
45 
46 PointsMarker::PointsMarker(MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node)
47 : MarkerBase(owner, context, parent_node)
48 , points_(0)
49 {
50 }
51 
53 {
54  delete points_;
55 }
56 
57 void PointsMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
58 {
59  ROS_ASSERT(new_message->type == visualization_msgs::Marker::POINTS ||
60  new_message->type == visualization_msgs::Marker::CUBE_LIST ||
61  new_message->type == visualization_msgs::Marker::SPHERE_LIST);
62 
63  if (!points_)
64  {
65  points_ = new PointCloud();
66  scene_node_->attachObject(points_);
67  }
68 
69  Ogre::Vector3 pos, scale;
70  Ogre::Quaternion orient;
71  transform(new_message, pos, orient, scale);
72 
73  switch (new_message->type)
74  {
75  case visualization_msgs::Marker::POINTS:
77  points_->setDimensions(new_message->scale.x, new_message->scale.y, 0.0f);
78  break;
79  case visualization_msgs::Marker::CUBE_LIST:
81  points_->setDimensions(scale.x, scale.y, scale.z);
82  break;
83  case visualization_msgs::Marker::SPHERE_LIST:
85  points_->setDimensions(scale.x, scale.y, scale.z);
86  break;
87  }
88 
89  setPosition(pos);
90  setOrientation(orient);
91 
92  points_->clear();
93 
94  if (new_message->points.empty())
95  {
96  return;
97  }
98 
99  float r = new_message->color.r;
100  float g = new_message->color.g;
101  float b = new_message->color.b;
102  float a = new_message->color.a;
103 
104  bool has_per_point_color = new_message->colors.size() == new_message->points.size();
105 
106  bool has_nonzero_alpha = false;
107  bool has_per_point_alpha = false;
108 
109  typedef std::vector< PointCloud::Point > V_Point;
110  V_Point points;
111  points.resize(new_message->points.size());
112  std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
113  std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
114  for (int i = 0; it != end; ++it, ++i)
115  {
116  const geometry_msgs::Point& p = *it;
117  PointCloud::Point& point = points[i];
118 
119  Ogre::Vector3 v(p.x, p.y, p.z);
120 
121  point.position.x = v.x;
122  point.position.y = v.y;
123  point.position.z = v.z;
124 
125  if (has_per_point_color)
126  {
127  const std_msgs::ColorRGBA& color = new_message->colors[i];
128  r = color.r;
129  g = color.g;
130  b = color.b;
131  a = color.a;
132  has_nonzero_alpha = has_nonzero_alpha || a != 0.0;
133  has_per_point_alpha = has_per_point_alpha || a != 1.0;
134  }
135 
136  point.setColor(r, g, b, a);
137  }
138 
139  if (has_per_point_color)
140  {
141  if (!has_nonzero_alpha && owner_)
142  {
143  owner_->setMarkerStatus(getID(), StatusProperty::Warn, "All points have a zero alpha value.");
144  }
145  points_->setAlpha(1.0, has_per_point_alpha);
146  }
147  else
148  {
149  points_->setAlpha(a);
150  }
151 
152  points_->addPoints(&points.front(), points.size());
153 
154  handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
156 }
157 
158 void PointsMarker::setHighlightColor( float r, float g, float b )
159 {
160  points_->setHighlightColor( r, g, b );
161 }
162 
163 } // end namespace rviz
void addPoints(Point *points, uint32_t num_points)
Add points to this point cloud.
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
void setRenderMode(RenderMode mode)
Set what type of rendering primitives should be used, currently points, billboards and boxes are supp...
void setMarkerStatus(MarkerID id, StatusLevel level, const std::string &text)
void setPickColor(const Ogre::ColourValue &color)
PointsMarker(MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node)
static Ogre::ColourValue handleToColor(CollObjectHandle handle)
virtual void setPosition(const Ogre::Vector3 &position)
MarkerDisplay * owner_
Definition: marker_base.h:101
std::pair< std::string, int32_t > MarkerID
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
Representation of a point, with x/y/z position and r/g/b color.
Definition: point_cloud.h:123
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:130
DisplayContext * context_
Definition: marker_base.h:102
void setAlpha(float alpha, bool per_point_alpha=false)
void clear()
Clear all the points.
MarkerID getID()
Definition: marker_base.h:77
PointCloud * points_
Definition: points_marker.h:58
visualization_msgs::Marker::ConstPtr MarkerConstPtr
Definition: marker_base.h:63
virtual void onNewMessage(const MarkerConstPtr &old_message, const MarkerConstPtr &new_message)
void setHighlightColor(float r, float g, float b)
A visual representation of a set of points.
Definition: point_cloud.h:98
virtual void setOrientation(const Ogre::Quaternion &orientation)
#define ROS_ASSERT(cond)
r
Displays "markers" sent in by other ROS nodes on the "visualization_marker" topic.
void setHighlightColor(float r, float g, float b)
void setColor(float r, float g, float b, float a=1.0)
Definition: point_cloud.h:125


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:42