$search
00001 /* 00002 * Copyright (c) 2011, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include "interactive_markers/tools.h" 00031 00032 #include <LinearMath/btQuaternion.h> 00033 #include <LinearMath/btMatrix3x3.h> 00034 00035 #include <math.h> 00036 #include <assert.h> 00037 00038 namespace interactive_markers 00039 { 00040 00041 void autoComplete( visualization_msgs::InteractiveMarker &msg ) 00042 { 00043 // this is a 'delete' message. no need for action. 00044 if ( msg.controls.empty() ) 00045 { 00046 return; 00047 } 00048 00049 // default size 00050 if ( msg.scale == 0 ) 00051 { 00052 msg.scale = 1; 00053 } 00054 00055 // correct empty orientation, normalize 00056 if ( msg.pose.orientation.w == 0 && msg.pose.orientation.x == 0 && 00057 msg.pose.orientation.y == 0 && msg.pose.orientation.z == 0 ) 00058 { 00059 msg.pose.orientation.w = 1; 00060 } 00061 00062 //normalize quaternion 00063 btQuaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y, 00064 msg.pose.orientation.z, msg.pose.orientation.w ); 00065 int_marker_orientation.normalize(); 00066 msg.pose.orientation.x = int_marker_orientation.x(); 00067 msg.pose.orientation.y = int_marker_orientation.y(); 00068 msg.pose.orientation.z = int_marker_orientation.z(); 00069 msg.pose.orientation.w = int_marker_orientation.w(); 00070 00071 // complete the controls 00072 for ( unsigned c=0; c<msg.controls.size(); c++ ) 00073 { 00074 autoComplete( msg, msg.controls[c] ); 00075 } 00076 } 00077 00078 void autoComplete( const visualization_msgs::InteractiveMarker &msg, 00079 visualization_msgs::InteractiveMarkerControl &control ) 00080 { 00081 // correct empty orientation 00082 if ( control.orientation.w == 0 && control.orientation.x == 0 && 00083 control.orientation.y == 0 && control.orientation.z == 0 ) 00084 { 00085 control.orientation.w = 1; 00086 } 00087 00088 // add default tool tip if there is none 00089 if ( control.description.empty() ) 00090 { 00091 switch ( control.interaction_mode ) 00092 { 00093 case visualization_msgs::InteractiveMarkerControl::NONE: 00094 break; 00095 00096 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS: 00097 control.description = "Drag to move along the axis."; 00098 break; 00099 00100 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE: 00101 control.description = "Drag to move in the plane."; 00102 break; 00103 00104 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS: 00105 control.description = "Drag to rotate."; 00106 break; 00107 00108 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE: 00109 control.description = "Drag to rotate and move."; 00110 break; 00111 00112 case visualization_msgs::InteractiveMarkerControl::BUTTON: 00113 control.description = "Click to activate."; 00114 break; 00115 00116 case visualization_msgs::InteractiveMarkerControl::MENU: 00117 control.description = "Menu"; 00118 break; 00119 00120 default: 00121 break; 00122 } 00123 } 00124 00125 // add default control handles if there are none 00126 if ( control.markers.empty() ) 00127 { 00128 switch ( control.interaction_mode ) 00129 { 00130 case visualization_msgs::InteractiveMarkerControl::NONE: 00131 break; 00132 00133 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS: 00134 control.markers.reserve(2); 00135 makeArrow( msg, control, 1.0 ); 00136 makeArrow( msg, control, -1.0 ); 00137 break; 00138 00139 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE: 00140 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS: 00141 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE: 00142 makeDisc( msg, control ); 00143 break; 00144 00145 case visualization_msgs::InteractiveMarkerControl::BUTTON: 00146 break; 00147 00148 case visualization_msgs::InteractiveMarkerControl::MENU: 00149 makeViewFacingButton( msg, control, control.description ); 00150 break; 00151 00152 default: 00153 break; 00154 } 00155 } 00156 00157 // get interactive marker pose for later 00158 btQuaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y, 00159 msg.pose.orientation.z, msg.pose.orientation.w ); 00160 btVector3 int_marker_position( msg.pose.position.x, msg.pose.position.y, msg.pose.position.z ); 00161 00162 // fill in missing pose information into the markers 00163 for ( unsigned m=0; m<control.markers.size(); m++ ) 00164 { 00165 visualization_msgs::Marker &marker = control.markers[m]; 00166 00167 if ( marker.scale.x == 0 ) 00168 { 00169 marker.scale.x = 1; 00170 } 00171 if ( marker.scale.y == 0 ) 00172 { 00173 marker.scale.y = 1; 00174 } 00175 if ( marker.scale.z == 0 ) 00176 { 00177 marker.scale.z = 1; 00178 } 00179 00180 marker.ns = msg.name; 00181 00182 // correct empty orientation 00183 if ( marker.pose.orientation.w == 0 && marker.pose.orientation.x == 0 && 00184 marker.pose.orientation.y == 0 && marker.pose.orientation.z == 0 ) 00185 { 00186 marker.pose.orientation.w = 1; 00187 } 00188 00189 //normalize orientation 00190 btQuaternion marker_orientation( marker.pose.orientation.x, marker.pose.orientation.y, 00191 marker.pose.orientation.z, marker.pose.orientation.w ); 00192 btVector3 marker_position( marker.pose.position.x, marker.pose.position.y, marker.pose.position.z ); 00193 00194 marker_orientation.normalize(); 00195 00196 // if the header is empty, interpret as local coordinates relative to interactive marker pose 00197 if ( marker.header.frame_id.empty() ) 00198 { 00199 marker.header = msg.header; 00200 00201 // interpret marker pose as relative to interactive marker pose 00202 if ( control.orientation_mode == visualization_msgs::InteractiveMarkerControl::INHERIT ) 00203 { 00204 marker_orientation = int_marker_orientation * marker_orientation; 00205 } 00206 marker_position = int_marker_position + ( btMatrix3x3(int_marker_orientation) * marker_position); 00207 } 00208 00209 // write back corrected pose 00210 marker.pose.position.x = marker_position.x(); 00211 marker.pose.position.y = marker_position.y(); 00212 marker.pose.position.z = marker_position.z(); 00213 00214 marker.pose.orientation.x = marker_orientation.x(); 00215 marker.pose.orientation.y = marker_orientation.y(); 00216 marker.pose.orientation.z = marker_orientation.z(); 00217 marker.pose.orientation.w = marker_orientation.w(); 00218 00219 static volatile unsigned id = 0; 00220 marker.id = id++; 00221 marker.ns = msg.name; 00222 } 00223 } 00224 00225 void makeArrow( const visualization_msgs::InteractiveMarker &msg, 00226 visualization_msgs::InteractiveMarkerControl &control, float pos ) 00227 { 00228 visualization_msgs::Marker marker; 00229 00230 // rely on the auto-completion for the correct orientation 00231 marker.pose.orientation = control.orientation; 00232 00233 marker.type = visualization_msgs::Marker::ARROW; 00234 marker.scale.x = msg.scale * 0.3; 00235 marker.scale.y = msg.scale * 0.5; 00236 marker.scale.z = msg.scale * 0.2; 00237 00238 assignDefaultColor(marker, control.orientation); 00239 00240 float dist = fabs(pos); 00241 float dir = pos > 0 ? 1 : -1; 00242 00243 float inner = 0.5 * dist; 00244 float outer = inner + 0.4; 00245 00246 marker.points.resize(2); 00247 marker.points[0].x = dir * msg.scale * inner; 00248 marker.points[1].x = dir * msg.scale * outer; 00249 00250 control.markers.push_back( marker ); 00251 } 00252 00253 void makeDisc( const visualization_msgs::InteractiveMarker &msg, 00254 visualization_msgs::InteractiveMarkerControl &control, float width ) 00255 { 00256 visualization_msgs::Marker marker; 00257 00258 // rely on the auto-completion for the correct orientation 00259 marker.pose.orientation = control.orientation; 00260 00261 marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 00262 marker.scale.x = msg.scale; 00263 marker.scale.y = msg.scale; 00264 marker.scale.z = msg.scale; 00265 00266 assignDefaultColor(marker, control.orientation); 00267 00268 // compute points on a circle in the y-z plane 00269 int steps = 36; 00270 std::vector<geometry_msgs::Point> circle1, circle2; 00271 circle1.reserve(steps); 00272 circle2.reserve(steps); 00273 00274 geometry_msgs::Point v1,v2; 00275 00276 for ( int i=0; i<steps; i++ ) 00277 { 00278 float a = float(i)/float(steps) * M_PI * 2.0; 00279 00280 v1.y = 0.5 * cos(a); 00281 v1.z = 0.5 * sin(a); 00282 00283 v2.y = (1+width) * v1.y; 00284 v2.z = (1+width) * v1.z; 00285 00286 circle1.push_back( v1 ); 00287 circle2.push_back( v2 ); 00288 } 00289 00290 //construct disc from several segments, as otherwise z sorting won't work nicely 00291 control.markers.reserve( control.markers.size() + steps ); 00292 00293 switch ( control.interaction_mode ) 00294 { 00295 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS: 00296 { 00297 marker.points.resize(6); 00298 std_msgs::ColorRGBA base_color = marker.color; 00299 for ( int i=0; i<steps; i++ ) 00300 { 00301 int i1 = i; 00302 int i2 = (i+1) % steps; 00303 int i3 = (i+2) % steps; 00304 00305 marker.points[0] = circle1[i1]; 00306 marker.points[1] = circle2[i2]; 00307 marker.points[2] = circle1[i2]; 00308 00309 marker.points[3] = circle1[i2]; 00310 marker.points[4] = circle2[i2]; 00311 marker.points[5] = circle2[i3]; 00312 00313 float t = 0.6 + 0.4 * (i%2); 00314 marker.color.r = base_color.r * t; 00315 marker.color.g = base_color.g * t; 00316 marker.color.b = base_color.b * t; 00317 control.markers.push_back(marker); 00318 } 00319 break; 00320 } 00321 00322 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE: 00323 { 00324 marker.points.resize(6); 00325 std_msgs::ColorRGBA base_color = marker.color; 00326 for ( int i=0; i<steps-1; i+=2 ) 00327 { 00328 int i1 = i; 00329 int i2 = (i+1) % steps; 00330 int i3 = (i+2) % steps; 00331 00332 marker.points[0] = circle1[i1]; 00333 marker.points[1] = circle2[i2]; 00334 marker.points[2] = circle1[i2]; 00335 00336 marker.points[3] = circle1[i2]; 00337 marker.points[4] = circle2[i2]; 00338 marker.points[5] = circle1[i3]; 00339 00340 marker.color.r = base_color.r * 0.6; 00341 marker.color.g = base_color.g * 0.6; 00342 marker.color.b = base_color.b * 0.6; 00343 control.markers.push_back(marker); 00344 00345 marker.points[0] = circle2[i1]; 00346 marker.points[1] = circle2[i2]; 00347 marker.points[2] = circle1[i1]; 00348 00349 marker.points[3] = circle2[i2]; 00350 marker.points[4] = circle2[i3]; 00351 marker.points[5] = circle1[i3]; 00352 00353 marker.color = base_color; 00354 control.markers.push_back(marker); 00355 } 00356 break; 00357 } 00358 00359 default: 00360 marker.points.resize(6); 00361 00362 for ( int i=0; i<steps; i++ ) 00363 { 00364 int i1 = i; 00365 int i2 = (i+1) % steps; 00366 00367 marker.points[0] = circle1[i1]; 00368 marker.points[1] = circle2[i1]; 00369 marker.points[2] = circle1[i2]; 00370 00371 marker.points[3] = circle2[i1]; 00372 marker.points[4] = circle2[i2]; 00373 marker.points[5] = circle1[i2]; 00374 00375 control.markers.push_back(marker); 00376 } 00377 } 00378 } 00379 00380 void makeViewFacingButton( const visualization_msgs::InteractiveMarker &msg, 00381 visualization_msgs::InteractiveMarkerControl &control, std::string text ) 00382 { 00383 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; 00384 control.independent_marker_orientation = false; 00385 00386 visualization_msgs::Marker marker; 00387 00388 float base_scale = 0.25 * msg.scale; 00389 float base_z = 1.2 * msg.scale; 00390 00391 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 00392 marker.scale.x = base_scale; 00393 marker.scale.y = base_scale; 00394 marker.scale.z = base_scale; 00395 marker.color.r = 1.0; 00396 marker.color.g = 1.0; 00397 marker.color.b = 1.0; 00398 marker.color.a = 1.0; 00399 marker.pose.position.x = base_scale * -0.1; 00400 marker.pose.position.z = base_z + base_scale * -0.1; 00401 marker.text = text; 00402 00403 control.markers.push_back( marker ); 00404 } 00405 00406 00407 void assignDefaultColor(visualization_msgs::Marker &marker, const geometry_msgs::Quaternion &quat ) 00408 { 00409 geometry_msgs::Vector3 v; 00410 00411 btQuaternion bt_quat( quat.x, quat.y, quat.z, quat.w ); 00412 btVector3 bt_x_axis = btMatrix3x3(bt_quat) * btVector3(1,0,0); 00413 00414 float x,y,z; 00415 x = fabs(bt_x_axis.x()); 00416 y = fabs(bt_x_axis.y()); 00417 z = fabs(bt_x_axis.z()); 00418 00419 float max_xy = x>y ? x : y; 00420 float max_yz = y>z ? y : z; 00421 float max_xyz = max_xy > max_yz ? max_xy : max_yz; 00422 00423 marker.color.r = x / max_xyz; 00424 marker.color.g = y / max_xyz; 00425 marker.color.b = z / max_xyz; 00426 marker.color.a = 0.5; 00427 } 00428 00429 00430 visualization_msgs::InteractiveMarkerControl makeTitle( visualization_msgs::InteractiveMarker &msg ) 00431 { 00432 visualization_msgs::Marker marker; 00433 00434 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 00435 marker.scale.x = msg.scale * 0.15; 00436 marker.scale.y = msg.scale * 0.15; 00437 marker.scale.z = msg.scale * 0.15; 00438 marker.color.r = 1.0; 00439 marker.color.g = 1.0; 00440 marker.color.b = 1.0; 00441 marker.color.a = 1.0; 00442 marker.pose.position.z = msg.scale * 1.4; 00443 marker.text = msg.description; 00444 00445 visualization_msgs::InteractiveMarkerControl control; 00446 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::NONE; 00447 control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING; 00448 control.always_visible = true; 00449 control.markers.push_back( marker ); 00450 00451 autoComplete( msg, control ); 00452 00453 return control; 00454 } 00455 00456 }