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     shapes_.clear();
00127   }
00128 
00129   void TorusArrayDisplay::allocateShapes(int 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     }
00155   }
00156 
00157   void
00158   TorusArrayDisplay::calcurateTriangleMesh(
00159                                            int large_dimension, int small_dimension,
00160                                            float large_radius, float small_radius,
00161                                            Ogre::Vector3 pos, Ogre::Quaternion q,
00162                                            std::vector<Triangle> &triangles,
00163                                            std::vector<Ogre::Vector3> &vertices,
00164                                            std::vector<Ogre::Vector3> &normals
00165                                            ){
00166     //Create Vertex List and Normal List
00167     for (int i = 0; i < large_dimension; i ++){
00168       float target_circle_x = large_radius * cos( ( i * 1.0/ large_dimension) * 2 * PI) ;
00169       float target_circle_y = large_radius * sin( ( i * 1.0 / large_dimension) * 2 * PI) ;
00170       for (int j = 0; j < small_dimension; j++){
00171         Ogre::Vector3 new_point;
00172         new_point.x = target_circle_x + small_radius * cos ( (j * 1.0 / small_dimension) * 2 * PI) * cos( ( i * 1.0/ large_dimension) * 2 * PI);
00173         new_point.y = target_circle_y + small_radius * cos ( (j * 1.0/ small_dimension) * 2 * PI) * sin( ( i * 1.0/ large_dimension) * 2 * PI);
00174         new_point.z = small_radius * sin ( (j * 1.0/ small_dimension) * 2 * PI);
00175 
00176         //new_point rotate
00177         new_point = q * new_point;
00178         //new_point translate
00179         new_point += pos;
00180         vertices.push_back(new_point);
00181 
00182         //GetNormals
00183         Ogre::Vector3 normal;
00184         normal.x = small_radius * cos ( (j * 1.0 / small_dimension) * 2 * PI) * cos( ( i * 1.0/ large_dimension) * 2 * PI);
00185         normal.y = small_radius * cos ( (j * 1.0/ small_dimension) * 2 * PI) * sin( ( i * 1.0/ large_dimension) * 2 * PI);
00186         normal.z = small_radius * sin ( (j * 1.0/ small_dimension) * 2 * PI);
00187         normal = q * normal;
00188         normals.push_back(normal);
00189       }
00190     }
00191 
00192     //Create Index List and push into triangles
00193     for(int i = 0; i < large_dimension; i++){
00194       for(int j = 0; j < small_dimension; j++){
00195         int target_index = i * large_dimension + j;
00196         int next_index = target_index + 1;
00197         if(next_index >= small_dimension * large_dimension)
00198           next_index = 0;
00199         int next_circle_target_index = target_index + large_dimension;
00200         if (next_circle_target_index >= large_dimension*small_dimension)
00201           next_circle_target_index -= large_dimension*small_dimension;
00202         int prev_circle_next_index = target_index - large_dimension + 1;
00203         if (prev_circle_next_index < 0)
00204           prev_circle_next_index += large_dimension*small_dimension;
00205         Triangle t1;
00206         t1.v1 = target_index;
00207         t1.v3 = next_index;
00208         t1.v2 = next_circle_target_index;
00209         Triangle t2;
00210         t2.v1 = target_index;
00211         t2.v2 = next_index;
00212         t2.v3 = prev_circle_next_index;
00213         triangles.push_back(t1);
00214         triangles.push_back(t2);
00215       }
00216     }
00217   }
00218 
00219   void TorusArrayDisplay::updateShowNormal()
00220   {
00221     show_normal_ = show_normal_property_->getBool();
00222     if (show_normal_) {
00223       normal_length_property_->show();
00224     }
00225     else {
00226       normal_length_property_->hide();
00227       for (size_t i = 0; i < arrow_objects_.size(); i++) {
00228         arrow_nodes_[i]->setVisible(false);
00229       }
00230     }
00231   }
00232 
00233   void TorusArrayDisplay::updateNormalLength()
00234   {
00235     normal_length_ = normal_length_property_->getFloat();
00236   }
00237 
00238 
00239   void TorusArrayDisplay::processMessage(const jsk_recognition_msgs::TorusArray::ConstPtr& msg)
00240   {
00241     allocateShapes(msg->toruses.size());
00242     for (size_t i = 0; i < msg->toruses.size(); i++) {
00243       jsk_recognition_msgs::Torus torus = msg->toruses[i];
00244       ShapePtr shape = shapes_[i];
00245 
00246       Ogre::Vector3 position;
00247       Ogre::Quaternion quaternion;
00248       float large_radius = torus.large_radius;
00249       float small_radius = torus.small_radius;
00250 
00251       if(!context_->getFrameManager()->transform(torus.header, torus.pose,
00252                                                  position,
00253                                                  quaternion))
00254         {
00255           ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
00256                      qPrintable( getName() ), torus.header.frame_id.c_str(),
00257                      qPrintable( fixed_frame_ ));
00258           return;
00259         }
00260 
00261       shape->clear();
00262       std::vector<Triangle> triangles;
00263       std::vector<Ogre::Vector3> vertices;
00264       std::vector<Ogre::Vector3> normals;
00265 
00266       calcurateTriangleMesh(uv_dimension_, uv_dimension_,
00267                             large_radius, small_radius,
00268                             position,quaternion,
00269                             triangles, vertices, normals);
00270 
00271       shape->estimateVertexCount(vertices.size());
00272       shape->beginTriangles();
00273       for (std::size_t j = 0 ; j < vertices.size() ; ++j)
00274         shape->addVertex(vertices[j], normals[j]);
00275       for (std::size_t j = 0 ; j < triangles.size() ; ++j)
00276         shape->addTriangle(triangles[j].v1, triangles[j].v2, triangles[j].v3);
00277       shape->endTriangles();
00278       QColor color = getColor(i);
00279       shape->setColor(color.red() / 255.0,
00280                       color.green() / 255.0,
00281                       color.blue() / 255.0,
00282                       alpha_);
00283 
00284       if (show_normal_) {
00285         Ogre::Vector3 normal;
00286         arrow_nodes_[i]->setVisible(true);
00287         arrow_nodes_[i]->setPosition(position);
00288         arrow_nodes_[i]->setOrientation(quaternion);
00289         normal.x = 0;      normal.y = 0;      normal.z = 1;
00290         Ogre::Vector3 scale(normal_length_, normal_length_, normal_length_);
00291         arrow_objects_[i]->setScale(scale);
00292         arrow_objects_[i]->setColor(color.red() / 255.0,
00293                                     color.green() / 255.0,
00294                                     color.blue() / 255.0,
00295                                     alpha_);
00296       }
00297     }
00298   }
00299 }
00300 
00301 
00302 #include <pluginlib/class_list_macros.h>
00303 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::TorusArrayDisplay, rviz::Display )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03