grasp_adjust_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 "pcl/io/pcd_io.h"
#include "pcl/point_types.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/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/DebugConfig.h"
#include "pr2_grasp_adjust/GraspAdjust.h"


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

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

#include <pcl/ModelCoefficients.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.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;

class GraspAdjustServer{

    pcl::PointCloud<PointT> last_cloud_;

    ros::NodeHandle nh_, nh_pvt_;
    ros::Subscriber sub_cloud_;
    ros::Publisher pub_cloud_, pub_marker_, pub_marker_array_;
    ros::Publisher pub_cloud_roi_, pub_cloud_debug_2;

    ros::ServiceServer adjust_srv_, evaluate_srv_, plan_srv_;
    ros::Timer update_timer_;

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

    tf::TransformListener listener_;
    tf::TransformBroadcaster broadcaster_;

    ros::Time now_;

    bool got_first_cloud_;

    GraspAdjust grasp_adjust_;

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

    typedef pr2_grasp_adjust::DebugConfig Debug;
    dynamic_reconfigure::Server<Debug>                debug_srv;
    dynamic_reconfigure::Server<Debug>::CallbackType  debug_cb;

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

    // ***** Helpers for training ******
    bool training_;
    std::ofstream train_file_;

public:
    GraspAdjustServer(char* argv[]):
            nh_("/"),
            nh_pvt_("~"),
            input_topic_("/cloud_in"),
            output_topic_("/cloud_out"),
            dyn_srv(ros::NodeHandle("~config")),
            debug_srv(ros::NodeHandle("~debug")),
                        bounding_box_client_("/find_cluster_bounding_box"),
            training_(0)
    {
        got_first_cloud_ = false;
        now_ = ros::Time::now() - ros::Duration(1.0);

        // Clouds in and out
        sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(input_topic_, 1, boost::bind(&GraspAdjustServer::cloud_callback, this, _1));
        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", 20);
        pub_marker_array_ = nh_.advertise<visualization_msgs::MarkerArray>("/markers_array", 20);

        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_ = &listener_;
        grasp_adjust_.broadcaster_ = &broadcaster_;

        // Create callback for auto-running of algorithm
        update_timer_ = nh_.createTimer(ros::Duration(0.5), boost::bind(&GraspAdjustServer::timerCallback, this));

        bool load_debug_server = false;
        nh_pvt_.param("load_debug_server", load_debug_server, load_debug_server);

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

        // This server is used for debugging the geometry and algorithm code.
        if(load_debug_server){
            ROS_INFO("Loading the debug dynamic reconfigure server");
            debug_cb = boost::bind( &GraspAdjustServer::debugCallback, this, _1, _2 );
            debug_srv.setCallback(debug_cb);
        }

        adjust_srv_  = nh_.advertiseService("pr2_grasp_adjust",   &GraspAdjustServer::graspAdjustCallback, this);
        evaluate_srv_ = nh_.advertiseService("pr2_gripper_grasp_evaluate", &GraspAdjustServer::graspEvaluateCallback, this);
        plan_srv_ = nh_.advertiseService("pr2_gripper_grasp_plan", &GraspAdjustServer::graspPlanCallback, this);

        // if training, open the output file
        if(training_){
            ROS_INFO("in training mode, opening training_output.txt");
            train_file_.open("/u/hsiao/gripper_stereo_data/training_data/training_output.txt");
        }

        ROS_INFO("Finished constructor!");
    }


private:

