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

class GraspAdjustActionServer{

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

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

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

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

    ros::Time now_;

    GraspAdjust grasp_adjust_;

    actionlib::SimpleActionServer<pr2_grasp_adjust::GraspAdjustAction> 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_;

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", 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_;

        // 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)
    {
      // make sure that the action hasn't been canceled
      if (!as_.isActive())
        return;

      pr2_grasp_adjust::GraspAdjustResult result;

      ROS_INFO("Received action request!");
      bool success = false;

      pcl::PointCloud<pcl::PointXYZ> cloudXYZ;
      pcl::fromROSMsg(goal->cloud, cloudXYZ);

      pcl::PointCloud<PointT>::Ptr cloudT(new pcl::PointCloud<PointT>());
      prepareCloud(cloudXYZ, *cloudT);

      PointT seed_point = cloudT->points[goal->seed_index];

      tf::Vector3 seed_normal(seed_point.normal[0], seed_point.normal[1], seed_point.normal[2]);
      seed_normal *= -1.0;

      //Generate start poses from the top and sides
      std::vector<geometry_msgs::PoseStamped> start_poses;
      double start_dist = .01;

      // poses
      start_poses.push_back(generateAlignedPose(goal->pose_stamped, seed_normal, tf::Vector3(1, 0, 0)));
      start_poses.push_back(generateAlignedPose(goal->pose_stamped, seed_normal, tf::Vector3(0, 1, 0)));

      pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>());
      // Create the filtering object
      pcl::VoxelGrid<PointT > sor;
      sor.setInputCloud (cloudT);
      sor.setLeafSize (0.003, 0.003, 0.003);
      sor.filter (*cloud_filtered);
      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, cloudT->header.frame_id);
        success = success || (value == object_manipulation_msgs::ManipulationResult::SUCCESS);
      }

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

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

        result.grasp_poses.push_back(ps);
        result.gripper_openings.push_back(1);
        result.grasp_scores.push_back(model.score_);

      }

      if(success)
      {
        ROS_INFO("%s: Succeeded", action_name_.c_str());
        as_.setSucceeded(result);
      }
      else
      {
        ROS_INFO("%s: Failed", action_name_.c_str());
        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::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<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);
}
/* ]--- */

---

pr2_grasp_adjust
Author(s): Adam Leeper
autogenerated on Fri Jan 11 09:53:53 2013