render_shapes.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
41 
42 #include <OgreSceneNode.h>
43 #include <OgreSceneManager.h>
44 #include <OgreManualObject.h>
45 #include <OgreMaterialManager.h>
46 
49 
50 #include <rviz/display_context.h>
51 #include <rviz/robot/robot.h>
52 
53 #include <boost/lexical_cast.hpp>
54 #include <boost/math/constants/constants.hpp>
55 
56 #include <memory>
57 
58 namespace moveit_rviz_plugin
59 {
60 RenderShapes::RenderShapes(rviz::DisplayContext* context) : context_(context)
61 {
62 }
63 
65 {
66  clear();
67 }
68 
70 {
71  scene_shapes_.clear();
72  octree_voxel_grids_.clear();
73 }
74 
75 void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, const Eigen::Isometry3d& p,
76  OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode,
77  const rviz::Color& color, float alpha)
78 {
79  rviz::Shape* ogre_shape = nullptr;
80  Eigen::Vector3d translation = p.translation();
81  Ogre::Vector3 position(translation.x(), translation.y(), translation.z());
82  ASSERT_ISOMETRY(p) // unsanitized input, could contain a non-isometry
83  Eigen::Quaterniond q(p.linear());
84  Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z());
85 
86  switch (s->type)
87  {
88  case shapes::SPHERE:
89  {
90  ogre_shape = new rviz::Shape(rviz::Shape::Sphere, context_->getSceneManager(), node);
91  double d = 2.0 * static_cast<const shapes::Sphere*>(s)->radius;
92  ogre_shape->setScale(Ogre::Vector3(d, d, d));
93  }
94  break;
95  case shapes::BOX:
96  {
97  ogre_shape = new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(), node);
98  const double* sz = static_cast<const shapes::Box*>(s)->size;
99  ogre_shape->setScale(Ogre::Vector3(sz[0], sz[1], sz[2]));
100  }
101  break;
102  case shapes::CYLINDER:
103  {
104  ogre_shape = new rviz::Shape(rviz::Shape::Cylinder, context_->getSceneManager(), node);
105  double d = 2.0 * static_cast<const shapes::Cylinder*>(s)->radius;
106  double z = static_cast<const shapes::Cylinder*>(s)->length;
107  ogre_shape->setScale(Ogre::Vector3(d, z, d)); // the shape has z as major axis, but the rendered cylinder has y
108  // as major axis (assuming z is upright);
109  }
110  break;
111  case shapes::CONE:
112  {
113  ogre_shape = new rviz::Shape(rviz::Shape::Cone, context_->getSceneManager(), node);
114  double d = 2.0 * static_cast<const shapes::Cone*>(s)->radius;
115  double z = static_cast<const shapes::Cone*>(s)->length;
116  ogre_shape->setScale(Ogre::Vector3(d, z, d));
117  }
118  break;
119  case shapes::PLANE:
120  {
121  ogre_shape = new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(), node);
122  ogre_shape->setScale(Ogre::Vector3(10, 10, 0.001)); // model plane by thin box
123  alpha *= 0.5; // and make it transparent
124  const auto* plane = static_cast<const shapes::Plane*>(s);
125  Eigen::Vector3d normal(plane->a, plane->b, plane->c);
126  double norm = normal.norm();
127  normal /= norm;
128  // adapt pose to match desired normal direction and position
129  Eigen::Quaterniond offset = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), normal);
130  orientation = orientation * Ogre::Quaternion(offset.w(), offset.x(), offset.y(), offset.z());
131  position += plane->d / norm * Ogre::Vector3(normal.x(), normal.y(), normal.z());
132  }
133  break;
134  case shapes::MESH:
135  {
136  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(s);
137  if (mesh->triangle_count > 0)
138  {
140  ogre_shape = m;
141 
142  Ogre::Vector3 normal(0.0, 0.0, 0.0);
143  for (unsigned int i = 0; i < mesh->triangle_count; ++i)
144  {
145  unsigned int i3 = i * 3;
146  if (mesh->triangle_normals && !mesh->vertex_normals)
147  {
148  normal.x = mesh->triangle_normals[i3];
149  normal.y = mesh->triangle_normals[i3 + 1];
150  normal.z = mesh->triangle_normals[i3 + 2];
151  }
152 
153  for (int k = 0; k < 3; ++k)
154  {
155  unsigned int vi = 3 * mesh->triangles[i3 + k];
156  Ogre::Vector3 v(mesh->vertices[vi], mesh->vertices[vi + 1], mesh->vertices[vi + 2]);
157  if (mesh->vertex_normals)
158  {
159  Ogre::Vector3 n(mesh->vertex_normals[vi], mesh->vertex_normals[vi + 1], mesh->vertex_normals[vi + 2]);
160  m->addVertex(v, n);
161  }
162  else if (mesh->triangle_normals)
163  m->addVertex(v, normal);
164  else
165  m->addVertex(v);
166  }
167  }
168  m->endTriangles();
169  }
170  }
171  break;
172 
173  case shapes::OCTREE:
174  {
175  OcTreeRenderPtr octree(new OcTreeRender(static_cast<const shapes::OcTree*>(s)->octree, octree_voxel_rendering,
176  octree_color_mode, 0u, node));
177  octree->setPosition(position);
178  octree->setOrientation(orientation);
179  octree_voxel_grids_.push_back(octree);
180  }
181  break;
182 
183  default:
184  break;
185  }
186 
187  if (ogre_shape)
188  {
189  ogre_shape->setColor(color.r_, color.g_, color.b_, alpha);
190 
191  if (s->type == shapes::CYLINDER || s->type == shapes::CONE)
192  {
193  // in geometric shapes, the z axis of the cylinder is its height;
194  // for the rviz shape, the y axis is the height; we add a transform to fix this
195  static Ogre::Quaternion fix(Ogre::Radian(boost::math::constants::pi<double>() / 2.0),
196  Ogre::Vector3(1.0, 0.0, 0.0));
197  orientation = orientation * fix;
198  }
199 
200  ogre_shape->setPosition(position);
201  ogre_shape->setOrientation(orientation);
202  scene_shapes_.emplace_back(ogre_shape);
203  }
204 }
205 
206 void RenderShapes::updateShapeColors(float r, float g, float b, float a)
207 {
208  for (const std::unique_ptr<rviz::Shape>& shape : scene_shapes_)
209  shape->setColor(r, g, b, a);
210 }
211 
212 } // namespace moveit_rviz_plugin
rviz::Shape::Cylinder
Cylinder
octomap_render.h
rviz::Shape::setColor
void setColor(const Ogre::ColourValue &c)
shapes::Mesh::triangles
unsigned int * triangles
shapes::PLANE
PLANE
s
XmlRpcServer s
shapes::Mesh::vertex_normals
double * vertex_normals
check_isometry.h
mesh_operations.h
moveit_rviz_plugin::RenderShapes::renderShape
void renderShape(Ogre::SceneNode *node, const shapes::Shape *s, const Eigen::Isometry3d &p, OctreeVoxelRenderMode octree_voxel_rendering, OctreeVoxelColorMode octree_color_mode, const rviz::Color &color, float alpha)
Definition: render_shapes.cpp:107
shapes::Cylinder
moveit_rviz_plugin::RenderShapes::RenderShapes
RenderShapes(rviz::DisplayContext *context)
Definition: render_shapes.cpp:92
rviz::Shape::Cone
Cone
shapes::Mesh::triangle_count
unsigned int triangle_count
shapes::SPHERE
SPHERE
moveit_rviz_plugin::RenderShapes::scene_shapes_
std::vector< std::unique_ptr< rviz::Shape > > scene_shapes_
Definition: render_shapes.h:77
shapes::Shape
shapes::Mesh
shape.h
shapes::MESH
MESH
rviz::DisplayContext::getSceneManager
virtual Ogre::SceneManager * getSceneManager() const=0
shapes::CONE
CONE
moveit_rviz_plugin::OctreeVoxelRenderMode
OctreeVoxelRenderMode
Definition: octomap_render.h:52
shapes::Plane
shapes::Box
moveit_rviz_plugin::RenderShapes::context_
rviz::DisplayContext * context_
Definition: render_shapes.h:75
shapes::Sphere
rviz::Color::b_
float b_
rviz::MeshShape
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
moveit_rviz_plugin::RenderShapes::octree_voxel_grids_
std::vector< OcTreeRenderPtr > octree_voxel_grids_
Definition: render_shapes.h:78
rviz::Color::r_
float r_
setup.d
d
Definition: setup.py:4
shapes::Mesh::vertices
double * vertices
rviz::DisplayContext
q
q
shapes::OcTree
rviz::Shape::Sphere
Sphere
shapes::OCTREE
OCTREE
shapes::Mesh::triangle_normals
double * triangle_normals
moveit_rviz_plugin
Definition: motion_planning_display.h:80
m
m
moveit_rviz_plugin::RenderShapes::clear
void clear()
Definition: render_shapes.cpp:101
rviz::Color::g_
float g_
mesh_shape.h
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
rviz::Shape
moveit_rviz_plugin::RenderShapes::~RenderShapes
~RenderShapes()
Definition: render_shapes.cpp:96
shapes::CYLINDER
CYLINDER
v
v
moveit_rviz_plugin::RenderShapes::updateShapeColors
void updateShapeColors(float r, float g, float b, float a)
Definition: render_shapes.cpp:238
moveit_rviz_plugin::OctreeVoxelColorMode
OctreeVoxelColorMode
Definition: octomap_render.h:58
rviz::Shape::setScale
void setScale(const Ogre::Vector3 &scale) override
shapes::Cone
render_shapes.h
rviz::Shape::setPosition
void setPosition(const Ogre::Vector3 &position) override
rviz::Shape::setOrientation
void setOrientation(const Ogre::Quaternion &orientation) override
z
double z
shapes::BOX
BOX
rviz::Color
robot.h
rviz::Shape::Cube
Cube
display_context.h


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:26