grasp_adjust_action_server.cpp

Go to the documentation of this file.

/*
 * grasp_adjust_server
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Software License Agreement (BSD License)
 *
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Willow Garage nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */



#include <ros/ros.h>

#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include <boost/bind.hpp>


#include <std_msgs/String.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <object_manipulation_msgs/GraspPlanning.h>
#include <object_manipulation_msgs/ManipulationResult.h>
#include <object_manipulation_msgs/FindClusterBoundingBox.h>
#include <object_manipulator/tools/service_action_wrappers.h>

#include <dynamic_reconfigure/server.h>
#include "pr2_grasp_adjust/EstimateConfig.h"
//#include "pr2_grasp_adjust/GraspAdjustAction.h"
#include <object_manipulation_msgs/GraspPlanningAction.h>

#include <actionlib/server/simple_action_server.h>

#include <iostream>
#include <fstream>
#include <queue>

#include "gripper_model.h"
#include "helpers.h"
#include "grasp_adjust.h"

#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include <pcl/ModelCoefficients.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include "pcl/filters/voxel_grid.h"
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>


/*
#define PROF_ENABLED
#include <profiling/profiling.h>
PROF_DECLARE(TOTAL_TIMER)
PROF_DECLARE(TRANSFORM_INPUT_CLOUD)
PROF_DECLARE(DO_DESCENT)
PROF_DECLARE(PROF_1)
*/

typedef pcl::PointXYZRGBNormal PointT;

const int GLOBAL_SEARCH =  pr2_grasp_adjust::Estimate_global_search;
const int LOCAL_SEARCH =  pr2_grasp_adjust::Estimate_local_search;
const int SINGLE_POSE =  pr2_grasp_adjust::Estimate_single_pose;

// output file for training data
std::ofstream outputFile;

// **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120

class GraspAdjustActionServer{

    ros::NodeHandle nh_, nh_pvt_;
    ros::Subscriber sub_as_;
    ros::Publisher pub_cloud_, pub_cloud_roi_, pub_marker_, pub_marker_array_;
    ros::Publisher pub_im_status_;

    std::string input_topic_;
    std::string output_topic_;

    tf::TransformListener tfl_;
    tf::TransformBroadcaster tfb_;

    sensor_msgs::PointCloud2 cloud_msg_;
    geometry_msgs::PoseStamped seed_ps_;

    ros::Time now_;

    GraspAdjust grasp_adjust_;

    //actionlib::SimpleActionServer<pr2_grasp_adjust::GraspAdjustAction> as_;
    actionlib::SimpleActionServer<object_manipulation_msgs::GraspPlanningAction> as_;
    //pr2_grasp_adjust::GraspAdjustFeedback feedback_;
    //pr2_grasp_adjust::GraspAdjustResult result_;

    std::string action_name_;

    // ***** Dynamic reconfigure stuff *****
    typedef pr2_grasp_adjust::EstimateConfig Config;
    dynamic_reconfigure::Server<Config>                dyn_srv;
    dynamic_reconfigure::Server<Config>::CallbackType  dyn_cb;

//    // Service wrapper for cluster bounding box finder service
//    object_manipulator::ServiceWrapper<object_manipulation_msgs::FindClusterBoundingBox> bounding_box_client_;

// **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
public:
    GraspAdjustActionServer(char* argv[]):
            nh_("/"),
            nh_pvt_("~"),
            output_topic_("/cloud_out"),
            as_(nh_, ros::this_node::getName(), boost::bind(&GraspAdjustActionServer::executeCB, this, _1), false),
            action_name_(ros::this_node::getName()),
            dyn_srv(ros::NodeHandle("~config"))
    {
        // Clouds
        pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(output_topic_, 1);
        pub_cloud_roi_ = nh_.advertise<sensor_msgs::PointCloud2>("/roi_snapshot", 1);

        // Marker topics
        pub_marker_ = nh_.advertise<visualization_msgs::Marker>("/markers", 50);
        pub_marker_array_ = nh_.advertise<visualization_msgs::MarkerArray>("/markers_array", 50);

        // Interactive manipulation front-end status feedback
        pub_im_status_ = nh_.advertise<std_msgs::String>("interactive_manipulation_status", 1);

        grasp_adjust_.pub_cloud_ = &pub_cloud_;
        grasp_adjust_.pub_cloud_roi_ = &pub_cloud_roi_;
        grasp_adjust_.pub_marker_ = &pub_marker_;
        grasp_adjust_.pub_marker_array_ = &pub_marker_array_;
        grasp_adjust_.listener_ = &tfl_;
        grasp_adjust_.broadcaster_ = &tfb_;

        // This server is for the user to adjust the algorithm weights
        dyn_cb = boost::bind( &GraspAdjustActionServer::dynamicCallback, this, _1, _2 );
        dyn_srv.setCallback(dyn_cb);

        // -------------------------------------------------

        //register the goal and feeback callbacks
        //as_.registerGoalCallback(boost::bind(&GraspAdjustActionServer::goalCB, this));
        //as_.registerPreemptCallback(boost::bind(&GraspAdjustActionServer::preemptCB, this));

        //subscribe to the data topic of interest
        //sub_as_ = nh_.subscribe("/grasp_adjust_action_server", 1, &GraspAdjustActionServer::analysisCB, this);
        as_.start();

        ROS_INFO("Grasp adjust action server: Finished constructor!");
    }

private:

