Go to the documentation of this file.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 #include <cob_interactive_teleop/interactive_markers_tools.h>
00029
00030 namespace interactive_markers
00031 {
00032
00033 void makeCircle(visualization_msgs::InteractiveMarkerControl &control,
00034 float radius,
00035 float width,
00036 std_msgs::ColorRGBA color
00037 )
00038 {
00039 visualization_msgs::Marker marker;
00040
00041 marker.pose.orientation = control.orientation;
00042 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00043 marker.scale.x = radius;
00044 marker.scale.y = radius;
00045 marker.scale.z = radius;
00046 marker.color = color;
00047
00048 int steps = 36;
00049 std::vector<geometry_msgs::Point> circle1, circle2;
00050
00051 geometry_msgs::Point v1, v2;
00052
00053 for (int i = 0; i < steps; i++)
00054 {
00055 float a = float(i) / float(steps) * M_PI * 2.0;
00056
00057 v1.y = 0.5 * cos(a);
00058 v1.z = 0.5 * sin(a);
00059
00060 v2.y = (1 + width) * v1.y;
00061 v2.z = (1 + width) * v1.z;
00062
00063 circle1.push_back(v1);
00064 circle2.push_back(v2);
00065 }
00066
00067 for (int i = 0; i < steps; i++)
00068 {
00069 int i1 = i;
00070 int i2 = (i + 1) % steps;
00071
00072 marker.points.clear();
00073 marker.points.push_back(circle1[i1]);
00074 marker.points.push_back(circle2[i1]);
00075 marker.points.push_back(circle1[i2]);
00076 marker.points.push_back(circle2[i1]);
00077 marker.points.push_back(circle2[i2]);
00078 marker.points.push_back(circle1[i2]);
00079
00080 control.markers.push_back(marker);
00081 }
00082 }
00083
00084 }