    void timerCallback()
    {
        now_= last_cloud_.header.stamp;
        geometry_msgs::PoseStamped pose_msg;
        tf::Pose pose;

        if(got_first_cloud_ && grasp_adjust_.debug_.run_continuous)
        {
            if(grasp_adjust_.debug_.custom_pose)
            {
                if(grasp_adjust_.debug_.custom_frame.empty())
                    pose_msg.header.frame_id = "torso_lift_link";
                else
                    pose_msg.header.frame_id = grasp_adjust_.debug_.custom_frame;
                pose_msg.header.stamp = now_;

                float x = grasp_adjust_.debug_.custom_x;
                float y = grasp_adjust_.debug_.custom_y;
                float z = grasp_adjust_.debug_.custom_z;
                float roll = grasp_adjust_.debug_.custom_roll     * M_PI/180.0;
                float pitch = grasp_adjust_.debug_.custom_pitch   * M_PI/180.0;
                float yaw = grasp_adjust_.debug_.custom_yaw       * M_PI/180.0;

                tf::Transform T_rot;
                tf::Transform T_offset;
                tf::Transform T_trans = tf::Transform(tf::Quaternion(0,0,0,1),
                                                      tf::Vector3(x, y, z));
                T_rot = tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
                                      tf::Vector3( 0, 0, 0) );
                T_offset = T_trans * grasp_adjust_.DEFAULT_GRIPPER.tool_frame_ * T_rot * grasp_adjust_.DEFAULT_GRIPPER.tool_frame_.inverse();
                pose = T_offset;
            }
            else // Set a pose 6cm in front of current gripper pose
            {
                pose_msg.header.frame_id = "r_wrist_roll_link";
                pose_msg.header.stamp = now_;;
                pose = tf::Pose(tf::Quaternion(0,0,0,1), tf::Vector3(0.06, 0, 0));
            }

            tf::poseTFToMsg(pose, pose_msg.pose);
            std::vector<geometry_msgs::PoseStamped> adjusted_poses;
            std::vector<double> pose_scores;

            int value = grasp_adjust_.findGrasps(pose_msg, &adjusted_poses, &pose_scores, grasp_adjust_.config_.search_mode);
            ROS_INFO("pr2_grasp_adjust::grasp_adjust returned %d", value);
        }
    }

    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);


      // 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());

      sensor_msgs::PointCloud2 cloud_out;
      pcl::toROSMsg(cloud, cloud_out);
      pub_cloud_.publish(cloud_out);

          grasp_adjust_.setInputCloud(cloud);
        }


    geometry_msgs::PoseStamped generateRelativePose(geometry_msgs::PoseStamped original_pose, double roll, double pitch, 
                                                                                                 double yaw, double xdist, double ydist, double zdist)
    {
      tf::Pose pose_tf;
      tf::poseMsgToTF(original_pose.pose, pose_tf);
      tf::Transform T_rot = tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
                                        tf::Vector3( xdist, ydist, zdist) );
      tf::Transform output_rot = pose_tf * T_rot;
      geometry_msgs::PoseStamped output_pose;
      output_pose.header.frame_id = original_pose.header.frame_id;
      tf::poseTFToMsg(output_rot, output_pose.pose);
      return output_pose;
    }

    bool graspPlanCallback(   object_manipulation_msgs::GraspPlanning::Request  &req,
                              object_manipulation_msgs::GraspPlanning::Response &res )
    {  
      //clear old stuff
      unsigned int MAX_GRASPS = 20; // ??
      GripperModel clear_model;
      clear_model.setStamp(ros::Time::now());
      clear_model.setFrameId(req.target.reference_frame_id);
      if(pub_marker_array_.getNumSubscribers() > 0)
        for(unsigned int i = 0; i < MAX_GRASPS; i++)
        {
          clear_model.publishModel(pub_marker_array_, "grasp_planning", i, ros::Duration(), 1, visualization_msgs::Marker::DELETE);
        }


      ROS_INFO("starting graspPlanCallback");
      pcl::PointCloud<PointT> cloud;
      setupGraspPlanning(req, cloud);

      //get the names of the hand joints
      std::string arm_name = "right_arm";
      if(req.arm_name.length() != 0) arm_name = req.arm_name;
      else ROS_ERROR("point cluster grasp planner: missing arm_name in request!  Using right_arm");
      std::vector<std::string> hand_joints;

      XmlRpc::XmlRpcValue my_list;
      nh_.getParam(std::string("/hand_description/")+arm_name+std::string("/hand_joints"), my_list);
      ROS_ASSERT(my_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
      for (int32_t i = 0; i < my_list.size(); ++i)
      {
      hand_joints.push_back(static_cast<std::string>(my_list[i]));
      }

      //Compute principal directions and object bounding box
      object_manipulation_msgs::FindClusterBoundingBox bounding_box_srv;
      ros::ServiceClient client = bounding_box_client_.client(ros::Duration(5.0));
      bounding_box_srv.request.cluster = req.target.cluster;
      bool result = client.call(bounding_box_srv);
      if(!result)
      {
        ROS_ERROR("Failed to call find_cluster_bounding_box service!");
        res.error_code.value = object_manipulation_msgs::GraspPlanningErrorCode::OTHER_ERROR;
        return true;
      }

      //Generate start poses from the top and sides
      std::vector<geometry_msgs::PoseStamped> start_poses;
      double start_dist = .01;
      //right
      start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,0.,0.,
                           -(bounding_box_srv.response.box_dims.x/2+.185+start_dist),0.,0.));
