selection.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 // %Tag(fullSource)%
00032 #include <ros/ros.h>
00033 #include <stdlib.h>
00034 
00035 #include <interactive_markers/interactive_marker_server.h>
00036 #include <interactive_markers/tools.h>
00037 
00038 #include <tf/LinearMath/Vector3.h>
00039 
00040 bool testPointAgainstAabb2(const tf::Vector3 &aabbMin1, const tf::Vector3 &aabbMax1,
00041                            const tf::Vector3 &point)
00042 {
00043         bool overlap = true;
00044         overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
00045         overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
00046         overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
00047         return overlap;
00048 }
00049 
00050 namespace vm = visualization_msgs;
00051 
00052 class PointCouldSelector
00053 {
00054 public:
00055         PointCouldSelector( boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server,
00056             std::vector<tf::Vector3>& points ) :
00057               server_( server ),
00058         min_sel_( -1, -1, -1 ),
00059         max_sel_( 1, 1, 1 ),
00060         points_( points )
00061         {
00062           updateBox( );
00063           updatePointClouds();
00064 
00065           makeSizeHandles();
00066         }
00067 
00068         void processAxisFeedback(
00069             const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00070         {
00071           ROS_INFO_STREAM( feedback->marker_name << " is now at "
00072               << feedback->pose.position.x << ", " << feedback->pose.position.y
00073               << ", " << feedback->pose.position.z );
00074 
00075     if ( feedback->marker_name == "min_x" ) min_sel_.setX( feedback->pose.position.x );
00076     if ( feedback->marker_name == "max_x" ) max_sel_.setX( feedback->pose.position.x );
00077     if ( feedback->marker_name == "min_y" ) min_sel_.setY( feedback->pose.position.y );
00078     if ( feedback->marker_name == "max_y" ) max_sel_.setY( feedback->pose.position.y );
00079     if ( feedback->marker_name == "min_z" ) min_sel_.setZ( feedback->pose.position.z );
00080     if ( feedback->marker_name == "max_z" ) max_sel_.setZ( feedback->pose.position.z );
00081 
00082     updateBox( );
00083     updateSizeHandles();
00084 
00085     if ( feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP )
00086     {
00087       updatePointClouds();
00088     }
00089 
00090     server_->applyChanges();
00091         }
00092 
00093         vm::Marker makeBox( vm::InteractiveMarker &msg,
00094             tf::Vector3 min_bound, tf::Vector3 max_bound )
00095         {
00096           vm::Marker marker;
00097 
00098           marker.type = vm::Marker::CUBE;
00099           marker.scale.x = max_bound.x() - min_bound.x();
00100           marker.scale.y = max_bound.y() - min_bound.y();
00101           marker.scale.z = max_bound.z() - min_bound.z();
00102     marker.pose.position.x = 0.5 * ( max_bound.x() + min_bound.x() );
00103     marker.pose.position.y = 0.5 * ( max_bound.y() + min_bound.y() );
00104     marker.pose.position.z = 0.5 * ( max_bound.z() + min_bound.z() );
00105           marker.color.r = 0.5;
00106           marker.color.g = 0.5;
00107           marker.color.b = 0.5;
00108           marker.color.a = 0.5;
00109 
00110           return marker;
00111         }
00112 
00113         void updateBox( )
00114         {
00115           vm::InteractiveMarker msg;
00116           msg.header.frame_id = "base_link";
00117 
00118           vm::InteractiveMarkerControl control;
00119           control.always_visible = false;
00120           control.markers.push_back( makeBox(msg, min_sel_, max_sel_) );
00121           msg.controls.push_back( control );
00122 
00123           server_->insert( msg );
00124         }
00125 
00126         void updatePointCloud( std::string name, std_msgs::ColorRGBA color, std::vector<tf::Vector3> &points )
00127         {
00128           // create an interactive marker for our server
00129           vm::InteractiveMarker int_marker;
00130           int_marker.header.frame_id = "base_link";
00131           int_marker.name = name;
00132 
00133           // create a point cloud marker
00134           vm::Marker points_marker;
00135           points_marker.type = vm::Marker::SPHERE_LIST;
00136           points_marker.scale.x = 0.05;
00137           points_marker.scale.y = 0.05;
00138           points_marker.scale.z = 0.05;
00139           points_marker.color = color;
00140 
00141           for ( unsigned i=0; i<points.size(); i++ )
00142           {
00143             geometry_msgs::Point p;
00144       p.x = points[i].x();
00145       p.y = points[i].y();
00146       p.z = points[i].z();
00147             points_marker.points.push_back( p );
00148           }
00149 
00150           // create container control
00151           vm::InteractiveMarkerControl points_control;
00152           points_control.always_visible = true;
00153           points_control.interaction_mode = vm::InteractiveMarkerControl::NONE;
00154           points_control.markers.push_back( points_marker );
00155 
00156           // add the control to the interactive marker
00157           int_marker.controls.push_back( points_control );
00158 
00159           server_->insert( int_marker );
00160         }
00161 
00162         void updatePointClouds()
00163         {
00164           std::vector<tf::Vector3> points_in, points_out;
00165     points_in.reserve( points_.size() );
00166     points_out.reserve( points_.size() );
00167 
00168     // determine which points are selected (i.e. inside the selection box)
00169           for ( unsigned i=0; i<points_.size(); i++ )
00170           {
00171             if ( testPointAgainstAabb2( min_sel_, max_sel_, points_[i] ) )
00172             {
00173               points_in.push_back( points_[i] );
00174             }
00175             else
00176             {
00177         points_out.push_back( points_[i] );
00178             }
00179           }
00180 
00181     std_msgs::ColorRGBA in_color;
00182     in_color.r = 1.0;
00183     in_color.g = 0.8;
00184     in_color.b = 0.0;
00185     in_color.a = 1.0;
00186 
00187     std_msgs::ColorRGBA out_color;
00188     out_color.r = 0.5;
00189     out_color.g = 0.5;
00190     out_color.b = 0.5;
00191     out_color.a = 1.0;
00192 
00193     updatePointCloud( "selected_points", in_color, points_in );
00194     updatePointCloud( "unselected_points", out_color, points_out );
00195         }
00196 
00197   void makeSizeHandles( )
00198   {
00199     for ( int axis=0; axis<3; axis++ )
00200     {
00201       for ( int sign=-1; sign<=1; sign+=2 )
00202       {
00203         vm::InteractiveMarker int_marker;
00204         int_marker.header.frame_id = "base_link";
00205         int_marker.scale = 1.0;
00206 
00207         vm::InteractiveMarkerControl control;
00208         control.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
00209         control.orientation_mode = vm::InteractiveMarkerControl::INHERIT;
00210         control.always_visible = false;
00211 
00212         control.orientation.w = 1;
00213 
00214         switch ( axis )
00215         {
00216         case 0:
00217           int_marker.name = sign>0 ? "max_x" : "min_x";
00218           int_marker.pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x();
00219           int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
00220           int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
00221           control.orientation.x = 1;
00222           control.orientation.y = 0;
00223           control.orientation.z = 0;
00224           break;
00225         case 1:
00226           int_marker.name = sign>0 ? "max_y" : "min_y";
00227           int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
00228           int_marker.pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y();
00229           int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
00230           control.orientation.x = 0;
00231           control.orientation.y = 0;
00232           control.orientation.z = 1;
00233           break;
00234         default:
00235           int_marker.name = sign>0 ? "max_z" : "min_z";
00236           int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
00237           int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
00238           int_marker.pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z();
00239           control.orientation.x = 0;
00240           control.orientation.y = -1;
00241           control.orientation.z = 0;
00242           break;
00243         }
00244 
00245         interactive_markers::makeArrow( int_marker, control, 0.5 * sign );
00246 
00247         int_marker.controls.push_back( control );
00248         server_->insert( int_marker, boost::bind( &PointCouldSelector::processAxisFeedback, this, _1 ) );
00249       }
00250     }
00251   }
00252 
00253   void updateSizeHandles( )
00254   {
00255     for ( int axis=0; axis<3; axis++ )
00256     {
00257       for ( int sign=-1; sign<=1; sign+=2 )
00258       {
00259         std::string name;
00260         geometry_msgs::Pose pose;
00261 
00262         switch ( axis )
00263         {
00264         case 0:
00265           name = sign>0 ? "max_x" : "min_x";
00266           pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x();
00267           pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
00268           pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
00269           break;
00270         case 1:
00271           name = sign>0 ? "max_y" : "min_y";
00272           pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
00273           pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y();
00274           pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
00275           break;
00276         default:
00277           name = sign>0 ? "max_z" : "min_z";
00278           pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
00279           pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
00280           pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z();
00281           break;
00282         }
00283 
00284         server_->setPose( name, pose );
00285       }
00286     }
00287   }
00288 
00289 private:
00290         boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
00291 
00292         tf::Vector3 min_sel_, max_sel_;
00293         std::vector<tf::Vector3> points_;
00294 
00295         vm::InteractiveMarker sel_points_marker_;
00296         vm::InteractiveMarker unsel_points_marker_;
00297 };
00298 
00299 
00300 
00301 
00302 double rand( double min, double max )
00303 {
00304   double t = (double)rand() / (double)RAND_MAX;
00305   return min + t*(max-min);
00306 }
00307 
00308 
00309 void makePoints( std::vector<tf::Vector3>& points_out, int num_points )
00310 {
00311   double radius = 3;
00312   double scale = 0.2;
00313   points_out.resize(num_points);
00314   for( int i = 0; i < num_points; i++ )
00315   {
00316     points_out[i].setX( scale * rand( -radius, radius ) );
00317           points_out[i].setY( scale * rand( -radius, radius ) );
00318     points_out[i].setZ( scale * radius * 0.2 * ( sin( 10.0 / radius * points_out[i].x() ) + cos( 10.0 / radius * points_out[i].y() ) ) );
00319   }
00320 }
00321 
00322 
00323 int main(int argc, char** argv)
00324 {
00325   ros::init(argc, argv, "selection");
00326 
00327   // create an interactive marker server on the topic namespace simple_marker
00328   boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server(
00329       new interactive_markers::InteractiveMarkerServer("selection") );
00330 
00331   std::vector<tf::Vector3> points;
00332   makePoints( points, 10000 );
00333 
00334   PointCouldSelector selector( server, points );
00335 
00336   // 'commit' changes and send to all clients
00337   server->applyChanges();
00338 
00339   // start the ROS main loop
00340   ros::spin();
00341 }
00342 // %Tag(fullSource)%


interactive_marker_tutorials
Author(s): David Gossow
autogenerated on Fri Feb 12 2016 00:35:13