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 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
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
00190 new_point = q * new_point;
00191
00192 new_point += pos;
00193 vertices.push_back(new_point);
00194
00195
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
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 )