tools.cpp
Go to the documentation of this file.
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 <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   // this is a 'delete' message. no need for action.
00047   if ( msg.controls.empty() )
00048   {
00049     return;
00050   }
00051 
00052   // default size
00053   if ( msg.scale == 0 )
00054   {
00055     msg.scale = 1;
00056   }
00057 
00058   // correct empty orientation, normalize
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   //normalize quaternion
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   // complete the controls
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   // correct empty orientation
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   // add default control handles if there are none
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   // get interactive marker pose for later
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   // fill in missing pose information into the markers
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     // correct empty orientation
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     //normalize orientation
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     // If transparency is disabled, set alpha to 1.0 for all semi-transparent markers
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   // rely on the auto-completion for the correct orientation
00204   marker.pose.orientation = control.orientation;
00205 
00206   marker.type = visualization_msgs::Marker::ARROW;
00207   marker.scale.x = msg.scale * 0.15; // aleeper: changed from 0.3 due to Rviz fix
00208   marker.scale.y = msg.scale * 0.25; // aleeper: changed from 0.5 due to Rviz fix
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   // rely on the auto-completion for the correct orientation
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   // compute points on a circle in the y-z plane
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 }


interactive_markers
Author(s): David Gossow
autogenerated on Fri Aug 28 2015 11:11:26