block_manipulation.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2011, Vanadium Labs LLC
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 Vanadium Labs LLC nor the names of its 
00014  *       contributors may be used to endorse or promote products derived 
00015  *       from this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020  * DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00021  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027  * 
00028  * Author: Michael Ferguson, Helen Oleynikova
00029  */
00030 
00031 
00032 #include <ros/ros.h>
00033 #include <sensor_msgs/PointCloud2.h>
00034 
00035 #include <actionlib/client/simple_action_client.h>
00036 #include <simple_arm_server/MoveArmAction.h>
00037 
00038 #include <pcl/ros/conversions.h>
00039 #include <pcl/point_cloud.h>
00040 #include <pcl/point_types.h>
00041 #include <pcl/filters/passthrough.h>
00042 #include <pcl/segmentation/extract_clusters.h>
00043 #include <pcl/kdtree/kdtree.h>
00044 
00045 #include <pcl_ros/point_cloud.h>
00046 #include <pcl_ros/transforms.h>
00047 
00048 #include <interactive_markers/interactive_marker_server.h>
00049 
00050 #include <vector>
00051 
00052 
00053 using namespace visualization_msgs;
00054 
00055 const std::string arm_link = "/arm_base_link";
00056 const double gripper_open = 0.04;
00057 const double gripper_closed = 0.024;
00058 
00059 const double z_up = 0.08;
00060 const double z_down = -0.04;
00061 
00062 const double block_size = 0.0127;
00063 
00064 // Block storage
00065 class Block
00066 {
00067   public:
00068     int id;
00069     bool active;
00070     double x;
00071     double y;
00072     
00073     Block(const Block& b) : id(b.id), active(b.active), x(b.x), y(b.y) {}
00074     Block(int _id, double _x, double _y) : id(_id) , active(true), x(_x), y(_y) {}
00075 
00076     std::string getName()
00077     {
00078       std::stringstream conv;
00079       conv << id;
00080       return std::string("block") + conv.str(); 
00081     }
00082 };
00083 
00084 
00085 class BlockManipulation
00086 {
00087 private:
00088   interactive_markers::InteractiveMarkerServer server;
00089   actionlib::SimpleActionClient<simple_arm_server::MoveArmAction> client_;
00090   ros::Publisher pub_;
00091   ros::Subscriber sub_;
00092   tf::TransformListener tf_listener_;
00093   int markers_;
00094   int moving_;
00095   int skip_;
00096   float x_, y_;
00097   std::string last_block_;
00098   std::vector<Block> marker_names_;
00099   
00100   ros::NodeHandle nh;
00101 
00102 public:
00103 
00104   BlockManipulation() : server("block_controls"), client_("move_arm", true)
00105   {
00106     // create marker server
00107     markers_ = 0;
00108     last_block_ = std::string("");
00109 
00110     ros::Duration(0.1).sleep();
00111     
00112     skip_ = 0;
00113 
00114     // open gripper
00115     simple_arm_server::MoveArmGoal goal;
00116     simple_arm_server::ArmAction grip;
00117     grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
00118     grip.command = gripper_open;
00119     goal.motions.push_back(grip);
00120     goal.header.frame_id = arm_link;
00121     client_.sendGoal(goal);
00122     client_.waitForResult(/*ros::Duration(30.0)*/);
00123 
00124     // subscribe to point cloud
00125     sub_ = nh.subscribe("/camera/depth_registered/points", 1, &BlockManipulation::cloudCb, this);
00126     pub_ = nh.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("block_output", 1);
00127 
00128     server.applyChanges();
00129     
00130     ROS_INFO("Finished initializing.");
00131   }
00132 
00133   // Move the real block!
00134   void moveBlock( const InteractiveMarkerFeedbackConstPtr &feedback )
00135   {
00136     switch ( feedback->event_type )
00137     {
00138       case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
00139         ROS_INFO_STREAM("Staging " << feedback->marker_name);     
00140         x_ = feedback->pose.position.x;
00141         y_ = feedback->pose.position.y;
00142         last_block_ = feedback->marker_name;
00143         break;
00144    
00145       case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
00146         moving_ = true;
00147         ROS_INFO_STREAM("Now moving " << feedback->marker_name); 
00148 
00149         simple_arm_server::MoveArmGoal goal;
00150         simple_arm_server::ArmAction action;
00151         
00152         /* arm straight up */
00153         tf::Quaternion temp;
00154         temp.setRPY(0,1.57,0);
00155         action.goal.orientation.x = temp.getX();
00156         action.goal.orientation.y = temp.getY();
00157         action.goal.orientation.z = temp.getZ();
00158         action.goal.orientation.w = temp.getW();
00159 
00160         /* hover over */
00161         action.goal.position.x = x_;
00162         action.goal.position.y = y_;
00163         action.goal.position.z = z_up;
00164         goal.motions.push_back(action);
00165         action.move_time.sec = 1.5;
00166 
00167         /* go down */
00168         action.goal.position.z = z_down;
00169         goal.motions.push_back(action);
00170         action.move_time.sec = 1.5;
00171 
00172         /* close gripper */
00173         simple_arm_server::ArmAction grip;
00174         grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
00175         grip.command = gripper_closed;
00176         grip.move_time.sec = 1.0;
00177         goal.motions.push_back(grip);
00178 
00179         /* go up */
00180         action.goal.position.z = z_up;
00181         goal.motions.push_back(action);
00182         action.move_time.sec = 0.25;
00183 
00184         /* hover over */
00185         action.goal.position.x = feedback->pose.position.x;
00186         action.goal.position.y = feedback->pose.position.y;
00187         action.goal.position.z = z_up;
00188         goal.motions.push_back(action);
00189         action.move_time.sec = 1.5;
00190 
00191         /* go down */
00192         action.goal.position.z = z_down;
00193         goal.motions.push_back(action);
00194         action.move_time.sec = 1.5;
00195 
00196         /* open gripper */
00197         grip.command = gripper_open;
00198         goal.motions.push_back(grip);
00199 
00200         /* go up */
00201         action.goal.position.z = z_up;
00202         goal.motions.push_back(action);
00203         action.move_time.sec = 0.25;
00204       
00205         goal.header.frame_id = arm_link;
00206         client_.sendGoal(goal);
00207         client_.waitForResult(/*ros::Duration(30.0)*/);
00208         /* update location */ 
00209         for( std::vector<Block>::iterator it=marker_names_.begin(); it < marker_names_.end(); it++)
00210         {
00211           if( it->getName() == feedback->marker_name )
00212           {
00213             it->x = feedback->pose.position.x;
00214             it->y = feedback->pose.position.y;
00215             break;
00216           }
00217         }
00218 
00219         moving_ = false;
00220         break;
00221     }
00222     
00223     server.applyChanges(); 
00224   }
00225 
00226   // Make a box
00227   Marker makeBox( InteractiveMarker &msg, float r, float g, float b )
00228   {
00229     Marker m;
00230 
00231     m.type = Marker::CUBE;
00232     m.scale.x = msg.scale;
00233     m.scale.y = msg.scale;
00234     m.scale.z = msg.scale;
00235     m.color.r = r;
00236     m.color.g = g;
00237     m.color.b = b;
00238     m.color.a = 1.0;
00239 
00240     return m;
00241   }
00242    
00243   // Add a new block
00244   void addBlock( float x, float y, float z, float rz, float r, float g, float b, int n)
00245   {
00246     InteractiveMarker marker;
00247     marker.header.frame_id = arm_link;
00248     marker.pose.position.x = x;
00249     marker.pose.position.y = y;
00250     marker.pose.position.z = z;
00251     marker.scale = 0.03;
00252     
00253     Block block( n, x, y );
00254     marker_names_.push_back( block );
00255     marker.name = block.getName(); 
00256     marker.description = "Another block";
00257 
00258     InteractiveMarkerControl control;
00259     control.orientation.w = 1;
00260     control.orientation.x = 0;
00261     control.orientation.y = 1;
00262     control.orientation.z = 0;
00263     control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00264     marker.controls.push_back( control );
00265     
00266     control.markers.push_back( makeBox(marker, r, g, b) );
00267     control.always_visible = true;
00268     marker.controls.push_back( control );
00269     
00270 
00271     server.insert( marker );
00272     server.setCallback( marker.name, boost::bind( &BlockManipulation::moveBlock, this, _1 ));
00273   }
00274 
00275   // Process an incoming cloud
00276   void cloudCb ( const sensor_msgs::PointCloud2ConstPtr& msg )
00277   {
00278     if( (skip_++)%15 != 0 ) return;
00279     
00280     // convert to PCL
00281     pcl::PointCloud<pcl::PointXYZRGB> cloud;
00282     pcl::fromROSMsg (*msg, cloud);
00283     
00284     // transform to base_link
00285     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZRGB>);
00286     
00287     tf_listener_.waitForTransform(std::string(arm_link), cloud.header.frame_id, cloud.header.stamp, ros::Duration(1.0));
00288     if (!pcl_ros::transformPointCloud (std::string(arm_link), cloud, *cloud_transformed, tf_listener_))
00289     {
00290       ROS_ERROR ("Error converting to base_link");
00291       return;
00292     }
00293 
00294     // drop things on ground
00295     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
00296     pcl::PassThrough<pcl::PointXYZRGB> pass;
00297     pass.setInputCloud(cloud_transformed); 
00298     pass.setFilterFieldName("z");
00299     pass.setFilterLimits(z_down - 0.01, z_down + block_size*2);
00300     pass.filter(*cloud_filtered);
00301     if( cloud_filtered->points.size() == 0 ){
00302       ROS_ERROR("0 points left");
00303       return;
00304     }else
00305       ROS_INFO("Filtered, %d points left", (int) cloud_filtered->points.size());
00306     pub_.publish(*cloud_filtered);
00307 
00308     // cluster
00309     pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
00310     tree->setInputCloud(cloud_filtered);
00311     
00312     std::vector<pcl::PointIndices> clusters;
00313     pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
00314     ec.setClusterTolerance(0.02);
00315     ec.setMinClusterSize(20);
00316     ec.setMaxClusterSize(25000);
00317     ec.setSearchMethod(tree);
00318     ec.setInputCloud(cloud_filtered);
00319     ec.extract(clusters);
00320 
00321     // need to delete old blocks
00322     for( std::vector<Block>::iterator it=marker_names_.begin(); it < marker_names_.end(); it++)
00323     {
00324       it->active = false;
00325     }
00326 
00327     // for each cluster, see if it is a block
00328     for (size_t c = 0; c < clusters.size (); ++c)
00329     {  
00330       // find cluster centroid/color
00331       float x = 0; float y = 0; float z = 0; int r = 0; int g = 0; int b = 0;
00332       for (size_t i = 0; i < clusters[c].indices.size(); i++)
00333       {
00334           int j = clusters[c].indices[i];
00335           x += cloud_filtered->points[j].x;
00336           y += cloud_filtered->points[j].y;
00337           z += cloud_filtered->points[j].z;
00338           
00339           r += cloud_filtered->points[j].r;
00340           g += cloud_filtered->points[j].g;
00341           b += cloud_filtered->points[j].b;
00342       }
00343       x = x/clusters[c].indices.size();
00344       y = y/clusters[c].indices.size();
00345       z = z/clusters[c].indices.size();
00346       
00347       r = r/clusters[c].indices.size();
00348       g = g/clusters[c].indices.size();
00349       b = b/clusters[c].indices.size();
00350 
00351       bool new_ = true;
00352       // see if we have it detected
00353       for( std::vector<Block>::iterator it=marker_names_.begin(); it < marker_names_.end(); it++)
00354       {
00355         if( (fabs(it->x - x) < block_size*2) &&
00356             (fabs(it->y - y) < block_size*2) )
00357         {
00358           new_ = false;
00359           it->active = true;
00360           break;
00361         }
00362       }
00363 
00364       if (new_){
00365         // else, add new block
00366         addBlock( x, y, z - block_size, 0.0, (float) r/255.0, (float) g/255.0, (float) b/255.0, markers_++ );
00367       }
00368     }
00369 
00370     // need to delete old blocks
00371     for( std::vector<Block>::iterator it=marker_names_.begin(); it < marker_names_.end(); )
00372     {
00373       if(it->active or it->getName() == last_block_){
00374         it++;
00375       }else{
00376         server.erase( it->getName() );
00377         it = marker_names_.erase(it);
00378       }
00379     }
00380 
00381     server.applyChanges();
00382 }
00383 
00384 };
00385 
00386 int main(int argc, char** argv)
00387 {
00388   // initialize node
00389   ros::init(argc, argv, "block_manipulation");
00390 
00391   BlockManipulation manip;
00392 
00393   // everything is done in cloud callback, just spin
00394   ros::spin();
00395 }
00396 


turtlebot_block_manipulation
Author(s): Michael Ferguson, Helen Oleynikova
autogenerated on Mon Oct 6 2014 08:07:45