torus_array_display.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ryohei Ueda and JSK Lab
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 the JSK Lab 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 #include "torus_array_display.h"
37 
38 #define PI 3.14159265
39 
40 namespace jsk_rviz_plugins
41 {
43  {
44  color_property_ = new rviz::ColorProperty("color", QColor(25, 255, 0),
45  "color to draw the toruses",
46  this, SLOT(updateColor()));
47  alpha_property_ = new rviz::FloatProperty("alpha", 0.8,
48  "alpha value to draw the toruses",
49  this, SLOT(updateAlpha()));
50  uv_property_ = new rviz::IntProperty("uv-smooth", 50,
51  "torus uv dimension setting",
52  this, SLOT(updateUVdimension()));
53  auto_color_property_ = new rviz::BoolProperty("auto color", false,
54  "change the color of the toruses automatically",
55  this, SLOT(updateAutoColor()));
56  show_normal_property_ = new rviz::BoolProperty("show normal", true,
57  "show normal direction",
58  this, SLOT(updateShowNormal()));
59  normal_length_property_ = new rviz::FloatProperty("normal length", 0.1,
60  "normal length",
61  this, SLOT(updateNormalLength()));
62 
63  uv_property_->setMin(5);
64  }
65 
67  {
68  delete color_property_;
69  delete alpha_property_;
70  delete auto_color_property_;
71  delete uv_property_;
72  delete show_normal_property_;
74  }
75 
76  QColor TorusArrayDisplay::getColor(size_t index)
77  {
78  if (auto_color_) {
79  std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(index);
80  return QColor(ros_color.r * 255.0, ros_color.g * 255.0, ros_color.b * 255.0,
81  ros_color.a * 255.0);
82  }
83  else {
84  return color_;
85  }
86  }
87 
89  {
91  scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
92 
93  updateColor();
94  updateAlpha();
99 
100  uv_dimension_ = 50;
101  }
102 
104  {
106  }
107 
109  {
111  }
112 
114  {
116  }
117 
119  {
121  }
122 
124  {
125  MFDClass::reset();
126  allocateShapes(0);
127  }
128 
129  void TorusArrayDisplay::allocateShapes(const size_t num)
130  {
131  if (num > shapes_.size()) {
132  for (size_t i = shapes_.size(); i < num; i++) {
134  shapes_.push_back(shape);
135  }
136  }
137  else if (num < shapes_.size())
138  {
139  shapes_.resize(num);
140  }
141 
142  if (num > arrow_objects_.size()) {
143  for (size_t i = arrow_objects_.size(); i < num; i++) {
144  Ogre::SceneNode* scene_node = scene_node_->createChildSceneNode();
145  ArrowPtr arrow (new rviz::Arrow(scene_manager_, scene_node));
146  arrow_objects_.push_back(arrow);
147  arrow_nodes_.push_back(scene_node);
148  }
149  }
150  else if (num < arrow_objects_.size()) {
151  for (size_t i = num; i < arrow_objects_.size(); i++) {
152  arrow_nodes_[i]->setVisible(false);
153  }
154  arrow_objects_.resize(num);
155  arrow_nodes_.resize(num);
156  }
157  }
158 
159  void TorusArrayDisplay::allocateShapes(const jsk_recognition_msgs::TorusArray::ConstPtr& msg)
160  {
161  size_t num = 0;
162  for (size_t i = 0; i < msg->toruses.size(); i++) {
163  if (!msg->toruses[i].failure) {
164  ++num;
165  }
166  }
167  allocateShapes(num);
168  }
169 
170  void
172  int large_dimension, int small_dimension,
173  float large_radius, float small_radius,
174  Ogre::Vector3 pos, Ogre::Quaternion q,
175  std::vector<Triangle> &triangles,
176  std::vector<Ogre::Vector3> &vertices,
177  std::vector<Ogre::Vector3> &normals
178  ){
179  //Create Vertex List and Normal List
180  for (int i = 0; i < large_dimension; i ++){
181  float target_circle_x = large_radius * cos( ( i * 1.0/ large_dimension) * 2 * PI) ;
182  float target_circle_y = large_radius * sin( ( i * 1.0 / large_dimension) * 2 * PI) ;
183  for (int j = 0; j < small_dimension; j++){
184  Ogre::Vector3 new_point;
185  new_point.x = target_circle_x + small_radius * cos ( (j * 1.0 / small_dimension) * 2 * PI) * cos( ( i * 1.0/ large_dimension) * 2 * PI);
186  new_point.y = target_circle_y + small_radius * cos ( (j * 1.0/ small_dimension) * 2 * PI) * sin( ( i * 1.0/ large_dimension) * 2 * PI);
187  new_point.z = small_radius * sin ( (j * 1.0/ small_dimension) * 2 * PI);
188 
189  //new_point rotate
190  new_point = q * new_point;
191  //new_point translate
192  new_point += pos;
193  vertices.push_back(new_point);
194 
195  //GetNormals
196  Ogre::Vector3 normal;
197  normal.x = small_radius * cos ( (j * 1.0 / small_dimension) * 2 * PI) * cos( ( i * 1.0/ large_dimension) * 2 * PI);
198  normal.y = small_radius * cos ( (j * 1.0/ small_dimension) * 2 * PI) * sin( ( i * 1.0/ large_dimension) * 2 * PI);
199  normal.z = small_radius * sin ( (j * 1.0/ small_dimension) * 2 * PI);
200  normal = q * normal;
201  normals.push_back(normal);
202  }
203  }
204 
205  //Create Index List and push into triangles
206  for(int i = 0; i < large_dimension; i++){
207  for(int j = 0; j < small_dimension; j++){
208  int target_index = i * large_dimension + j;
209  int next_index = target_index + 1;
210  if(next_index >= small_dimension * large_dimension)
211  next_index = 0;
212  int next_circle_target_index = target_index + large_dimension;
213  if (next_circle_target_index >= large_dimension*small_dimension)
214  next_circle_target_index -= large_dimension*small_dimension;
215  int prev_circle_next_index = target_index - large_dimension + 1;
216  if (prev_circle_next_index < 0)
217  prev_circle_next_index += large_dimension*small_dimension;
218  Triangle t1;
219  t1.v1 = target_index;
220  t1.v3 = next_index;
221  t1.v2 = next_circle_target_index;
222  Triangle t2;
223  t2.v1 = target_index;
224  t2.v2 = next_index;
225  t2.v3 = prev_circle_next_index;
226  triangles.push_back(t1);
227  triangles.push_back(t2);
228  }
229  }
230  }
231 
233  {
235  if (show_normal_) {
237  }
238  else {
240  for (size_t i = 0; i < arrow_objects_.size(); i++) {
241  arrow_nodes_[i]->setVisible(false);
242  }
243  }
244  }
245 
247  {
249  }
250 
251 
252  void TorusArrayDisplay::processMessage(const jsk_recognition_msgs::TorusArray::ConstPtr& msg)
253  {
254  allocateShapes(msg);
255  for (size_t i = 0; i < msg->toruses.size(); i++) {
256  jsk_recognition_msgs::Torus torus = msg->toruses[i];
257  if (torus.failure) {
258  continue;
259  }
260  ShapePtr shape = shapes_[i];
261 
262  Ogre::Vector3 position;
263  Ogre::Quaternion quaternion;
264  float large_radius = torus.large_radius;
265  float small_radius = torus.small_radius;
266 
267  if(!context_->getFrameManager()->transform(torus.header, torus.pose,
268  position,
269  quaternion))
270  {
271  std::ostringstream oss;
272  oss << "Error transforming pose";
273  oss << " from frame '" << torus.header.frame_id << "'";
274  oss << " to frame '" << qPrintable(fixed_frame_) << "'";
275  ROS_ERROR_STREAM(oss.str());
276  setStatus(rviz::StatusProperty::Error, "Transform", QString::fromStdString(oss.str()));
277  return;
278  }
279 
280  shape->clear();
281  std::vector<Triangle> triangles;
282  std::vector<Ogre::Vector3> vertices;
283  std::vector<Ogre::Vector3> normals;
284 
286  large_radius, small_radius,
287  position,quaternion,
288  triangles, vertices, normals);
289 
290  shape->estimateVertexCount(vertices.size());
291  shape->beginTriangles();
292  for (std::size_t j = 0 ; j < vertices.size() ; ++j)
293  shape->addVertex(vertices[j], normals[j]);
294  for (std::size_t j = 0 ; j < triangles.size() ; ++j)
295  shape->addTriangle(triangles[j].v1, triangles[j].v2, triangles[j].v3);
296  shape->endTriangles();
297  QColor color = getColor(i);
298  shape->setColor(color.red() / 255.0,
299  color.green() / 255.0,
300  color.blue() / 255.0,
301  alpha_);
302 
303  if (show_normal_) {
304  Ogre::Vector3 normal;
305  arrow_nodes_[i]->setVisible(true);
306  arrow_nodes_[i]->setPosition(position);
307  arrow_nodes_[i]->setOrientation(quaternion);
308  normal.x = 0; normal.y = 0; normal.z = 1;
310  arrow_objects_[i]->setScale(scale);
311  arrow_objects_[i]->setColor(color.red() / 255.0,
312  color.green() / 255.0,
313  color.blue() / 255.0,
314  alpha_);
315  }
316  }
317  }
318 }
319 
320 
num
void allocateShapes(const jsk_recognition_msgs::TorusArray::ConstPtr &msg)
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
virtual QColor getColor() const
void processMessage(const jsk_recognition_msgs::TorusArray::ConstPtr &msg)
void setMin(int min)
rviz::FloatProperty * normal_length_property_
Ogre::SceneNode * scene_node_
std::vector< Ogre::SceneNode * > arrow_nodes_
QString fixed_frame_
#define PI
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
virtual FrameManager * getFrameManager() const=0
Ogre::SceneManager * scene_manager_
virtual int getInt() const
virtual Ogre::SceneManager * getSceneManager() const=0
void calcurateTriangleMesh(int large_dimension, int small_dimension, float large_radius, float small_radius, Ogre::Vector3 pos, Ogre::Quaternion q, std::vector< Triangle > &triangles, std::vector< Ogre::Vector3 > &vertices, std::vector< Ogre::Vector3 > &normals)
std_msgs::ColorRGBA colorCategory20(int i)
virtual float getFloat() const
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
#define ROS_ERROR_STREAM(args)
std::vector< ArrowPtr > arrow_objects_
virtual bool getBool() const
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58