torus_array_display.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ryohei Ueda and JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "torus_array_display.h"
00036 #include <jsk_topic_tools/color_utils.h>
00037 
00038 #define PI 3.14159265
00039 
00040 namespace jsk_rviz_plugins
00041 {
00042   TorusArrayDisplay::TorusArrayDisplay()
00043   {
00044     color_property_ = new rviz::ColorProperty("color", QColor(25, 255, 0),
00045                                               "color to draw the toruses",
00046                                               this, SLOT(updateColor()));
00047     alpha_property_ = new rviz::FloatProperty("alpha", 0.8,
00048                                               "alpha value to draw the toruses",
00049                                               this, SLOT(updateAlpha()));
00050     uv_property_ = new rviz::IntProperty("uv-smooth", 50,
00051                                             "torus uv dimension setting",
00052                                             this, SLOT(updateUVdimension()));
00053     auto_color_property_ = new rviz::BoolProperty("auto color", false,
00054                                                   "change the color of the toruses automatically",
00055                                                   this, SLOT(updateAutoColor()));
00056     show_normal_property_ = new rviz::BoolProperty("show normal", true,
00057                                                    "show normal direction",
00058                                                    this, SLOT(updateShowNormal()));
00059     normal_length_property_ = new rviz::FloatProperty("normal length", 0.1,
00060                                                       "normal length",
00061                                                       this, SLOT(updateNormalLength()));
00062 
00063     uv_property_->setMin(5);
00064   }
00065 
00066   TorusArrayDisplay::~TorusArrayDisplay()
00067   {
00068     delete color_property_;
00069     delete alpha_property_;
00070     delete auto_color_property_;
00071     delete uv_property_;
00072     delete show_normal_property_;
00073     delete normal_length_property_;
00074   }
00075 
00076   QColor TorusArrayDisplay::getColor(size_t index)
00077   {
00078     if (auto_color_) {
00079       std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(index);
00080       return QColor(ros_color.r * 255.0, ros_color.g * 255.0, ros_color.b * 255.0,
00081                     ros_color.a * 255.0);
00082     }
00083     else {
00084       return color_;
00085     }
00086   }
00087   
00088   void TorusArrayDisplay::onInitialize()
00089   {
00090     MFDClass::onInitialize();
00091     scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00092 
00093     updateColor();
00094     updateAlpha();
00095     updateAutoColor();
00096     updateUVdimension();
00097     updateNormalLength();
00098     updateShowNormal();
00099 
00100     uv_dimension_ = 50;
00101   }
00102 
00103   void TorusArrayDisplay::updateColor()
00104   {
00105     color_ = color_property_->getColor();
00106   }
00107 
00108   void TorusArrayDisplay::updateAlpha()
00109   {
00110     alpha_ = alpha_property_->getFloat();
00111   }
00112 
00113   void TorusArrayDisplay::updateUVdimension()
00114   {
00115     uv_dimension_ = uv_property_->getInt();
00116   }
00117 
00118   void TorusArrayDisplay::updateAutoColor()
00119   {
00120     auto_color_ = auto_color_property_->getBool();
00121   }
00122 
00123   void TorusArrayDisplay::reset()
00124   {
00125     MFDClass::reset();
00126     allocateShapes(0);
00127   }
00128   
00129   void TorusArrayDisplay::allocateShapes(const size_t num) 
00130   {
00131      if (num > shapes_.size()) {
00132       for (size_t i = shapes_.size(); i < num; i++) {
00133         ShapePtr shape (new rviz::MeshShape(context_->getSceneManager()));
00134         shapes_.push_back(shape);
00135       }
00136     }
00137     else if (num < shapes_.size())
00138     {
00139       shapes_.resize(num);
00140     }
00141 
00142     if (num > arrow_objects_.size()) {
00143       for (size_t i = arrow_objects_.size(); i < num; i++) {
00144         Ogre::SceneNode* scene_node = scene_node_->createChildSceneNode();
00145         ArrowPtr arrow (new rviz::Arrow(scene_manager_, scene_node));
00146         arrow_objects_.push_back(arrow);
00147         arrow_nodes_.push_back(scene_node);
00148       }
00149     }
00150     else if (num < arrow_objects_.size()) {
00151       for (size_t i = num; i < arrow_objects_.size(); i++) {
00152         arrow_nodes_[i]->setVisible(false);
00153       }
00154       arrow_objects_.resize(num);
00155       arrow_nodes_.resize(num);
00156     }
00157   }
00158 
00159   void TorusArrayDisplay::allocateShapes(const jsk_recognition_msgs::TorusArray::ConstPtr& msg)
00160   {
00161     size_t num = 0;
00162     for (size_t i = 0; i < msg->toruses.size(); i++) {
00163       if (!msg->toruses[i].failure) {
00164         ++num;
00165       }
00166     }
00167     allocateShapes(num);
00168   }
00169 
00170   void
00171   TorusArrayDisplay::calcurateTriangleMesh(
00172                                            int large_dimension, int small_dimension,
00173                                            float large_radius, float small_radius,
00174                                            Ogre::Vector3 pos, Ogre::Quaternion q,
00175                                            std::vector<Triangle> &triangles,
00176                                            std::vector<Ogre::Vector3> &vertices,
00177                                            std::vector<Ogre::Vector3> &normals
00178                                            ){
00179     //Create Vertex List and Normal List
00180     for (int i = 0; i < large_dimension; i ++){
00181       float target_circle_x = large_radius * cos( ( i * 1.0/ large_dimension) * 2 * PI) ;
00182       float target_circle_y = large_radius * sin( ( i * 1.0 / large_dimension) * 2 * PI) ;
00183       for (int j = 0; j < small_dimension; j++){
00184         Ogre::Vector3 new_point;
00185         new_point.x = target_circle_x + small_radius * cos ( (j * 1.0 / small_dimension) * 2 * PI) * cos( ( i * 1.0/ large_dimension) * 2 * PI);
00186         new_point.y = target_circle_y + small_radius * cos ( (j * 1.0/ small_dimension) * 2 * PI) * sin( ( i * 1.0/ large_dimension) * 2 * PI);
00187         new_point.z = small_radius * sin ( (j * 1.0/ small_dimension) * 2 * PI);
00188 
00189         //new_point rotate
00190         new_point = q * new_point;
00191         //new_point translate
00192         new_point += pos;
00193         vertices.push_back(new_point);
00194 
00195         //GetNormals
00196         Ogre::Vector3 normal;
00197         normal.x = small_radius * cos ( (j * 1.0 / small_dimension) * 2 * PI) * cos( ( i * 1.0/ large_dimension) * 2 * PI);
00198         normal.y = small_radius * cos ( (j * 1.0/ small_dimension) * 2 * PI) * sin( ( i * 1.0/ large_dimension) * 2 * PI);
00199         normal.z = small_radius * sin ( (j * 1.0/ small_dimension) * 2 * PI);
00200         normal = q * normal;
00201         normals.push_back(normal);
00202       }
00203     }
00204 
00205     //Create Index List and push into triangles
00206     for(int i = 0; i < large_dimension; i++){
00207       for(int j = 0; j < small_dimension; j++){
00208         int target_index = i * large_dimension + j;
00209         int next_index = target_index + 1;
00210         if(next_index >= small_dimension * large_dimension)
00211           next_index = 0;
00212         int next_circle_target_index = target_index + large_dimension;
00213         if (next_circle_target_index >= large_dimension*small_dimension)
00214           next_circle_target_index -= large_dimension*small_dimension;
00215         int prev_circle_next_index = target_index - large_dimension + 1;
00216         if (prev_circle_next_index < 0)
00217           prev_circle_next_index += large_dimension*small_dimension;
00218         Triangle t1;
00219         t1.v1 = target_index;
00220         t1.v3 = next_index;
00221         t1.v2 = next_circle_target_index;
00222         Triangle t2;
00223         t2.v1 = target_index;
00224         t2.v2 = next_index;
00225         t2.v3 = prev_circle_next_index;
00226         triangles.push_back(t1);
00227         triangles.push_back(t2);
00228       }
00229     }
00230   }
00231 
00232   void TorusArrayDisplay::updateShowNormal()
00233   {
00234     show_normal_ = show_normal_property_->getBool();
00235     if (show_normal_) {
00236       normal_length_property_->show();
00237     }
00238     else {
00239       normal_length_property_->hide();
00240       for (size_t i = 0; i < arrow_objects_.size(); i++) {
00241         arrow_nodes_[i]->setVisible(false);
00242       }
00243     }
00244   }
00245 
00246   void TorusArrayDisplay::updateNormalLength()
00247   {
00248     normal_length_ = normal_length_property_->getFloat();
00249   }
00250 
00251 
00252   void TorusArrayDisplay::processMessage(const jsk_recognition_msgs::TorusArray::ConstPtr& msg)
00253   {
00254     allocateShapes(msg);
00255     for (size_t i = 0; i < msg->toruses.size(); i++) {
00256       jsk_recognition_msgs::Torus torus = msg->toruses[i];
00257       if (torus.failure) {
00258         continue;
00259       }
00260       ShapePtr shape = shapes_[i];
00261 
00262       Ogre::Vector3 position;
00263       Ogre::Quaternion quaternion;
00264       float large_radius = torus.large_radius;
00265       float small_radius = torus.small_radius;
00266 
00267       if(!context_->getFrameManager()->transform(torus.header, torus.pose,
00268                                                  position,
00269                                                  quaternion))
00270       {
00271           std::ostringstream oss;
00272           oss << "Error transforming pose";
00273           oss << " from frame '" << torus.header.frame_id << "'";
00274           oss << " to frame '" << qPrintable(fixed_frame_) << "'";
00275           ROS_ERROR_STREAM(oss.str());
00276           setStatus(rviz::StatusProperty::Error, "Transform", QString::fromStdString(oss.str()));
00277           return;
00278       }
00279 
00280       shape->clear();
00281       std::vector<Triangle> triangles;
00282       std::vector<Ogre::Vector3> vertices;
00283       std::vector<Ogre::Vector3> normals;
00284 
00285       calcurateTriangleMesh(uv_dimension_, uv_dimension_,
00286                             large_radius, small_radius,
00287                             position,quaternion,
00288                             triangles, vertices, normals);
00289 
00290       shape->estimateVertexCount(vertices.size());
00291       shape->beginTriangles();
00292       for (std::size_t j = 0 ; j < vertices.size() ; ++j)
00293         shape->addVertex(vertices[j], normals[j]);
00294       for (std::size_t j = 0 ; j < triangles.size() ; ++j)
00295         shape->addTriangle(triangles[j].v1, triangles[j].v2, triangles[j].v3);
00296       shape->endTriangles();
00297       QColor color = getColor(i);
00298       shape->setColor(color.red() / 255.0,
00299                       color.green() / 255.0,
00300                       color.blue() / 255.0,
00301                       alpha_);
00302 
00303       if (show_normal_) {
00304         Ogre::Vector3 normal;
00305         arrow_nodes_[i]->setVisible(true);
00306         arrow_nodes_[i]->setPosition(position);
00307         arrow_nodes_[i]->setOrientation(quaternion);
00308         normal.x = 0;      normal.y = 0;      normal.z = 1;
00309         Ogre::Vector3 scale(normal_length_, normal_length_, normal_length_);
00310         arrow_objects_[i]->setScale(scale);
00311         arrow_objects_[i]->setColor(color.red() / 255.0,
00312                                     color.green() / 255.0,
00313                                     color.blue() / 255.0,
00314                                     alpha_);
00315       }
00316     }
00317   }
00318 }
00319 
00320 
00321 #include <pluginlib/class_list_macros.h>
00322 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::TorusArrayDisplay, rviz::Display )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Wed May 1 2019 02:40:22