basic_grasping_perception.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013-2014, Unbounded Robotics 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 Unbounded Robotics, 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 // Author: Michael Ferguson
00031 
00032 #include <ros/ros.h>
00033 #include <actionlib/server/simple_action_server.h>
00034 #include <tf/transform_listener.h>
00035 
00036 #include <simple_grasping/object_support_segmentation.h>
00037 #include <simple_grasping/shape_grasp_planner.h>
00038 #include <grasping_msgs/FindGraspableObjectsAction.h>
00039 
00040 #include <pcl_ros/point_cloud.h>
00041 #include <pcl_ros/transforms.h>
00042 #include <pcl/filters/passthrough.h>
00043 #include <pcl_conversions/pcl_conversions.h>
00044 
00045 namespace simple_grasping
00046 {
00047 
00051 class BasicGraspingPerception
00052 {
00053   typedef actionlib::SimpleActionServer<grasping_msgs::FindGraspableObjectsAction> server_t;
00054 
00055 public:
00056   BasicGraspingPerception(ros::NodeHandle n) : nh_(n), debug_(false), find_objects_(false)
00057   {
00058     // use_debug: enable/disable output of a cloud containing object points
00059     nh_.getParam("use_debug", debug_);
00060 
00061     // frame_id: frame to transform cloud to (should be XY horizontal)
00062     world_frame_ = "base_link";
00063     nh_.getParam("frame_id", world_frame_);
00064 
00065     // Create planner
00066     planner_.reset(new ShapeGraspPlanner(nh_));
00067 
00068     // Create perception
00069     segmentation_.reset(new ObjectSupportSegmentation(nh_));
00070 
00071     // Advertise an action for perception + planning
00072     server_.reset(new server_t(nh_, "find_objects",
00073                                boost::bind(&BasicGraspingPerception::executeCallback, this, _1),
00074                                false));
00075 
00076     // Publish debugging views
00077     if (debug_)
00078     {
00079       object_cloud_pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("object_cloud", 1);
00080       support_cloud_pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("support_cloud", 1);
00081     }
00082 
00083     // Range filter for cloud
00084     range_filter_.setFilterFieldName("z");
00085     range_filter_.setFilterLimits(0, 2.5);
00086 
00087     // Subscribe to head camera cloud
00088     cloud_sub_ = nh_.subscribe< pcl::PointCloud<pcl::PointXYZRGB> >("/head_camera/depth_registered/points",
00089                                                                      1,
00090                                                                      &BasicGraspingPerception::cloudCallback,
00091                                                                      this);
00092 
00093     // Start thread for action
00094     server_->start();
00095 
00096     ROS_INFO("basic_grasping_perception initialized");
00097   }
00098 
00099 private:
00100   void cloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
00101   {
00102     // be lazy
00103     if (!find_objects_)
00104       return;
00105 
00106     ROS_DEBUG("Cloud recieved with %d points.", static_cast<int>(cloud->points.size()));
00107 
00108     // Filter out noisy long-range points
00109     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
00110     range_filter_.setInputCloud(cloud);
00111     range_filter_.filter(*cloud_filtered);
00112     ROS_DEBUG("Filtered for range, now %d points.", static_cast<int>(cloud_filtered->points.size()));
00113 
00114     // Transform to grounded
00115     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
00116     if (!pcl_ros::transformPointCloud(world_frame_, *cloud_filtered, *cloud_transformed, listener_))
00117     {
00118       ROS_ERROR("Error transforming to frame %s", world_frame_.c_str());
00119       return;
00120     }
00121 
00122     // Run segmentation
00123     objects_.clear();
00124     supports_.clear();
00125     pcl::PointCloud<pcl::PointXYZRGB> object_cloud;
00126     pcl::PointCloud<pcl::PointXYZRGB> support_cloud;
00127     if (debug_)
00128     {
00129       object_cloud.header.frame_id = cloud_transformed->header.frame_id;
00130       support_cloud.header.frame_id = cloud_transformed->header.frame_id;
00131     }
00132     segmentation_->segment(cloud_transformed, objects_, supports_, object_cloud, support_cloud, debug_);
00133 
00134     if (debug_)
00135     {
00136       object_cloud_pub_.publish(object_cloud);
00137       support_cloud_pub_.publish(support_cloud);
00138     }
00139 
00140     // Ok to continue processing
00141     find_objects_ = false;
00142   }
00143 
00144   void executeCallback(const grasping_msgs::FindGraspableObjectsGoalConstPtr& goal)
00145   {
00146     grasping_msgs::FindGraspableObjectsResult result;
00147 
00148     // Get objects
00149     find_objects_ = true;
00150     ros::Time t = ros::Time::now();
00151     while (find_objects_ == true)
00152     {
00153       ros::Duration(1/50.0).sleep();
00154       if (ros::Time::now() - t > ros::Duration(3.0))
00155       {
00156         find_objects_ = false;
00157         server_->setAborted(result, "Failed to get camera data in alloted time.");
00158         ROS_ERROR("Failed to get camera data in alloted time.");
00159         return;
00160       }
00161     }
00162 
00163     // Set object results
00164     for (size_t i = 0; i < objects_.size(); ++i)
00165     {
00166       grasping_msgs::GraspableObject g;
00167       g.object = objects_[i];
00168       if (goal->plan_grasps)
00169       {
00170         // Plan grasps for object
00171         planner_->plan(objects_[i], g.grasps);
00172       }
00173       result.objects.push_back(g);
00174     }
00175     // Set support surfaces
00176     result.support_surfaces = supports_;
00177 
00178     server_->setSucceeded(result, "Succeeded.");
00179   }
00180 
00181   ros::NodeHandle nh_;
00182 
00183   bool debug_;
00184 
00185   tf::TransformListener listener_;
00186   std::string world_frame_;
00187 
00188   bool find_objects_;
00189   std::vector<grasping_msgs::Object> objects_;
00190   std::vector<grasping_msgs::Object> supports_;
00191 
00192   ros::Subscriber cloud_sub_;
00193   ros::Publisher object_cloud_pub_;
00194   ros::Publisher support_cloud_pub_;
00195 
00196   boost::shared_ptr<ShapeGraspPlanner> planner_;
00197   boost::shared_ptr<ObjectSupportSegmentation> segmentation_;
00198 
00199   boost::shared_ptr<server_t> server_;
00200 
00201   pcl::PassThrough<pcl::PointXYZRGB> range_filter_;
00202 };
00203 
00204 }  // namespace simple_grasping
00205 
00206 int main(int argc, char* argv[])
00207 {
00208   ros::init(argc, argv, "basic_grasping_perception");
00209   ros::NodeHandle n("~");
00210   simple_grasping::BasicGraspingPerception perception_n_planning(n);
00211   ros::spin();
00212   return 0;
00213 }


simple_grasping
Author(s): Michael Ferguson
autogenerated on Fri Aug 26 2016 13:17:24