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
00029
00030 #include "interactive_markers/tools.h"
00031
00032 #include <tf/LinearMath/Quaternion.h>
00033 #include <tf/LinearMath/Matrix3x3.h>
00034
00035 #include <math.h>
00036 #include <assert.h>
00037
00038 #include <set>
00039 #include <sstream>
00040
00041 namespace interactive_markers
00042 {
00043
00044 void autoComplete( visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency )
00045 {
00046
00047 if ( msg.controls.empty() )
00048 {
00049 return;
00050 }
00051
00052
00053 if ( msg.scale == 0 )
00054 {
00055 msg.scale = 1;
00056 }
00057
00058
00059 if ( msg.pose.orientation.w == 0 && msg.pose.orientation.x == 0 &&
00060 msg.pose.orientation.y == 0 && msg.pose.orientation.z == 0 )
00061 {
00062 msg.pose.orientation.w = 1;
00063 }
00064
00065
00066 tf::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
00067 msg.pose.orientation.z, msg.pose.orientation.w );
00068 int_marker_orientation.normalize();
00069 msg.pose.orientation.x = int_marker_orientation.x();
00070 msg.pose.orientation.y = int_marker_orientation.y();
00071 msg.pose.orientation.z = int_marker_orientation.z();
00072 msg.pose.orientation.w = int_marker_orientation.w();
00073
00074
00075 for ( unsigned c=0; c<msg.controls.size(); c++ )
00076 {
00077 autoComplete( msg, msg.controls[c], enable_autocomplete_transparency);
00078 }
00079
00080 uniqueifyControlNames( msg );
00081 }
00082
00083 void uniqueifyControlNames( visualization_msgs::InteractiveMarker& msg )
00084 {
00085 int uniqueification_number = 0;
00086 std::set<std::string> names;
00087 for( unsigned c = 0; c < msg.controls.size(); c++ )
00088 {
00089 std::string name = msg.controls[c].name;
00090 while( names.find( name ) != names.end() )
00091 {
00092 std::stringstream ss;
00093 ss << name << "_u" << uniqueification_number++;
00094 name = ss.str();
00095 }
00096 msg.controls[c].name = name;
00097 names.insert( name );
00098 }
00099 }
00100
00101 void autoComplete( const visualization_msgs::InteractiveMarker &msg,
00102 visualization_msgs::InteractiveMarkerControl &control, bool enable_autocomplete_transparency)
00103 {
00104
00105 if ( control.orientation.w == 0 && control.orientation.x == 0 &&
00106 control.orientation.y == 0 && control.orientation.z == 0 )
00107 {
00108 control.orientation.w = 1;
00109 }
00110
00111
00112 if ( control.markers.empty() )
00113 {
00114 switch ( control.interaction_mode )
00115 {
00116 case visualization_msgs::InteractiveMarkerControl::NONE:
00117 break;
00118
00119 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
00120 control.markers.reserve(2);
00121 makeArrow( msg, control, 1.0 );
00122 makeArrow( msg, control, -1.0 );
00123 break;
00124
00125 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
00126 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
00127 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
00128 makeDisc( msg, control );
00129 break;
00130
00131 case visualization_msgs::InteractiveMarkerControl::BUTTON:
00132 break;
00133
00134 case visualization_msgs::InteractiveMarkerControl::MENU:
00135 makeViewFacingButton( msg, control, control.description );
00136 break;
00137
00138 default:
00139 break;
00140 }
00141 }
00142
00143
00144 tf::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
00145 msg.pose.orientation.z, msg.pose.orientation.w );
00146 tf::Vector3 int_marker_position( msg.pose.position.x, msg.pose.position.y, msg.pose.position.z );
00147
00148
00149 for ( unsigned m=0; m<control.markers.size(); m++ )
00150 {
00151 visualization_msgs::Marker &marker = control.markers[m];
00152
00153 if ( marker.scale.x == 0 )
00154 {
00155 marker.scale.x = 1;
00156 }
00157 if ( marker.scale.y == 0 )
00158 {
00159 marker.scale.y = 1;
00160 }
00161 if ( marker.scale.z == 0 )
00162 {
00163 marker.scale.z = 1;
00164 }
00165
00166 marker.ns = msg.name;
00167
00168
00169 if ( marker.pose.orientation.w == 0 && marker.pose.orientation.x == 0 &&
00170 marker.pose.orientation.y == 0 && marker.pose.orientation.z == 0 )
00171 {
00172 marker.pose.orientation.w = 1;
00173 }
00174
00175
00176 tf::Quaternion marker_orientation( marker.pose.orientation.x, marker.pose.orientation.y,
00177 marker.pose.orientation.z, marker.pose.orientation.w );
00178
00179 marker_orientation.normalize();
00180
00181 marker.pose.orientation.x = marker_orientation.x();
00182 marker.pose.orientation.y = marker_orientation.y();
00183 marker.pose.orientation.z = marker_orientation.z();
00184 marker.pose.orientation.w = marker_orientation.w();
00185
00186 static volatile unsigned id = 0;
00187 marker.id = id++;
00188 marker.ns = msg.name;
00189
00190
00191 if ( !enable_autocomplete_transparency && marker.color.a > 0.0 )
00192 {
00193 marker.color.a = 1.0;
00194 }
00195 }
00196 }
00197
00198 void makeArrow( const visualization_msgs::InteractiveMarker &msg,
00199 visualization_msgs::InteractiveMarkerControl &control, float pos )
00200 {
00201 visualization_msgs::Marker marker;
00202
00203
00204 marker.pose.orientation = control.orientation;
00205
00206 marker.type = visualization_msgs::Marker::ARROW;
00207 marker.scale.x = msg.scale * 0.15;
00208 marker.scale.y = msg.scale * 0.25;
00209 marker.scale.z = msg.scale * 0.2;
00210
00211 assignDefaultColor(marker, control.orientation);
00212
00213 float dist = fabs(pos);
00214 float dir = pos > 0 ? 1 : -1;
00215
00216 float inner = 0.5 * dist;
00217 float outer = inner + 0.4;
00218
00219 marker.points.resize(2);
00220 marker.points[0].x = dir * msg.scale * inner;
00221 marker.points[1].x = dir * msg.scale * outer;
00222
00223 control.markers.push_back( marker );
00224 }
00225
00226 void makeDisc( const visualization_msgs::InteractiveMarker &msg,
00227 visualization_msgs::InteractiveMarkerControl &control, float width )
00228 {
00229 visualization_msgs::Marker marker;
00230
00231
00232 marker.pose.orientation = control.orientation;
00233
00234 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00235 marker.scale.x = msg.scale;
00236 marker.scale.y = msg.scale;
00237 marker.scale.z = msg.scale;
00238
00239 assignDefaultColor(marker, control.orientation);
00240
00241
00242 int steps = 36;
00243 std::vector<geometry_msgs::Point> circle1, circle2;
00244 circle1.reserve(steps);
00245 circle2.reserve(steps);
00246
00247 geometry_msgs::Point v1,v2;
00248
00249 for ( int i=0; i<steps; i++ )
00250 {
00251 float a = float(i)/float(steps) * M_PI * 2.0;
00252
00253 v1.y = 0.5 * cos(a);
00254 v1.z = 0.5 * sin(a);
00255
00256 v2.y = (1+width) * v1.y;
00257 v2.z = (1+width) * v1.z;
00258
00259 circle1.push_back( v1 );
00260 circle2.push_back( v2 );
00261 }
00262
00263 marker.points.resize(6*steps);
00264
00265 std_msgs::ColorRGBA color;
00266 color.r=color.g=color.b=color.a=1;
00267
00268 switch ( control.interaction_mode )
00269 {
00270 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
00271 {
00272 marker.colors.resize(2*steps);
00273 std_msgs::ColorRGBA base_color = marker.color;
00274 for ( int i=0; i<steps; i++ )
00275 {
00276 int i1 = i;
00277 int i2 = (i+1) % steps;
00278 int i3 = (i+2) % steps;
00279
00280 int p = i*6;
00281 int c = i*2;
00282
00283 marker.points[p+0] = circle1[i1];
00284 marker.points[p+1] = circle2[i2];
00285 marker.points[p+2] = circle1[i2];
00286
00287 marker.points[p+3] = circle1[i2];
00288 marker.points[p+4] = circle2[i2];
00289 marker.points[p+5] = circle2[i3];
00290
00291 float t = 0.6 + 0.4 * (i%2);
00292 color.r = base_color.r * t;
00293 color.g = base_color.g * t;
00294 color.b = base_color.b * t;
00295
00296 marker.colors[c] = color;
00297 marker.colors[c+1] = color;
00298 }
00299 break;
00300 }
00301
00302 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
00303 {
00304 marker.colors.resize(2*steps);
00305 std_msgs::ColorRGBA base_color = marker.color;
00306 for ( int i=0; i<steps-1; i+=2 )
00307 {
00308 int i1 = i;
00309 int i2 = (i+1) % steps;
00310 int i3 = (i+2) % steps;
00311
00312 int p = i * 6;
00313 int c = i * 2;
00314
00315 marker.points[p+0] = circle1[i1];
00316 marker.points[p+1] = circle2[i2];
00317 marker.points[p+2] = circle1[i2];
00318
00319 marker.points[p+3] = circle1[i2];
00320 marker.points[p+4] = circle2[i2];
00321 marker.points[p+5] = circle1[i3];
00322
00323 color.r = base_color.r * 0.6;
00324 color.g = base_color.g * 0.6;
00325 color.b = base_color.b * 0.6;
00326
00327 marker.colors[c] = color;
00328 marker.colors[c+1] = color;
00329
00330 p += 6;
00331 c += 2;
00332
00333 marker.points[p+0] = circle2[i1];
00334 marker.points[p+1] = circle2[i2];
00335 marker.points[p+2] = circle1[i1];
00336
00337 marker.points[p+3] = circle2[i2];
00338 marker.points[p+4] = circle2[i3];
00339 marker.points[p+5] = circle1[i3];
00340
00341 marker.colors[c] = base_color;
00342 marker.colors[c+1] = base_color;
00343 }
00344 break;
00345 }
00346
00347 default:
00348 for ( int i=0; i<steps; i++ )
00349 {
00350 int i1 = i;
00351 int i2 = (i+1) % steps;
00352
00353 int p = i*6;
00354
00355 marker.points[p+0] = circle1[i1];
00356 marker.points[p+1] = circle2[i1];
00357 marker.points[p+2] = circle1[i2];
00358
00359 marker.points[p+3] = circle2[i1];
00360 marker.points[p+4] = circle2[i2];
00361 marker.points[p+5] = circle1[i2];
00362 }
00363 break;
00364 }
00365
00366 control.markers.push_back(marker);
00367 }
00368
00369 void makeViewFacingButton( const visualization_msgs::InteractiveMarker &msg,
00370 visualization_msgs::InteractiveMarkerControl &control, std::string text )
00371 {
00372 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
00373 control.independent_marker_orientation = false;
00374
00375 visualization_msgs::Marker marker;
00376
00377 float base_scale = 0.25 * msg.scale;
00378 float base_z = 1.2 * msg.scale;
00379
00380 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00381 marker.scale.x = base_scale;
00382 marker.scale.y = base_scale;
00383 marker.scale.z = base_scale;
00384 marker.color.r = 1.0;
00385 marker.color.g = 1.0;
00386 marker.color.b = 1.0;
00387 marker.color.a = 1.0;
00388 marker.pose.position.x = base_scale * -0.1;
00389 marker.pose.position.z = base_z + base_scale * -0.1;
00390 marker.text = text;
00391
00392 control.markers.push_back( marker );
00393 }
00394
00395
00396 void assignDefaultColor(visualization_msgs::Marker &marker, const geometry_msgs::Quaternion &quat )
00397 {
00398 geometry_msgs::Vector3 v;
00399
00400 tf::Quaternion bt_quat( quat.x, quat.y, quat.z, quat.w );
00401 tf::Vector3 bt_x_axis = tf::Matrix3x3(bt_quat) * tf::Vector3(1,0,0);
00402
00403 float x,y,z;
00404 x = fabs(bt_x_axis.x());
00405 y = fabs(bt_x_axis.y());
00406 z = fabs(bt_x_axis.z());
00407
00408 float max_xy = x>y ? x : y;
00409 float max_yz = y>z ? y : z;
00410 float max_xyz = max_xy > max_yz ? max_xy : max_yz;
00411
00412 marker.color.r = x / max_xyz;
00413 marker.color.g = y / max_xyz;
00414 marker.color.b = z / max_xyz;
00415 marker.color.a = 0.5;
00416 }
00417
00418
00419 visualization_msgs::InteractiveMarkerControl makeTitle( const visualization_msgs::InteractiveMarker &msg )
00420 {
00421 visualization_msgs::Marker marker;
00422
00423 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00424 marker.scale.x = msg.scale * 0.15;
00425 marker.scale.y = msg.scale * 0.15;
00426 marker.scale.z = msg.scale * 0.15;
00427 marker.color.r = 1.0;
00428 marker.color.g = 1.0;
00429 marker.color.b = 1.0;
00430 marker.color.a = 1.0;
00431 marker.pose.position.z = msg.scale * 1.4;
00432 marker.text = msg.description;
00433
00434 visualization_msgs::InteractiveMarkerControl control;
00435 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::NONE;
00436 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
00437 control.always_visible = true;
00438 control.markers.push_back( marker );
00439
00440 autoComplete( msg, control );
00441
00442 return control;
00443 }
00444
00445 }