00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
00177 new_point = q * new_point;
00178
00179 new_point += pos;
00180 vertices.push_back(new_point);
00181
00182
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
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 )