cube.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 
00031 #include <ros/ros.h>
00032 
00033 #include <interactive_markers/interactive_marker_server.h>
00034 
00035 #include <math.h>
00036 
00037 #include <tf/LinearMath/Vector3.h>
00038 
00039 
00040 using namespace visualization_msgs;
00041 
00042 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
00043 
00044 std::vector< tf::Vector3 > positions;
00045 
00046 void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00047 {
00048   switch ( feedback->event_type )
00049   {
00050     case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00051     {
00052       //compute difference vector for this cube
00053 
00054       tf::Vector3 fb_pos(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z);
00055       unsigned index = atoi( feedback->marker_name.c_str() );
00056 
00057       if ( index > positions.size() )
00058       {
00059         return;
00060       }
00061       tf::Vector3 fb_delta = fb_pos - positions[index];
00062 
00063       // move all markers in that direction
00064       for ( unsigned i=0; i<positions.size(); i++ )
00065       {
00066         float d = fb_pos.distance( positions[i] );
00067         float t = 1 / (d*5.0+1.0) - 0.2;
00068         if ( t < 0.0 ) t=0.0;
00069 
00070         positions[i] += t * fb_delta;
00071 
00072         if ( i == index ) {
00073           ROS_INFO_STREAM( d );
00074           positions[i] = fb_pos;
00075         }
00076 
00077         geometry_msgs::Pose pose;
00078         pose.position.x = positions[i].x();
00079         pose.position.y = positions[i].y();
00080         pose.position.z = positions[i].z();
00081 
00082         std::stringstream s;
00083         s << i;
00084         server->setPose( s.str(), pose );
00085       }
00086 
00087 
00088       break;
00089     }
00090   }
00091   server->applyChanges();
00092 }
00093 
00094 InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
00095 {
00096   InteractiveMarkerControl control;
00097   control.always_visible = true;
00098   control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
00099   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00100   control.independent_marker_orientation = true;
00101 
00102   Marker marker;
00103 
00104   marker.type = Marker::CUBE;
00105   marker.scale.x = msg.scale;
00106   marker.scale.y = msg.scale;
00107   marker.scale.z = msg.scale;
00108   marker.color.r = 0.65+0.7*msg.pose.position.x;
00109   marker.color.g = 0.65+0.7*msg.pose.position.y;
00110   marker.color.b = 0.65+0.7*msg.pose.position.z;
00111   marker.color.a = 1.0;
00112 
00113   control.markers.push_back( marker );
00114   msg.controls.push_back( control );
00115 
00116   return msg.controls.back();
00117 }
00118 
00119 
00120 void makeCube( )
00121 {
00122   int side_length = 10;
00123   float step = 1.0/ (float)side_length;
00124   int count = 0;
00125 
00126   positions.reserve( side_length*side_length*side_length );
00127 
00128   for ( double x=-0.5; x<0.5; x+=step )
00129   {
00130     for ( double y=-0.5; y<0.5; y+=step )
00131     {
00132       for ( double z=0.0; z<1.0; z+=step )
00133       {
00134         InteractiveMarker int_marker;
00135         int_marker.header.frame_id = "/base_link";
00136         int_marker.scale = step;
00137 
00138         int_marker.pose.position.x = x;
00139         int_marker.pose.position.y = y;
00140         int_marker.pose.position.z = z;
00141 
00142         positions.push_back( tf::Vector3(x,y,z) );
00143 
00144         std::stringstream s;
00145         s << count;
00146         int_marker.name = s.str();
00147 
00148         makeBoxControl(int_marker);
00149 
00150         server->insert( int_marker );
00151         server->setCallback( int_marker.name, &processFeedback );
00152 
00153         count++;
00154       }
00155     }
00156   }
00157 }
00158 
00159 int main(int argc, char** argv)
00160 {
00161   ros::init(argc, argv, "cube");
00162 
00163   server.reset( new interactive_markers::InteractiveMarkerServer("cube") );
00164 
00165   ros::Duration(0.1).sleep();
00166 
00167   ROS_INFO("initializing..");
00168   makeCube();
00169   server->applyChanges();
00170   ROS_INFO("ready.");
00171 
00172   ros::spin();
00173 
00174   server.reset();
00175 }


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Thu Aug 27 2015 15:40:39