$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: interactive_markers_tools.cpp 650 2012-04-18 07:14:58Z spanel $ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Tomas Lokaj (xlokaj03@stud.fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 11/02/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 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 }