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 
39 #include <OgreQuaternion.h>
40 #include <OgreSceneNode.h>
41 #include <OgreSceneManager.h>
42 
43 namespace rviz
44 {
45 PointsMarker::PointsMarker(MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node)
46  : MarkerBase(owner, context, parent_node), points_(nullptr)
47 {
48 }
49 
51 {
52  delete points_;
53 }
54 
55 void PointsMarker::onNewMessage(const MarkerConstPtr& /*old_message*/, const MarkerConstPtr& new_message)
56 {
57  ROS_ASSERT(new_message->type == visualization_msgs::Marker::POINTS ||
58  new_message->type == visualization_msgs::Marker::CUBE_LIST ||
59  new_message->type == visualization_msgs::Marker::SPHERE_LIST);
60 
61  if (!points_)
62  {
63  points_ = new PointCloud();
64  scene_node_->attachObject(points_);
65  }
66 
67  Ogre::Vector3 pos, scale;
68  Ogre::Quaternion orient;
69  if (!transform(new_message, pos, orient, scale))
70  {
71  scene_node_->setVisible(false);
72  return;
73  }
74 
75  scene_node_->setVisible(true);
76 
77  switch (new_message->type)
78  {
79  case visualization_msgs::Marker::POINTS:
81  points_->setDimensions(new_message->scale.x, new_message->scale.y, 0.0f);
82  break;
83  case visualization_msgs::Marker::CUBE_LIST:
85  points_->setDimensions(scale.x, scale.y, scale.z);
86  break;
87  case visualization_msgs::Marker::SPHERE_LIST:
89  points_->setDimensions(scale.x, scale.y, scale.z);
90  break;
91  }
92 
93  setPosition(pos);
94  setOrientation(orient);
95 
96  points_->clear();
97 
98  if (new_message->points.empty())
99  {
100  return;
101  }
102 
103  float r = new_message->color.r;
104  float g = new_message->color.g;
105  float b = new_message->color.b;
106  float a = new_message->color.a;
107 
108  bool has_per_point_color = new_message->colors.size() == new_message->points.size();
109 
110  bool has_nonzero_alpha = false;
111  bool has_per_point_alpha = false;
112 
113  typedef std::vector<PointCloud::Point> V_Point;
114  V_Point points;
115  points.resize(new_message->points.size());
116  std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
117  std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
118  for (int i = 0; it != end; ++it, ++i)
119  {
120  const geometry_msgs::Point& p = *it;
121  PointCloud::Point& point = points[i];
122 
123  Ogre::Vector3 v(p.x, p.y, p.z);
124 
125  point.position.x = v.x;
126  point.position.y = v.y;
127  point.position.z = v.z;
128 
129  if (has_per_point_color)
130  {
131  const std_msgs::ColorRGBA& color = new_message->colors[i];
132  r = color.r;
133  g = color.g;
134  b = color.b;
135  a = color.a;
136  has_nonzero_alpha = has_nonzero_alpha || a != 0.0;
137  has_per_point_alpha = has_per_point_alpha || a != 1.0;
138  }
139 
140  point.setColor(r, g, b, a);
141  }
142 
143  if (has_per_point_color)
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
rviz::MarkerBase::context_
DisplayContext * context_
Definition: marker_base.h:108
rviz::PointCloud
A visual representation of a set of points.
Definition: point_cloud.h:108
rviz::PointsMarker::points_
PointCloud * points_
Definition: points_marker.h:57
points_marker.h
rviz::MarkerDisplay
Displays "markers" sent in by other ROS nodes on the "visualization_marker" topic.
Definition: marker_display.h:70
marker_selection_handler.h
rviz::PointCloud::setAlpha
void setAlpha(float alpha, bool per_point_alpha=false)
Definition: point_cloud.cpp:368
rviz::MarkerBase::setPosition
virtual void setPosition(const Ogre::Vector3 &position)
Definition: marker_base.cpp:124
rviz::MarkerBase
Definition: marker_base.h:53
rviz::PointCloud::addPoints
void addPoints(Point *points, uint32_t num_points)
Add points to this point cloud.
Definition: point_cloud.cpp:400
rviz::PointsMarker::setHighlightColor
void setHighlightColor(float r, float g, float b)
Definition: points_marker.cpp:158
marker_display.h
rviz::MarkerBase::scene_node_
Ogre::SceneNode * scene_node_
Definition: marker_base.h:110
rviz::MarkerBase::setOrientation
virtual void setOrientation(const Ogre::Quaternion &orientation)
Definition: marker_base.cpp:129
point_cloud.h
rviz::MarkerBase::handler_
boost::shared_ptr< MarkerSelectionHandler > handler_
Definition: marker_base.h:116
selection_manager.h
rviz::PointCloud::RM_SPHERES
@ RM_SPHERES
Definition: point_cloud.h:116
rviz::PointCloud::Point
Representation of a point, with x/y/z position and r/g/b color.
Definition: point_cloud.h:133
rviz::PointsMarker::PointsMarker
PointsMarker(MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node)
Definition: points_marker.cpp:45
rviz::PointCloud::RM_SQUARES
@ RM_SQUARES
Definition: point_cloud.h:114
rviz::MarkerBase::transform
bool transform(const MarkerConstPtr &message, Ogre::Vector3 &pos, Ogre::Quaternion &orient, Ogre::Vector3 &scale)
Definition: marker_base.cpp:87
rviz
Definition: add_display_dialog.cpp:54
rviz::PointsMarker::~PointsMarker
~PointsMarker() override
Definition: points_marker.cpp:50
rviz::MarkerSelectionHandler
Definition: marker_selection_handler.h:45
ogre_vector.h
rviz::PointCloud::Point::setColor
void setColor(float r, float g, float b, float a=1.0)
Definition: point_cloud.h:135
rviz::PointCloud::setPickColor
void setPickColor(const Ogre::ColourValue &color)
Definition: point_cloud.cpp:703
rviz::DisplayContext
Pure-virtual base class for objects which give Display subclasses context in which to work.
Definition: display_context.h:81
rviz::PointCloud::setDimensions
void setDimensions(float width, float height, float depth)
Set the dimensions of the billboards used to render each point.
Definition: point_cloud.cpp:300
rviz::PointCloud::setRenderMode
void setRenderMode(RenderMode mode)
Set what type of rendering primitives should be used, currently points, billboards and boxes are supp...
Definition: point_cloud.cpp:218
rviz::PointCloud::Point::position
Ogre::Vector3 position
Definition: point_cloud.h:140
rviz::PointCloud::clear
void clear()
Clear all the points.
Definition: point_cloud.cpp:164
rviz::MarkerID
std::pair< std::string, int32_t > MarkerID
Definition: interactive_marker_display.h:58
rviz::SelectionManager::handleToColor
static Ogre::ColourValue handleToColor(CollObjectHandle handle)
Definition: selection_manager.cpp:1106
rviz::PointCloud::RM_BOXES
@ RM_BOXES
Definition: point_cloud.h:118
rviz::PointCloud::setHighlightColor
void setHighlightColor(float r, float g, float b)
Definition: point_cloud.cpp:206
display_context.h
rviz::PointsMarker::onNewMessage
void onNewMessage(const MarkerConstPtr &old_message, const MarkerConstPtr &new_message) override
Definition: points_marker.cpp:55
ROS_ASSERT
#define ROS_ASSERT(cond)
rviz::MarkerBase::MarkerConstPtr
visualization_msgs::Marker::ConstPtr MarkerConstPtr
Definition: marker_base.h:57


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:10