pr2_grasp_adjust: pr2_grasp_adjust: grasp_adjust_action

00001 /*
00002  * grasp_adjust_server
00003  * Copyright (c) 2010, Willow Garage, Inc.
00004  * All rights reserved.
00005  *
00006  * Software License Agreement (BSD License)
00007  *
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of Willow Garage nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00036 
00037 
00038 #include <ros/ros.h>
00039 
00040 #include <tf/tf.h>
00041 #include <tf/transform_listener.h>
00042 #include <tf/transform_broadcaster.h>
00043 
00044 #include <boost/bind.hpp>
00045 
00046 
00047 #include <std_msgs/String.h>
00048 #include <sensor_msgs/point_cloud_conversion.h>
00049 
00050 #include <visualization_msgs/Marker.h>
00051 #include <visualization_msgs/MarkerArray.h>
00052 
00053 #include <object_manipulation_msgs/GraspPlanning.h>
00054 #include <object_manipulation_msgs/ManipulationResult.h>
00055 #include <object_manipulation_msgs/FindClusterBoundingBox.h>
00056 #include <object_manipulator/tools/service_action_wrappers.h>
00057 
00058 #include <dynamic_reconfigure/server.h>
00059 #include "pr2_grasp_adjust/EstimateConfig.h"
00060 //#include "pr2_grasp_adjust/GraspAdjustAction.h"
00061 #include <object_manipulation_msgs/GraspPlanningAction.h>
00062 
00063 #include <actionlib/server/simple_action_server.h>
00064 
00065 #include <iostream>
00066 #include <fstream>
00067 #include <queue>
00068 
00069 #include "gripper_model.h"
00070 #include "helpers.h"
00071 #include "grasp_adjust.h"
00072 
00073 #include "pcl/io/pcd_io.h"
00074 #include "pcl/point_types.h"
00075 #include <pcl/ModelCoefficients.h>
00076 #include <pcl/features/normal_3d.h>
00077 #include <pcl/features/normal_3d_omp.h>
00078 #include <pcl/filters/extract_indices.h>
00079 #include <pcl/filters/passthrough.h>
00080 #include "pcl/filters/voxel_grid.h"
00081 #include <pcl/sample_consensus/method_types.h>
00082 #include <pcl/sample_consensus/model_types.h>
00083 #include <pcl/segmentation/sac_segmentation.h>
00084 
00085 
00086 /*
00087 #define PROF_ENABLED
00088 #include <profiling/profiling.h>
00089 PROF_DECLARE(TOTAL_TIMER)
00090 PROF_DECLARE(TRANSFORM_INPUT_CLOUD)
00091 PROF_DECLARE(DO_DESCENT)
00092 PROF_DECLARE(PROF_1)
00093 */
00094 
00095 typedef pcl::PointXYZRGBNormal PointT;
00096 
00097 const int GLOBAL_SEARCH =  pr2_grasp_adjust::Estimate_global_search;
00098 const int LOCAL_SEARCH =  pr2_grasp_adjust::Estimate_local_search;
00099 const int SINGLE_POSE =  pr2_grasp_adjust::Estimate_single_pose;
00100 
00101 // output file for training data
00102 std::ofstream outputFile;
00103 
00104 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00105 
00106 class GraspAdjustActionServer{
00107 
00108     ros::NodeHandle nh_, nh_pvt_;
00109     ros::Subscriber sub_as_;
00110     ros::Publisher pub_cloud_, pub_cloud_roi_, pub_marker_, pub_marker_array_;
00111     ros::Publisher pub_im_status_;
00112 
00113     std::string input_topic_;
00114     std::string output_topic_;
00115 
00116     tf::TransformListener tfl_;
00117     tf::TransformBroadcaster tfb_;
00118 
00119     sensor_msgs::PointCloud2 cloud_msg_;
00120     geometry_msgs::PoseStamped seed_ps_;
00121 
00122     ros::Time now_;
00123 
00124     GraspAdjust grasp_adjust_;
00125 
00126     //actionlib::SimpleActionServer<pr2_grasp_adjust::GraspAdjustAction> as_;
00127     actionlib::SimpleActionServer<object_manipulation_msgs::GraspPlanningAction> as_;
00128     //pr2_grasp_adjust::GraspAdjustFeedback feedback_;
00129     //pr2_grasp_adjust::GraspAdjustResult result_;
00130 
00131     std::string action_name_;
00132 
00133     // ***** Dynamic reconfigure stuff *****
00134     typedef pr2_grasp_adjust::EstimateConfig Config;
00135     dynamic_reconfigure::Server<Config>                dyn_srv;
00136     dynamic_reconfigure::Server<Config>::CallbackType  dyn_cb;
00137 
00138 //    // Service wrapper for cluster bounding box finder service
00139 //    object_manipulator::ServiceWrapper<object_manipulation_msgs::FindClusterBoundingBox> bounding_box_client_;
00140 
00141 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00142 public:
00146     GraspAdjustActionServer(char* argv[]):
00147             nh_("/"),
00148             nh_pvt_("~"),
00149             output_topic_("/cloud_out"),
00150             as_(nh_, ros::this_node::getName(), boost::bind(&GraspAdjustActionServer::executeCB, this, _1), false),
00151             action_name_(ros::this_node::getName()),
00152             dyn_srv(ros::NodeHandle("~config"))
00153     {
00154         // Clouds
00155         pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(output_topic_, 1);
00156         pub_cloud_roi_ = nh_.advertise<sensor_msgs::PointCloud2>("/roi_snapshot", 1);
00157 
00158         // Marker topics
00159         pub_marker_ = nh_.advertise<visualization_msgs::Marker>("/markers", 50);
00160         pub_marker_array_ = nh_.advertise<visualization_msgs::MarkerArray>("/markers_array", 50);
00161 
00162         // Interactive manipulation front-end status feedback
00163         pub_im_status_ = nh_.advertise<std_msgs::String>("interactive_manipulation_status", 1);
00164 
00165         grasp_adjust_.pub_cloud_ = &pub_cloud_;
00166         grasp_adjust_.pub_cloud_roi_ = &pub_cloud_roi_;
00167         grasp_adjust_.pub_marker_ = &pub_marker_;
00168         grasp_adjust_.pub_marker_array_ = &pub_marker_array_;
00169         grasp_adjust_.listener_ = &tfl_;
00170         grasp_adjust_.broadcaster_ = &tfb_;
00171 
00172         // This server is for the user to adjust the algorithm weights
00173         dyn_cb = boost::bind( &GraspAdjustActionServer::dynamicCallback, this, _1, _2 );
00174         dyn_srv.setCallback(dyn_cb);
00175 
00176         // -------------------------------------------------
00177 
00178         //register the goal and feeback callbacks
00179         //as_.registerGoalCallback(boost::bind(&GraspAdjustActionServer::goalCB, this));
00180         //as_.registerPreemptCallback(boost::bind(&GraspAdjustActionServer::preemptCB, this));
00181 
00182         //subscribe to the data topic of interest
00183         //sub_as_ = nh_.subscribe("/grasp_adjust_action_server", 1, &GraspAdjustActionServer::analysisCB, this);
00184         as_.start();
00185 
00186         ROS_INFO("Grasp adjust action server: Finished constructor!");
00187     }
00188 
00189 private:
00190 
00191     //void executeCB(const pr2_grasp_adjust::GraspAdjustGoalConstPtr &goal)
00192     void executeCB(const object_manipulation_msgs::GraspPlanningGoalConstPtr &goal)
00193     {
00194 
00195       // make sure that the action hasn't been canceled
00196       if (!as_.isActive())
00197         return;
00198 
00199       std_msgs::String status;
00200       status.data = "Running grasp planning action!";
00201       pub_im_status_.publish(status);
00202 
00203       //pr2_grasp_adjust::GraspAdjustResult result;
00204       object_manipulation_msgs::GraspPlanningResult result;
00205 
00206       ROS_INFO("Received action request, cloud with %d points!", goal->target.region.cloud.width * goal->target.region.cloud.height);
00207 
00208       //pub_cloud_.publish(goal->target.region.cloud);
00209 
00210       bool success = false;
00211 
00212       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
00213       pcl::PointCloud<PointT>::Ptr cloudT(new pcl::PointCloud<PointT>());
00214       pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>());
00215       pcl::fromROSMsg(goal->target.region.cloud, *cloud);
00216       cloud->header.stamp = ros::Time(0);
00217       pcl::copyPointCloud(*cloud, *cloudT);
00218       pcl::copyPointCloud(*cloud, *cloud_filtered);
00219       ROS_ASSERT(cloudT->points.size() == cloud_filtered->points.size());
00220 
00221 
00222 
00223 
00224       // ----------------
00225 
00226       // Create the filtering object
00227       pcl::VoxelGrid<PointT > vox;
00228       vox.setInputCloud (cloudT);
00229       vox.setLeafSize (0.003, 0.003, 0.003);
00230       vox.setFilterFieldName("z");
00231       vox.setFilterLimits(-100, 100);
00232       vox.filter (*cloud_filtered);
00233 
00234       ROS_INFO("Down-sampled cloud has %d points!", (int)cloud_filtered->points.size());
00235 
00236 
00237 
00238       // Estimate point normals
00239       pcl::KdTreeFLANN<PointT>::Ptr tree (new pcl::KdTreeFLANN<PointT> ());
00240       pcl::NormalEstimationOMP<PointT, PointT> ne;
00241       ne.setSearchMethod (tree);
00242       ne.setInputCloud (cloud_filtered);
00243       ne.setKSearch (12);
00244       ne.compute (*cloud_filtered);
00245       ROS_INFO("Cloud in frame %s has %d points, stamp %f!",
00246                cloud_filtered->header.frame_id.c_str(),
00247                (int)cloud_filtered->size(),
00248                cloud_filtered->header.stamp.toSec());
00249 
00250       if(pub_cloud_.getNumSubscribers() > 0)
00251       {
00252         sensor_msgs::PointCloud2 cloud_out;
00253         pcl::toROSMsg(*cloud_filtered, cloud_out);
00254         pub_cloud_.publish(cloud_out);
00255       }
00256 
00257 
00258 
00259 //      grasp_adjust_.setInputCloud(cloud);
00260 //      ROS_INFO("Finished preparing cloud!");
00261 
00262       //Generate start poses from the top and sides
00263       std::vector<geometry_msgs::PoseStamped> start_poses;
00264 
00265       // -----------------
00266 
00267       if(goal->grasps_to_evaluate.empty())
00268       {
00269         geometry_msgs::Vector3Stamped seed_normal;
00270         seed_normal.header.frame_id = "/base_link";
00271         seed_normal.header.stamp = cloud->header.stamp;
00272         seed_normal.vector = object_manipulator::msg::createVector3Msg(0,0,-1);
00273         tfl_.transformVector(cloud_filtered->header.frame_id, seed_normal, seed_normal);
00274 
00275         geometry_msgs::Vector3Stamped forward;
00276         forward.header.frame_id = "/base_link";
00277         forward.header.stamp = cloud->header.stamp;
00278         forward.vector = object_manipulator::msg::createVector3Msg(1,0,0);
00279         tfl_.transformVector(cloud_filtered->header.frame_id, forward, forward);
00280 
00281         geometry_msgs::Vector3Stamped right;
00282         right.header.frame_id = "/base_link";
00283         right.header.stamp = cloud->header.stamp;
00284         right.vector = object_manipulator::msg::createVector3Msg(0,-1,0);
00285         tfl_.transformVector(cloud_filtered->header.frame_id, right, right);
00286 
00287         // poses
00288 //        start_poses.push_back(generateAlignedPose(goal->pose_stamped, seed_normal.vector, forward.vector));
00289 //        start_poses.push_back(generateAlignedPose(goal->pose_stamped, seed_normal.vector, right.vector));
00290       }
00291       else
00292       {
00293         for(unsigned int i = 0; i < goal->grasps_to_evaluate.size(); i++)
00294         {
00295           start_poses.push_back(
00296               object_manipulator::msg::createPoseStampedMsg( goal->grasps_to_evaluate[i].grasp_pose,
00297                                                              goal->target.reference_frame_id, ros::Time(0)));
00298         }
00299       }
00300 
00301       grasp_adjust_.setInputCloud(*cloud_filtered);
00302 
00303       std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> ranked_poses;
00304 
00305       //Adjust the start poses to find good grasps
00306       for(size_t start_pose_ind=0; start_pose_ind < start_poses.size(); start_pose_ind++)
00307       {
00308         int mode = GLOBAL_SEARCH;
00309         int value = grasp_adjust_.findGrasps(start_poses[start_pose_ind], &ranked_poses, mode,  goal->target.reference_frame_id);
00310         success = success || (value == object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS);
00311       }
00312 
00313       if (as_.isPreemptRequested() || !ros::ok())
00314       {
00315         ROS_INFO("%s: Preempted", action_name_.c_str());
00316         // set the action state to preempted
00317         as_.setPreempted();
00318         success = false;
00319       }
00320       if(success)
00321       {
00322         result.error_code.value = object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS;
00323       }
00324 
00325       ROS_INFO("Globally, found %d valid poses!", (int)ranked_poses.size());
00326       while(success && !ranked_poses.empty())
00327       {
00328         GripperModel model = ranked_poses.top();
00329         ranked_poses.pop();
00330         ROS_INFO("Pose with score %.3f", model.score_);
00331         geometry_msgs::PoseStamped ps;
00332         model.toPoseStamped(ps);
00333         model.publishModel(pub_marker_array_, "grasp_adjust_action", 0, ros::Duration(), model.score_*model.score_);
00334 
00335         object_manipulation_msgs::Grasp grasp;
00336 
00337         grasp.grasp_pose = ps.pose;
00338         grasp.min_approach_distance = 0.05;
00339         grasp.desired_approach_distance = 0.07;
00340         grasp.success_probability = model.score_;
00341 
00342         result.grasps.push_back(grasp);
00343 
00344       }
00345 
00346 
00347 
00348       if(success)
00349       {
00350         ROS_INFO("%s: Succeeded", action_name_.c_str());
00351         status.data = "Found a grasp!";
00352         pub_im_status_.publish(status);
00353         as_.setSucceeded(result);
00354       }
00355       else
00356       {
00357         ROS_INFO("%s: Failed", action_name_.c_str());
00358         status.data = "Didn't find a grasp; keeping previous pose.";
00359         pub_im_status_.publish(status);
00360         as_.setAborted();
00361       }
00362     }
00363 
00369     bool graspAdjustCallback(   pr2_grasp_adjust::GraspAdjust::Request  &req,
00370                                 pr2_grasp_adjust::GraspAdjust::Response &res )
00371     {
00372 
00373         int mode = req.search_mode;
00374         res.result.value = grasp_adjust_.findGrasps(req.grasp_pose, &(res.grasp_poses), &(res.pose_scores), mode);
00375 
00376         ROS_INFO("pr2_grasp_adjust::grasp_adjust returned %d", res.result.value);
00377 
00378         return true;
00379     }
00380 
00386     void setupGraspPlanning(object_manipulation_msgs::GraspPlanning::Request &req, pcl::PointCloud<PointT> &cloud)
00387     {
00388       pcl::PointCloud<pcl::PointXYZ> cloud_in;
00389       //pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
00390       sensor_msgs::PointCloud2 cloud_msg;
00391 
00392       sensor_msgs::convertPointCloudToPointCloud2(req.target.cluster, cloud_msg);
00393       ROS_INFO("Cloud_in in frame %s, reference frame %s!", cloud_msg.header.frame_id.c_str(), req.target.reference_frame_id.c_str());
00394       //for(int i = 0; i < cloud_msg.fields.size(); i++) ROS_INFO_STREAM("Cloud msg has field " << cloud_msg.fields[i]);
00395       pcl::fromROSMsg(cloud_msg, cloud_in);
00396 
00397       prepareCloud(cloud_in, cloud);
00398   }
00399 
00400   void prepareCloud(pcl::PointCloud<pcl::PointXYZ> cloud_in, pcl::PointCloud<PointT> &cloud)
00401   {
00402 
00403     // Estimate point normals
00404     pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
00405     pcl::NormalEstimation<pcl::PointXYZ, PointT> ne;
00406     ne.setSearchMethod (tree);
00407     ne.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00408     ne.setKSearch (15);
00409     ne.compute (cloud);
00410     for(unsigned int i = 0; i < cloud_in.size(); i++){
00411       cloud.points[i].x = cloud_in.points[i].x;
00412       cloud.points[i].y = cloud_in.points[i].y;
00413       cloud.points[i].z = cloud_in.points[i].z;
00414     }
00415     ROS_INFO("Cloud_out in frame %s has %d points, stamp %f!", cloud.header.frame_id.c_str(), (int)cloud.size(), cloud.header.stamp.toSec());
00416 
00417     if(pub_cloud_.getNumSubscribers() > 0)
00418     {
00419       sensor_msgs::PointCloud2 cloud_out;
00420       pcl::toROSMsg(cloud, cloud_out);
00421       pub_cloud_.publish(cloud_out);
00422     }
00423 
00424     grasp_adjust_.setInputCloud(cloud);
00425     ROS_INFO("Finished preparing cloud!");
00426   }
00427 
00428 
00429 
00433   void dynamicCallback(Config &new_config, uint32_t id)
00434     {
00435         // This info will get printed back to reconfigure gui
00436         char status[256] = "\0";
00437 
00438         switch(id)
00439         {
00440         case(-1): //Init // If you are tempted to put anything here, it should probably go in the constructor
00441           break;
00442         case(0): // Connect
00443           printf("Reconfigure GUI connected to me!\n");
00444           new_config = grasp_adjust_.config_;
00445           break;
00446         default:
00447           ROS_INFO("Dynamic reconfigure did something, variable id %d.", id);
00448         }
00449         grasp_adjust_.config_ = new_config;
00450         new_config.status = status;
00451     }
00452 };
00453 
00454 
00455 
00456 /* ---[ */
00457 int main (int argc, char* argv[])
00458 {
00459   ros::init(argc, argv, "grasp_adjust_action_server_node");
00460   GraspAdjustActionServer server(argv);
00461   ros::spin();
00462   return (0);
00463 }
00464 /* ]--- */
grasp_adjust_action_server.cpp