    //void executeCB(const pr2_grasp_adjust::GraspAdjustGoalConstPtr &goal)
    void executeCB(const object_manipulation_msgs::GraspPlanningGoalConstPtr &goal)
    {

      // make sure that the action hasn't been canceled
      if (!as_.isActive())
        return;

      std_msgs::String status;
      status.data = "Running grasp planning action!";
      pub_im_status_.publish(status);

      //pr2_grasp_adjust::GraspAdjustResult result;
      object_manipulation_msgs::GraspPlanningResult result;

      ROS_INFO("Received action request, cloud with %d points!", goal->target.region.cloud.width * goal->target.region.cloud.height);

      //pub_cloud_.publish(goal->target.region.cloud);

      bool success = false;

      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
      pcl::PointCloud<PointT>::Ptr cloudT(new pcl::PointCloud<PointT>());
      pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>());
      pcl::fromROSMsg(goal->target.region.cloud, *cloud);
      cloud->header.stamp = ros::Time(0);
      pcl::copyPointCloud(*cloud, *cloudT);
      pcl::copyPointCloud(*cloud, *cloud_filtered);
      ROS_ASSERT(cloudT->points.size() == cloud_filtered->points.size());




      // ----------------

      // Create the filtering object
      pcl::VoxelGrid<PointT > vox;
      vox.setInputCloud (cloudT);
      vox.setLeafSize (0.003, 0.003, 0.003);
      vox.setFilterFieldName("z");
      vox.setFilterLimits(-100, 100);
      vox.filter (*cloud_filtered);

      ROS_INFO("Down-sampled cloud has %d points!", (int)cloud_filtered->points.size());



      // Estimate point normals
      pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
      pcl::NormalEstimationOMP<PointT, PointT> ne;
      ne.setSearchMethod (tree);
      ne.setInputCloud (cloud_filtered);
      ne.setKSearch (12);
      ne.compute (*cloud_filtered);
      ROS_INFO("Cloud in frame %s has %d points, stamp %f!",
               cloud_filtered->header.frame_id.c_str(),
               (int)cloud_filtered->size(),
               cloud_filtered->header.stamp.toSec());

      if(pub_cloud_.getNumSubscribers() > 0)
      {
        sensor_msgs::PointCloud2 cloud_out;
        pcl::toROSMsg(*cloud_filtered, cloud_out);
        pub_cloud_.publish(cloud_out);
      }



//      grasp_adjust_.setInputCloud(cloud);
//      ROS_INFO("Finished preparing cloud!");

      //Generate start poses from the top and sides
      std::vector<geometry_msgs::PoseStamped> start_poses;

      // -----------------

      if(goal->grasps_to_evaluate.empty())
      {
        geometry_msgs::Vector3Stamped seed_normal;
        seed_normal.header.frame_id = "/base_link";
        seed_normal.header.stamp = cloud->header.stamp;
        seed_normal.vector = object_manipulator::msg::createVector3Msg(0,0,-1);
        tfl_.transformVector(cloud_filtered->header.frame_id, seed_normal, seed_normal);

        geometry_msgs::Vector3Stamped forward;
        forward.header.frame_id = "/base_link";
        forward.header.stamp = cloud->header.stamp;
        forward.vector = object_manipulator::msg::createVector3Msg(1,0,0);
        tfl_.transformVector(cloud_filtered->header.frame_id, forward, forward);

        geometry_msgs::Vector3Stamped right;
        right.header.frame_id = "/base_link";
        right.header.stamp = cloud->header.stamp;
        right.vector = object_manipulator::msg::createVector3Msg(0,-1,0);
        tfl_.transformVector(cloud_filtered->header.frame_id, right, right);

        // poses
//        start_poses.push_back(generateAlignedPose(goal->pose_stamped, seed_normal.vector, forward.vector));
//        start_poses.push_back(generateAlignedPose(goal->pose_stamped, seed_normal.vector, right.vector));
      }
      else
      {
        for(unsigned int i = 0; i < goal->grasps_to_evaluate.size(); i++)
        {
          start_poses.push_back(
              object_manipulator::msg::createPoseStampedMsg( goal->grasps_to_evaluate[i].grasp_pose,
                                                             goal->target.reference_frame_id, ros::Time(0)));
        }
      }

      grasp_adjust_.setInputCloud(*cloud_filtered);

