grasp_adjust_server.cpp
Go to the documentation of this file.
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 #include "pcl/io/pcd_io.h"
00047 #include "pcl/point_types.h"
00048 
00049 #include <sensor_msgs/point_cloud_conversion.h>
00050 
00051 #include <visualization_msgs/Marker.h>
00052 #include <visualization_msgs/MarkerArray.h>
00053 
00054 #include <object_manipulation_msgs/GraspPlanning.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/DebugConfig.h"
00061 #include "pr2_grasp_adjust/GraspAdjust.h"
00062 
00063 
00064 #include <iostream>
00065 #include <fstream>
00066 #include <queue>
00067 
00068 #include "gripper_model.h"
00069 #include "helpers.h"
00070 #include "grasp_adjust.h"
00071 
00072 #include <pcl/ModelCoefficients.h>
00073 #include <pcl/features/normal_3d.h>
00074 #include <pcl/filters/extract_indices.h>
00075 #include <pcl/filters/passthrough.h>
00076 #include <pcl/sample_consensus/method_types.h>
00077 #include <pcl/sample_consensus/model_types.h>
00078 #include <pcl/segmentation/sac_segmentation.h>
00079 
00080 
00081 /*
00082 #define PROF_ENABLED
00083 #include <profiling/profiling.h>
00084 PROF_DECLARE(TOTAL_TIMER)
00085 PROF_DECLARE(TRANSFORM_INPUT_CLOUD)
00086 PROF_DECLARE(DO_DESCENT)
00087 PROF_DECLARE(PROF_1)
00088 */
00089 
00090 typedef pcl::PointXYZRGBNormal PointT;
00091 
00092 const int GLOBAL_SEARCH =  pr2_grasp_adjust::Estimate_global_search;
00093 const int LOCAL_SEARCH =  pr2_grasp_adjust::Estimate_local_search;
00094 const int SINGLE_POSE =  pr2_grasp_adjust::Estimate_single_pose;
00095 
00096 // output file for training data
00097 std::ofstream outputFile;
00098 
00099 class GraspAdjustServer{
00100 
00101     pcl::PointCloud<PointT> last_cloud_;
00102 
00103     ros::NodeHandle nh_, nh_pvt_;
00104     ros::Subscriber sub_cloud_;
00105     ros::Publisher pub_cloud_, pub_marker_, pub_marker_array_;
00106     ros::Publisher pub_cloud_roi_, pub_cloud_debug_2;
00107 
00108     ros::ServiceServer adjust_srv_, evaluate_srv_, plan_srv_;
00109     ros::Timer update_timer_;
00110 
00111     std::string input_topic_;
00112     std::string output_topic_;
00113 
00114     tf::TransformListener listener_;
00115     tf::TransformBroadcaster broadcaster_;
00116 
00117     ros::Time now_;
00118 
00119     bool got_first_cloud_;
00120 
00121     GraspAdjust grasp_adjust_;
00122 
00123     // ***** Dynamic reconfigure stuff *****
00124     typedef pr2_grasp_adjust::EstimateConfig Config;
00125     dynamic_reconfigure::Server<Config>                dyn_srv;
00126     dynamic_reconfigure::Server<Config>::CallbackType  dyn_cb;
00127 
00128     typedef pr2_grasp_adjust::DebugConfig Debug;
00129     dynamic_reconfigure::Server<Debug>                debug_srv;
00130     dynamic_reconfigure::Server<Debug>::CallbackType  debug_cb;
00131 
00132     // Service wrapper for cluster bounding box finder service
00133     object_manipulator::ServiceWrapper<object_manipulation_msgs::FindClusterBoundingBox> bounding_box_client_;
00134 
00135     // ***** Helpers for training ******
00136     bool training_;
00137     std::ofstream train_file_;
00138 
00139 public:
00143     GraspAdjustServer(char* argv[]):
00144             nh_("/"),
00145             nh_pvt_("~"),
00146             input_topic_("/cloud_in"),
00147             output_topic_("/cloud_out"),
00148             dyn_srv(ros::NodeHandle("~config")),
00149             debug_srv(ros::NodeHandle("~debug")),
00150                         bounding_box_client_("/find_cluster_bounding_box"),
00151             training_(0)
00152     {
00153         got_first_cloud_ = false;
00154         now_ = ros::Time::now() - ros::Duration(1.0);
00155 
00156         // Clouds in and out
00157         sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(input_topic_, 1, boost::bind(&GraspAdjustServer::cloud_callback, this, _1));
00158         pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(output_topic_, 1);
00159         pub_cloud_roi_ = nh_.advertise<sensor_msgs::PointCloud2>("/roi_snapshot", 1);
00160 
00161         // Marker topics
00162         pub_marker_ = nh_.advertise<visualization_msgs::Marker>("/markers", 20);
00163         pub_marker_array_ = nh_.advertise<visualization_msgs::MarkerArray>("/markers_array", 20);
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_ = &listener_;
00170         grasp_adjust_.broadcaster_ = &broadcaster_;
00171 
00172         // Create callback for auto-running of algorithm
00173         update_timer_ = nh_.createTimer(ros::Duration(0.5), boost::bind(&GraspAdjustServer::timerCallback, this));
00174 
00175         bool load_debug_server = false;
00176         nh_pvt_.param("load_debug_server", load_debug_server, load_debug_server);
00177 
00178         // This server is for the user to adjust the algorithm weights
00179         dyn_cb = boost::bind( &GraspAdjustServer::dynamicCallback, this, _1, _2 );
00180         dyn_srv.setCallback(dyn_cb);
00181 
00182         // This server is used for debugging the geometry and algorithm code.
00183         if(load_debug_server){
00184             ROS_INFO("Loading the debug dynamic reconfigure server");
00185             debug_cb = boost::bind( &GraspAdjustServer::debugCallback, this, _1, _2 );
00186             debug_srv.setCallback(debug_cb);
00187         }
00188 
00189         adjust_srv_  = nh_.advertiseService("pr2_grasp_adjust",   &GraspAdjustServer::graspAdjustCallback, this);
00190         evaluate_srv_ = nh_.advertiseService("pr2_gripper_grasp_evaluate", &GraspAdjustServer::graspEvaluateCallback, this);
00191         plan_srv_ = nh_.advertiseService("pr2_gripper_grasp_plan", &GraspAdjustServer::graspPlanCallback, this);
00192 
00193         // if training, open the output file
00194         if(training_){
00195             ROS_INFO("in training mode, opening training_output.txt");
00196             train_file_.open("/u/hsiao/gripper_stereo_data/training_data/training_output.txt");
00197         }
00198 
00199         ROS_INFO("Finished constructor!");
00200     }
00201 
00202 
00203 private:
00204 
00208     void timerCallback()
00209     {
00210         now_= last_cloud_.header.stamp;
00211         geometry_msgs::PoseStamped pose_msg;
00212         tf::Pose pose;
00213 
00214         if(got_first_cloud_ && grasp_adjust_.debug_.run_continuous)
00215         {
00216             if(grasp_adjust_.debug_.custom_pose)
00217             {
00218                 if(grasp_adjust_.debug_.custom_frame.empty())
00219                     pose_msg.header.frame_id = "torso_lift_link";
00220                 else
00221                     pose_msg.header.frame_id = grasp_adjust_.debug_.custom_frame;
00222                 pose_msg.header.stamp = now_;
00223 
00224                 float x = grasp_adjust_.debug_.custom_x;
00225                 float y = grasp_adjust_.debug_.custom_y;
00226                 float z = grasp_adjust_.debug_.custom_z;
00227                 float roll = grasp_adjust_.debug_.custom_roll     * M_PI/180.0;
00228                 float pitch = grasp_adjust_.debug_.custom_pitch   * M_PI/180.0;
00229                 float yaw = grasp_adjust_.debug_.custom_yaw       * M_PI/180.0;
00230 
00231                 tf::Transform T_rot;
00232                 tf::Transform T_offset;
00233                 tf::Transform T_trans = tf::Transform(tf::Quaternion(0,0,0,1),
00234                                                       tf::Vector3(x, y, z));
00235                 T_rot = tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
00236                                       tf::Vector3( 0, 0, 0) );
00237                 T_offset = T_trans * grasp_adjust_.DEFAULT_GRIPPER.tool_frame_ * T_rot * grasp_adjust_.DEFAULT_GRIPPER.tool_frame_.inverse();
00238                 pose = T_offset;
00239             }
00240             else // Set a pose 6cm in front of current gripper pose
00241             {
00242                 pose_msg.header.frame_id = "r_wrist_roll_link";
00243                 pose_msg.header.stamp = now_;;
00244                 pose = tf::Pose(tf::Quaternion(0,0,0,1), tf::Vector3(0.06, 0, 0));
00245             }
00246 
00247             tf::poseTFToMsg(pose, pose_msg.pose);
00248             std::vector<geometry_msgs::PoseStamped> adjusted_poses;
00249             std::vector<double> pose_scores;
00250 
00251             int value = grasp_adjust_.findGrasps(pose_msg, &adjusted_poses, &pose_scores, grasp_adjust_.config_.search_mode);
00252             ROS_INFO("pr2_grasp_adjust::grasp_adjust returned %d", value);
00253         }
00254     }
00255 
00261     bool graspAdjustCallback(   pr2_grasp_adjust::GraspAdjust::Request  &req,
00262                                 pr2_grasp_adjust::GraspAdjust::Response &res )
00263     {
00264 
00265         int mode = req.search_mode;
00266         res.result.value = grasp_adjust_.findGrasps(req.grasp_pose, &(res.grasp_poses), &(res.pose_scores), mode);
00267 
00268         ROS_INFO("pr2_grasp_adjust::grasp_adjust returned %d", res.result.value);
00269 
00270         return true;
00271     }
00272 
00278     void setupGraspPlanning(object_manipulation_msgs::GraspPlanning::Request &req, pcl::PointCloud<PointT> &cloud)
00279     {
00280           pcl::PointCloud<pcl::PointXYZ> cloud_in;
00281       //pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
00282       sensor_msgs::PointCloud2 cloud_msg;
00283 
00284       sensor_msgs::convertPointCloudToPointCloud2(req.target.cluster, cloud_msg);
00285       ROS_INFO("Cloud_in in frame %s, reference frame %s!", cloud_msg.header.frame_id.c_str(), req.target.reference_frame_id.c_str());
00286       //for(int i = 0; i < cloud_msg.fields.size(); i++) ROS_INFO_STREAM("Cloud msg has field " << cloud_msg.fields[i]);
00287       pcl::fromROSMsg(cloud_msg, cloud_in);
00288 
00289 
00290       // Estimate point normals
00291       pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
00292       pcl::NormalEstimation<pcl::PointXYZ, PointT> ne;
00293       ne.setSearchMethod (tree);
00294       ne.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00295       ne.setKSearch (15);
00296       ne.compute (cloud);
00297       for(unsigned int i = 0; i < cloud_in.size(); i++){
00298         cloud.points[i].x = cloud_in.points[i].x;
00299         cloud.points[i].y = cloud_in.points[i].y;
00300         cloud.points[i].z = cloud_in.points[i].z;
00301       }
00302       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());
00303 
00304       sensor_msgs::PointCloud2 cloud_out;
00305       pcl::toROSMsg(cloud, cloud_out);
00306       pub_cloud_.publish(cloud_out);
00307 
00308           grasp_adjust_.setInputCloud(cloud);
00309         }
00310 
00311 
00315     geometry_msgs::PoseStamped generateRelativePose(geometry_msgs::PoseStamped original_pose, double roll, double pitch, 
00316                                                                                                  double yaw, double xdist, double ydist, double zdist)
00317     {
00318       tf::Pose pose_tf;
00319       tf::poseMsgToTF(original_pose.pose, pose_tf);
00320       tf::Transform T_rot = tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
00321                                         tf::Vector3( xdist, ydist, zdist) );
00322       tf::Transform output_rot = pose_tf * T_rot;
00323       geometry_msgs::PoseStamped output_pose;
00324       output_pose.header.frame_id = original_pose.header.frame_id;
00325       tf::poseTFToMsg(output_rot, output_pose.pose);
00326       return output_pose;
00327     }
00328 
00334     bool graspPlanCallback(   object_manipulation_msgs::GraspPlanning::Request  &req,
00335                               object_manipulation_msgs::GraspPlanning::Response &res )
00336     {  
00337       //clear old stuff
00338       unsigned int MAX_GRASPS = 20; // ??
00339       GripperModel clear_model;
00340       clear_model.setStamp(ros::Time::now());
00341       clear_model.setFrameId(req.target.reference_frame_id);
00342       if(pub_marker_array_.getNumSubscribers() > 0)
00343         for(unsigned int i = 0; i < MAX_GRASPS; i++)
00344         {
00345           clear_model.publishModel(pub_marker_array_, "grasp_planning", i, ros::Duration(), 1, visualization_msgs::Marker::DELETE);
00346         }
00347 
00348 
00349       ROS_INFO("starting graspPlanCallback");
00350       pcl::PointCloud<PointT> cloud;
00351       setupGraspPlanning(req, cloud);
00352 
00353       //get the names of the hand joints
00354       std::string arm_name = "right_arm";
00355       if(req.arm_name.length() != 0) arm_name = req.arm_name;
00356       else ROS_ERROR("point cluster grasp planner: missing arm_name in request!  Using right_arm");
00357       std::vector<std::string> hand_joints;
00358 
00359       XmlRpc::XmlRpcValue my_list;
00360       nh_.getParam(std::string("/hand_description/")+arm_name+std::string("/hand_joints"), my_list);
00361       ROS_ASSERT(my_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00362       for (int32_t i = 0; i < my_list.size(); ++i)
00363       {
00364       hand_joints.push_back(static_cast<std::string>(my_list[i]));
00365       }
00366 
00367       //Compute principal directions and object bounding box
00368       object_manipulation_msgs::FindClusterBoundingBox bounding_box_srv;
00369       ros::ServiceClient client = bounding_box_client_.client(ros::Duration(5.0));
00370       bounding_box_srv.request.cluster = req.target.cluster;
00371       bool result = client.call(bounding_box_srv);
00372       if(!result)
00373       {
00374         ROS_ERROR("Failed to call find_cluster_bounding_box service!");
00375         res.error_code.value = object_manipulation_msgs::GraspPlanningErrorCode::OTHER_ERROR;
00376         return true;
00377       }
00378 
00379       //Generate start poses from the top and sides
00380       std::vector<geometry_msgs::PoseStamped> start_poses;
00381       double start_dist = .01;
00382       //right
00383       start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,0.,0.,
00384                            -(bounding_box_srv.response.box_dims.x/2+.185+start_dist),0.,0.));
00385 //      //back ???
00386 //      start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,0.,M_PI/2,
00387 //                           0.,-(bounding_box_srv.response.box_dims.y/2+.185+start_dist),0.));
00388       //front
00389       start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,0.,-M_PI/2,
00390                            0.,bounding_box_srv.response.box_dims.y/2+.185+start_dist,0));
00391       //left
00392       start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,0.,M_PI,
00393                            bounding_box_srv.response.box_dims.x/2+.185+start_dist,0.,0.));
00394       //top
00395       start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, 0.,M_PI/2,0.,
00396                            0.,0.,bounding_box_srv.response.box_dims.z/2+.185+start_dist));
00397       //top rotated
00398       start_poses.push_back(generateRelativePose(bounding_box_srv.response.pose, M_PI/2,M_PI/2.0,0.,
00399                            0.,0.,bounding_box_srv.response.box_dims.z/2+.185+start_dist));
00400 
00401 
00402       std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> ranked_poses;
00403 
00404       //Adjust the start poses to find good grasps
00405       for(size_t start_pose_ind=0; start_pose_ind < start_poses.size(); start_pose_ind++)
00406       {
00407         int mode = GLOBAL_SEARCH;
00408         std::vector<geometry_msgs::PoseStamped> adjusted_poses;
00409         std::vector<double> pose_scores;
00410         //int value = grasp_adjust_.findGrasps(start_poses[start_pose_ind], &adjusted_poses, & pose_scores, mode, req.target.reference_frame_id);
00411         int value = grasp_adjust_.findGrasps(start_poses[start_pose_ind], &ranked_poses, mode, req.target.reference_frame_id);
00412       }
00413 
00414       ROS_INFO("Globally, found %d valid poses!", (int)ranked_poses.size());
00415       while(!ranked_poses.empty() && res.grasps.size() < MAX_GRASPS)
00416       {
00417         GripperModel model = ranked_poses.top();
00418         ranked_poses.pop();
00419 
00420         geometry_msgs::PoseStamped ps;
00421         model.toPoseStamped(ps);
00422 
00423         //Create the Grasp message
00424         object_manipulation_msgs::Grasp new_grasp;
00425         for(size_t i=0; i<hand_joints.size(); i++)
00426         {
00427           new_grasp.pre_grasp_posture.name.push_back(hand_joints[i]);
00428           new_grasp.pre_grasp_posture.position.push_back(0.5);
00429           new_grasp.grasp_posture.effort.push_back(100);
00430           new_grasp.grasp_posture.name.push_back(hand_joints[i]);
00431           new_grasp.grasp_posture.position.push_back(0);
00432           new_grasp.grasp_posture.effort.push_back(50);
00433         }
00434         new_grasp.grasp_pose = ps.pose;
00435         new_grasp.desired_approach_distance = 0.10;
00436         new_grasp.min_approach_distance = 0.05;
00437         new_grasp.success_probability = model.score_;
00438 
00439         //Add it to the response
00440         res.grasps.push_back(new_grasp);
00441 
00442         ROS_INFO("Added pose % 2d with score %.3f", (int)res.grasps.size(), model.score_);
00443         model.publishModel(pub_marker_array_, "grasp_planning", res.grasps.size(), ros::Duration(), model.score_*model.score_);
00444         //sleep(0.3);
00445       }
00446 
00447 
00448       res.error_code.value = object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS;
00449         ROS_INFO("pr2_grasp_adjust::grasp_evaluate returned %d", res.error_code.value);
00450         return true;
00451     }
00452 
00453 
00459     bool graspEvaluateCallback(   object_manipulation_msgs::GraspPlanning::Request  &req,
00460                                   object_manipulation_msgs::GraspPlanning::Response &res )
00461     {      
00462 
00463       pcl::PointCloud<PointT> cloud;
00464           setupGraspPlanning(req, cloud);
00465 
00466       // Compute cloud centroid...
00467       tf::Vector3 centroid = tf::Vector3(0,0,0);
00468       for(unsigned int i = 0; i < cloud.points.size(); i++ )
00469       {
00470           PointT point = cloud.points[i];
00471           centroid += tf::Vector3(point.x, point.y, point.z);
00472       }
00473       centroid /= cloud.points.size() + 0.001;
00474 
00475       ROS_INFO("There are %d grasps to evaluate", (int)req.grasps_to_evaluate.size());
00476       ROS_INFO("object  grasp   centroid   symmetry     points  pnt_bonus    normals  collision");
00477       bool CONTINUE = false;
00478 
00479       for(unsigned j = 0; j < req.grasps_to_evaluate.size(); j ++)
00480       {
00481 
00482         this->broadcaster_.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0,0,0,1), centroid),
00483                                                               cloud.header.stamp, cloud.header.frame_id.c_str(), "/centroid"));
00484 
00485         geometry_msgs::Pose pose_msg = req.grasps_to_evaluate[j].grasp_pose;
00486         tf::Pose pose_tf;
00487         tf::poseMsgToTF(pose_msg, pose_tf);
00488 
00489         FeatureWeights features;
00490         GripperModel gripper;
00491         gripper.setHeader(cloud.header);
00492         gripper.transform(pose_tf, cloud.header.frame_id.c_str());
00493 
00494         gripper.publishModel(pub_marker_array_, "gripper_model", 0, ros::Duration(20.0), 2.0);
00495         gripper.evaluateGrasp(cloud, features, centroid, false, true);
00496 
00497         char feature_str[200];
00498 
00499         if(gripper.state_.no_object) {
00500           sprintf(feature_str, "% 6d % 6d   No points in contact with gripper!", req.target.potential_models[0].model_id, j);
00501         }
00502         else {
00503           sprintf(feature_str, "% 6d % 6d % 10.5f % 10.5f % 10.5f % 10.5f % 10.5f % 10.5f",
00504               req.target.potential_models[0].model_id,  j,
00505               features.centroid, features.symmetry, features.point,
00506               features.Iyz, features.normal, features.collision
00507               );
00508         }
00509 
00510         outputFile << feature_str << std::endl;
00511         ROS_INFO_STREAM(feature_str);
00512 
00513         if(!CONTINUE && grasp_adjust_.config_.debug_stepping) {
00514           char key[256]; std::cin.getline(key, 256);
00515           if(key[0] == 'c') CONTINUE = true;
00516         }
00517 
00518         usleep(grasp_adjust_.config_.pause_time*1000000);
00519       }
00520 
00521       res.error_code.value = object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS;
00522       ROS_INFO("pr2_grasp_adjust::grasp_evaluate returned %d", res.error_code.value);
00523       return true;
00524     }
00525 
00526 
00530     void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &input)
00531     {
00532       ROS_DEBUG("Got a cloud with %d points!", input->width*input->height);
00533 
00534       if( !strcmp(input->fields[4].name.c_str(), "normal_x") )
00535       {
00536         pcl::fromROSMsg(*input, last_cloud_);
00537         grasp_adjust_.setInputCloud(last_cloud_);
00538 
00539         if(!got_first_cloud_){
00540           got_first_cloud_ = true;
00541           ROS_INFO("Got first cloud!");
00542         }
00543       }
00544     }
00545 
00546 
00550     void dynamicCallback(Config &new_config, uint32_t id)
00551       {
00552           // This info will get printed back to reconfigure gui
00553           char status[256] = "\0";
00554 
00555           switch(id)
00556           {
00557           case(-1): //Init // If you are tempted to put anything here, it should probably go in the constructor
00558             break;
00559           case(0): // Connect
00560             printf("Reconfigure GUI connected to me!\n");
00561             new_config = grasp_adjust_.config_;
00562             break;
00563           case(91):
00564             if(new_config.write_file)
00565             {
00566               ROS_INFO("In training mode, opening %s", new_config.filename.c_str());
00567               outputFile.open(new_config.filename.c_str());
00568               outputFile << "object  grasp   centroid   symmetry     points  pnt_bonus    normals  collision" << std::endl;
00569             }
00570             else
00571             {
00572               ROS_INFO("Closing file %s", new_config.filename.c_str());
00573               outputFile.close();
00574 
00575             }
00576             break;
00577           default:
00578             ROS_INFO("Dynamic reconfigure did something, variable %d.", id);
00579           }
00580           grasp_adjust_.config_ = new_config;
00581           new_config.status = status;
00582       }
00583 
00584 
00588     void debugCallback(Debug &new_config, uint32_t id)
00589       {
00590           // This info will get printed back to reconfigure gui
00591           char status[256] = "\0";
00592 
00593           switch(id){
00594               case(-1): //Init // If you are tempted to put anything here, it should probably go in the constructor
00595                 break;
00596               case(0): // Connect
00597                 printf("Reconfigure GUI connected to me!\n");
00598                 new_config = grasp_adjust_.debug_;
00599                 break;
00600               default:
00601                 ROS_INFO("Dynamic reconfigure did something, variable %d.", id);
00602           }
00603           grasp_adjust_.debug_ = new_config;
00604           new_config.status = status;
00605       }
00606 };
00607 
00608 
00609 
00610 /* ---[ */
00611 int main (int argc, char* argv[])
00612 {
00613   ros::init(argc, argv, "filter");
00614 
00615   GraspAdjustServer server(argv);
00616 
00617   ros::spin();
00618 
00619   return (0);
00620 }
00621 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends


pr2_grasp_adjust
Author(s): Adam Leeper
autogenerated on Wed Jan 23 2013 15:27:53