jsk_interactive_marker: transformable

Go to the documentation of this file.
00001 #include <jsk_interactive_marker/transformable_torus.h>
00002 #include <math.h>
00003 
00004 #define PI 3.14159265
00005 namespace jsk_interactive_marker{
00006 
00007   TransformableTorus::TransformableTorus( float radius, float small_radius, int u_div, int v_div, float r, float g, float b, float a, std::string frame, std::string name, std::string description){
00008     torus_radius_ = radius;
00009     torus_small_radius_ = small_radius;
00010     torus_r_ = r;
00011     torus_g_ = g;
00012     torus_b_ = b;
00013     torus_a_ = a;
00014     marker_.type = visualization_msgs::Marker::TRIANGLE_LIST;
00015 
00016     frame_id_ = frame;
00017     name_ = name;
00018     description_ = description;
00019 
00020     u_division_num_ = u_div;
00021     v_division_num_ = v_div;
00022   }
00023 
00024   std::vector<geometry_msgs::Point > TransformableTorus::calcurateTriangleMesh(){
00025     std::vector<geometry_msgs::Point> triangle_mesh;
00026     float center_x = 0;
00027     float center_y = 0;
00028     float u_division_num = u_division_num_;
00029     float v_division_num = v_division_num_;
00030     std::vector<std::vector<geometry_msgs::Point> > points_array;
00031     for (int i = 0; i < u_division_num; i ++){
00032       std::vector<geometry_msgs::Point> points;
00033       float target_circle_x = torus_radius_ * cos( ( i / u_division_num) * 2 * PI) ;
00034       float target_circle_y = torus_radius_ * sin( ( i / u_division_num) * 2 * PI) ;
00035       for (int j = 0; j < v_division_num; j++){
00036         geometry_msgs::Point new_point;
00037         new_point.x = target_circle_x + torus_small_radius_ * cos ( (j / v_division_num) * 2 * PI) * cos( ( i / u_division_num) * 2 * PI);
00038         new_point.y = target_circle_y + torus_small_radius_ * cos ( (j / v_division_num) * 2 * PI) * sin( ( i / u_division_num) * 2 * PI);
00039         new_point.z = torus_small_radius_ * sin ( (j / v_division_num) * 2 * PI);
00040         points.push_back(new_point);
00041       }
00042       points_array.push_back(points);
00043     }
00044 
00045     //create mesh list;
00046     for(int i = 0; i < u_division_num; i++){
00047       std::vector<geometry_msgs::Point> target_points = points_array[i];
00048       float prev_index = i - 1, next_index = i + 1;
00049       if(prev_index < 0)
00050         prev_index = u_division_num - 1;
00051       if(next_index > u_division_num - 1)
00052         next_index = 0;
00053       std::vector<geometry_msgs::Point> prev_points = points_array[prev_index];
00054       std::vector<geometry_msgs::Point> next_points = points_array[next_index];
00055       for(int j = 0; j < v_division_num; j++){
00056         float next_point_index = j + 1;
00057         if( next_point_index > v_division_num - 1)
00058           next_point_index = 0;
00059         //first pushes
00060         triangle_mesh.push_back(target_points[j]);
00061         triangle_mesh.push_back(next_points[j]);
00062         triangle_mesh.push_back(target_points[next_point_index]);
00063         //second pushes
00064         triangle_mesh.push_back(target_points[j]);
00065         triangle_mesh.push_back(target_points[next_point_index]);
00066         triangle_mesh.push_back(prev_points[next_point_index]);
00067       }
00068     }
00069 
00070     return triangle_mesh;
00071   }
00072 
00073   visualization_msgs::Marker TransformableTorus::getVisualizationMsgMarker(){
00074     marker_.points = calcurateTriangleMesh();
00075     marker_.color.r = torus_r_;
00076     marker_.color.g = torus_g_;
00077     marker_.color.b = torus_b_;
00078     marker_.color.a = torus_a_;
00079     return marker_;
00080   }
00081 }