      std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> ranked_poses;

      //Adjust the start poses to find good grasps
      for(size_t start_pose_ind=0; start_pose_ind < start_poses.size(); start_pose_ind++)
      {
        int mode = GLOBAL_SEARCH;
        int value = grasp_adjust_.findGrasps(start_poses[start_pose_ind], &ranked_poses, mode,  goal->target.reference_frame_id);
        success = success || (value == object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS);
      }

      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_.setPreempted();
        success = false;
      }
      if(success)
      {
        result.error_code.value = object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS;
      }

      ROS_INFO("Globally, found %d valid poses!", (int)ranked_poses.size());
      while(success && !ranked_poses.empty())
      {
        GripperModel model = ranked_poses.top();
        ranked_poses.pop();
        ROS_INFO("Pose with score %.3f", model.score_);
        geometry_msgs::PoseStamped ps;
        model.toPoseStamped(ps);
        model.publishModel(pub_marker_array_, "grasp_adjust_action", 0, ros::Duration(), model.score_*model.score_);

        object_manipulation_msgs::Grasp grasp;

        grasp.grasp_pose = ps.pose;
        grasp.min_approach_distance = 0.05;
        grasp.desired_approach_distance = 0.07;
        grasp.success_probability = model.score_;

        result.grasps.push_back(grasp);

      }



      if(success)
      {
        ROS_INFO("%s: Succeeded", action_name_.c_str());
        status.data = "Found a grasp!";
        pub_im_status_.publish(status);
        as_.setSucceeded(result);
      }
      else
      {
        ROS_INFO("%s: Failed", action_name_.c_str());
        status.data = "Didn't find a grasp; keeping previous pose.";
        pub_im_status_.publish(status);
        as_.setAborted();
      }
    }

    bool graspAdjustCallback(   pr2_grasp_adjust::GraspAdjust::Request  &req,
                                pr2_grasp_adjust::GraspAdjust::Response &res )
    {

        int mode = req.search_mode;
        res.result.value = grasp_adjust_.findGrasps(req.grasp_pose, &(res.grasp_poses), &(res.pose_scores), mode);

        ROS_INFO("pr2_grasp_adjust::grasp_adjust returned %d", res.result.value);

        return true;
    }

    void setupGraspPlanning(object_manipulation_msgs::GraspPlanning::Request &req, pcl::PointCloud<PointT> &cloud)
    {
      pcl::PointCloud<pcl::PointXYZ> cloud_in;
      //pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
      sensor_msgs::PointCloud2 cloud_msg;

      sensor_msgs::convertPointCloudToPointCloud2(req.target.cluster, cloud_msg);
      ROS_INFO("Cloud_in in frame %s, reference frame %s!", cloud_msg.header.frame_id.c_str(), req.target.reference_frame_id.c_str());
      //for(int i = 0; i < cloud_msg.fields.size(); i++) ROS_INFO_STREAM("Cloud msg has field " << cloud_msg.fields[i]);
      pcl::fromROSMsg(cloud_msg, cloud_in);

      prepareCloud(cloud_in, cloud);
  }

  void prepareCloud(pcl::PointCloud<pcl::PointXYZ> cloud_in, pcl::PointCloud<PointT> &cloud)
  {

    // Estimate point normals
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    pcl::NormalEstimation<pcl::PointXYZ, PointT> ne;
    ne.setSearchMethod (tree);
    ne.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
    ne.setKSearch (15);
    ne.compute (cloud);
    for(unsigned int i = 0; i < cloud_in.size(); i++){
      cloud.points[i].x = cloud_in.points[i].x;
      cloud.points[i].y = cloud_in.points[i].y;
      cloud.points[i].z = cloud_in.points[i].z;
    }
    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());

    if(pub_cloud_.getNumSubscribers() > 0)
    {
      sensor_msgs::PointCloud2 cloud_out;
      pcl::toROSMsg(cloud, cloud_out);
      pub_cloud_.publish(cloud_out);
    }

    grasp_adjust_.setInputCloud(cloud);
    ROS_INFO("Finished preparing cloud!");
  }



  void dynamicCallback(Config &new_config, uint32_t id)
    {
        // This info will get printed back to reconfigure gui
        char status[256] = "\0";

        switch(id)
        {
        case(-1): //Init // If you are tempted to put anything here, it should probably go in the constructor
          break;
        case(0): // Connect
          printf("Reconfigure GUI connected to me!\n");
          new_config = grasp_adjust_.config_;
          break;
        default:
          ROS_INFO("Dynamic reconfigure did something, variable id %d.", id);
        }
        grasp_adjust_.config_ = new_config;
        new_config.status = status;
    }
};



/* ---[ */
int main (int argc, char* argv[])
{
  ros::init(argc, argv, "grasp_adjust_action_server_node");
  GraspAdjustActionServer server(argv);
  ros::spin();
  return (0);
}
/* ]--- */