//      //back ???
//      start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,0.,M_PI/2,
//                           0.,-(bounding_box_srv.response.box_dims.y/2+.185+start_dist),0.));
      //front
      start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,0.,-M_PI/2,
                           0.,bounding_box_srv.response.box_dims.y/2+.185+start_dist,0));
      //left
      start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,0.,M_PI,
                           bounding_box_srv.response.box_dims.x/2+.185+start_dist,0.,0.));
      //top
      start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,M_PI/2,0.,
                           0.,0.,bounding_box_srv.response.box_dims.z/2+.185+start_dist));
      //top rotated
      start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, M_PI/2,M_PI/2.0,0.,
                           0.,0.,bounding_box_srv.response.box_dims.z/2+.185+start_dist));


      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;
        std::vector<geometry_msgs::PoseStamped> adjusted_poses;
        std::vector<double> pose_scores;
        //int value = grasp_adjust_.findGrasps(start_poses[start_pose_ind], &adjusted_poses, & pose_scores, mode, req.target.reference_frame_id);
        int value = grasp_adjust_.findGrasps(start_poses[start_pose_ind], &ranked_poses, mode, req.target.reference_frame_id);
      }

      ROS_INFO("Globally, found %d valid poses!", (int)ranked_poses.size());
      while(!ranked_poses.empty() && res.grasps.size() < MAX_GRASPS)
      {
        GripperModel model = ranked_poses.top();
        ranked_poses.pop();

        geometry_msgs::PoseStamped ps;
        model.toPoseStamped(ps);

        //Create the Grasp message
        object_manipulation_msgs::Grasp new_grasp;
        for(size_t i=0; i<hand_joints.size(); i++)
        {
          new_grasp.pre_grasp_posture.name.push_back(hand_joints[i]);
          new_grasp.pre_grasp_posture.position.push_back(0.5);
          new_grasp.grasp_posture.effort.push_back(100);
          new_grasp.grasp_posture.name.push_back(hand_joints[i]);
          new_grasp.grasp_posture.position.push_back(0);
          new_grasp.grasp_posture.effort.push_back(50);
        }
        new_grasp.grasp_pose = ps.pose;
        new_grasp.desired_approach_distance = 0.10;
        new_grasp.min_approach_distance = 0.05;
        new_grasp.success_probability = model.score_;

        //Add it to the response
        res.grasps.push_back(new_grasp);

        ROS_INFO("Added pose % 2d with score %.3f", (int)res.grasps.size(), model.score_);
        model.publishModel(pub_marker_array_, "grasp_planning", res.grasps.size(), ros::Duration(), model.score_*model.score_);
        //sleep(0.3);
      }


      res.error_code.value = object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS;
        ROS_INFO("pr2_grasp_adjust::grasp_evaluate returned %d", res.error_code.value);
        return true;
    }


    bool graspEvaluateCallback(   object_manipulation_msgs::GraspPlanning::Request  &req,
                                  object_manipulation_msgs::GraspPlanning::Response &res )
    {      

      pcl::PointCloud<PointT> cloud;
          setupGraspPlanning(req, cloud);

      // Compute cloud centroid...
      tf::Vector3 centroid = tf::Vector3(0,0,0);
      for(unsigned int i = 0; i < cloud.points.size(); i++ )
      {
          PointT point = cloud.points[i];
          centroid += tf::Vector3(point.x, point.y, point.z);
      }
      centroid /= cloud.points.size() + 0.001;

      ROS_INFO("There are %d grasps to evaluate", (int)req.grasps_to_evaluate.size());
      ROS_INFO("object  grasp   centroid   symmetry     points  pnt_bonus    normals  collision");
      bool CONTINUE = false;

      for(unsigned j = 0; j < req.grasps_to_evaluate.size(); j ++)
      {

        this->broadcaster_.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0,0,0,1), centroid),
                                                              cloud.header.stamp, cloud.header.frame_id.c_str(), "/centroid"));

        geometry_msgs::Pose pose_msg = req.grasps_to_evaluate[j].grasp_pose;
        tf::Pose pose_tf;
        tf::poseMsgToTF(pose_msg, pose_tf);

        FeatureWeights features;
        GripperModel gripper;
        gripper.setHeader(cloud.header);
        gripper.transform(pose_tf, cloud.header.frame_id.c_str());

        gripper.publishModel(pub_marker_array_, "gripper_model", 0, ros::Duration(20.0), 2.0);
        gripper.evaluateGrasp(cloud, features, centroid, false, true);

        char feature_str[200];

        if(gripper.state_.no_object) {
          sprintf(feature_str, "% 6d % 6d   No points in contact with gripper!", req.target.potential_models[0].model_id, j);
        }
        else {
          sprintf(feature_str, "% 6d % 6d % 10.5f % 10.5f % 10.5f % 10.5f % 10.5f % 10.5f",
              req.target.potential_models[0].model_id,  j,
              features.centroid, features.symmetry, features.point,
              features.Iyz, features.normal, features.collision
              );
        }

        outputFile << feature_str << std::endl;
        ROS_INFO_STREAM(feature_str);

        if(!CONTINUE && grasp_adjust_.config_.debug_stepping) {
          char key[256]; std::cin.getline(key, 256);
          if(key[0] == 'c') CONTINUE = true;
        }

        usleep(grasp_adjust_.config_.pause_time*1000000);
      }

      res.error_code.value = object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS;
      ROS_INFO("pr2_grasp_adjust::grasp_evaluate returned %d", res.error_code.value);
      return true;
    }


    void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &input)
    {
      ROS_DEBUG("Got a cloud with %d points!", input->width*input->height);

      if( !strcmp(input->fields[4].name.c_str(), "normal_x") )
      {
        pcl::fromROSMsg(*input, last_cloud_);
        grasp_adjust_.setInputCloud(last_cloud_);

        if(!got_first_cloud_){
          got_first_cloud_ = true;
          ROS_INFO("Got first 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;
          case(91):
            if(new_config.write_file)
            {
              ROS_INFO("In training mode, opening %s", new_config.filename.c_str());
              outputFile.open(new_config.filename.c_str());
              outputFile << "object  grasp   centroid   symmetry     points  pnt_bonus    normals  collision" << std::endl;
            }
            else
            {
              ROS_INFO("Closing file %s", new_config.filename.c_str());
              outputFile.close();

            }
            break;
          default:
            ROS_INFO("Dynamic reconfigure did something, variable %d.", id);
          }
          grasp_adjust_.config_ = new_config;
          new_config.status = status;
      }


    void debugCallback(Debug &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_.debug_;
                break;
              default:
                ROS_INFO("Dynamic reconfigure did something, variable %d.", id);
          }
          grasp_adjust_.debug_ = new_config;
          new_config.status = status;
      }
};



/* ---[ */
int main (int argc, char* argv[])
{
  ros::init(argc, argv, "filter");

  GraspAdjustServer server(argv);

  ros::spin();

  return (0);
}
/* ]--- */