GraspCollector.cpp
Go to the documentation of this file.
00001 
00013 // RAIL Grasp Collection
00014 #include "rail_grasp_collection/GraspCollector.h"
00015 
00016 // ROS
00017 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
00018 
00019 using namespace std;
00020 using namespace rail::pick_and_place;
00021 
00022 GraspCollector::GraspCollector()
00023     : private_node_("~"), ac_wait_time_(AC_WAIT_TIME), tf_listener_(tf_buffer_),
00024       robot_fixed_frame_id_("base_footprint"), eef_frame_id_("eef_link"),
00025       as_(private_node_, "grasp_and_store", boost::bind(&GraspCollector::graspAndStore, this, _1), false)
00026 {
00027   // set defaults
00028   debug_ = DEFAULT_DEBUG;
00029   int port = graspdb::Client::DEFAULT_PORT;
00030   string segmented_objects_topic("/segmentation/segmented_objects");
00031   string gripper_action_server("/manipulation/gripper");
00032   string lift_action_server("/manipulation/lift");
00033   string verify_grasp_action_server("/manipulation/verify_grasp");
00034   string host("127.0.0.1");
00035   string user("ros");
00036   string password("");
00037   string db("graspdb");
00038 
00039   // grab any parameters we need
00040   private_node_.getParam("debug", debug_);
00041   private_node_.getParam("robot_fixed_frame_id", robot_fixed_frame_id_);
00042   private_node_.getParam("eef_frame_id", eef_frame_id_);
00043   private_node_.getParam("segmented_objects_topic", segmented_objects_topic);
00044   private_node_.getParam("gripper_action_server", gripper_action_server);
00045   private_node_.getParam("lift_action_server", lift_action_server);
00046   private_node_.getParam("verify_grasp_action_server", verify_grasp_action_server);
00047   node_.getParam("/graspdb/host", host);
00048   node_.getParam("/graspdb/port", port);
00049   node_.getParam("/graspdb/user", user);
00050   node_.getParam("/graspdb/password", password);
00051   node_.getParam("/graspdb/db", db);
00052 
00053   // set up a connection to the grasp database
00054   graspdb_ = new graspdb::Client(host, port, user, password, db);
00055   okay_ = graspdb_->connect();
00056 
00057   // setup a debug publisher if we need it
00058   if (debug_)
00059   {
00060     debug_pub_ = private_node_.advertise<sensor_msgs::PointCloud2>("debug", 1, true);
00061   }
00062 
00063   // subscribe to the list of segmented objects
00064   segmented_objects_sub_ = node_.subscribe(segmented_objects_topic, 1, &GraspCollector::segmentedObjectsCallback,
00065                                            this);
00066 
00067   // setup action clients
00068   gripper_ac_ = new actionlib::SimpleActionClient<rail_manipulation_msgs::GripperAction>(gripper_action_server, true);
00069   lift_ac_ = new actionlib::SimpleActionClient<rail_manipulation_msgs::LiftAction>(lift_action_server, true);
00070   verify_grasp_ac_ = new actionlib::SimpleActionClient<rail_manipulation_msgs::VerifyGraspAction>(
00071       verify_grasp_action_server, true
00072   );
00073 
00074   // start the action server
00075   as_.start();
00076 
00077   if (okay_)
00078   {
00079     ROS_INFO("Grasp Collector Successfully Initialized");
00080   }
00081 }
00082 
00083 GraspCollector::~GraspCollector()
00084 {
00085   // cleanup
00086   as_.shutdown();
00087   graspdb_->disconnect();
00088   delete gripper_ac_;
00089   delete lift_ac_;
00090   delete verify_grasp_ac_;
00091   delete graspdb_;
00092 }
00093 
00094 bool GraspCollector::okay() const
00095 {
00096   return okay_;
00097 }
00098 
00099 void GraspCollector::graspAndStore(const rail_pick_and_place_msgs::GraspAndStoreGoalConstPtr &goal)
00100 {
00101   ROS_INFO("Store grasp requset received.");
00102 
00103   rail_pick_and_place_msgs::GraspAndStoreFeedback feedback;
00104   rail_pick_and_place_msgs::GraspAndStoreResult result;
00105   // default to false
00106   result.success = false;
00107   result.id = 0;
00108 
00109   // used for action server checks
00110   bool completed, succeeded, success;
00111 
00112   // request a grasp from the arm
00113   feedback.message = "Requesting a close gripper action...";
00114   as_.publishFeedback(feedback);
00115   rail_manipulation_msgs::GripperGoal gripper_goal;
00116   gripper_goal.close = true;
00117   gripper_ac_->sendGoal(gripper_goal);
00118   completed = gripper_ac_->waitForResult(ac_wait_time_);
00119   succeeded = (gripper_ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED);
00120   success = gripper_ac_->getResult()->success;
00121   if (!completed || !succeeded || !success)
00122   {
00123     as_.setSucceeded(result, "Could not close the gripper.");
00124     return;
00125   }
00126 
00127   // get the grasp position information
00128   feedback.message = "Determinging grasp position...";
00129   as_.publishFeedback(feedback);
00130   // get the TF from the buffer
00131   geometry_msgs::TransformStamped grasp;
00132   try
00133   {
00134     grasp = tf_buffer_.lookupTransform(robot_fixed_frame_id_, eef_frame_id_, ros::Time(0));
00135   } catch (tf2::TransformException &ex)
00136   {
00137     ROS_WARN("%s", ex.what());
00138     as_.setSucceeded(result, "Could not transform from the grasp frame to the robot fixed frame.");
00139     return;
00140   }
00141 
00142   // check if we are doing a lift
00143   if (goal->lift)
00144   {
00145     // request a lift from the arm
00146     feedback.message = "Requesting lift...";
00147     as_.publishFeedback(feedback);
00148     rail_manipulation_msgs::LiftGoal lift_goal;
00149     lift_ac_->sendGoal(lift_goal);
00150     completed = lift_ac_->waitForResult(ac_wait_time_);
00151     succeeded = (lift_ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED);
00152     success = lift_ac_->getResult()->success;
00153     if (!completed || !succeeded || !success)
00154     {
00155       as_.setSucceeded(result, "Could not execute lift.");
00156       return;
00157     }
00158   }
00159 
00160   // check if we are doing a grasp verification check
00161   if (goal->verify)
00162   {
00163     // request a grasp verification from the arm
00164     feedback.message = "Requesting grasp verification...";
00165     as_.publishFeedback(feedback);
00166     rail_manipulation_msgs::VerifyGraspGoal verify_grasp_goal;
00167     verify_grasp_ac_->sendGoal(verify_grasp_goal);
00168     completed = verify_grasp_ac_->waitForResult(ac_wait_time_);
00169     succeeded = (verify_grasp_ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED);
00170     rail_manipulation_msgs::VerifyGraspResultConstPtr verify_result = verify_grasp_ac_->getResult();
00171     success = verify_result->success;
00172     if (!completed || !succeeded || !success)
00173     {
00174       as_.setSucceeded(result, "Could not verify grasp.");
00175       return;
00176     } else if (!verify_result->grasping)
00177     {
00178       as_.setSucceeded(result, "Grasp is not verified.");
00179       return;
00180     }
00181   }
00182 
00183   // check for the closest object
00184   feedback.message = "Searching for the closest segmented object...";
00185   as_.publishFeedback(feedback);
00186   // lock for the vector
00187   {
00188     boost::mutex::scoped_lock lock(mutex_);
00189     // check if we actually have some objects
00190     int closest = 0;
00191     if (object_list_->objects.size() == 0)
00192     {
00193       as_.setSucceeded(result, "No segmented objects found.");
00194       return;
00195     } else if (object_list_->objects.size() > 1)
00196     {
00197       // find the closest point
00198       float min = numeric_limits<float>::infinity();
00199       //geometry_msgs::Vector3 &v = grasp.transform.translation;
00200       // check each segmented object
00201       for (size_t i = 0; i < object_list_->objects.size(); i++)
00202       {
00203         geometry_msgs::TransformStamped eef_transform = tf_buffer_.lookupTransform(
00204             object_list_->objects[i].point_cloud.header.frame_id, eef_frame_id_, ros::Time(0)
00205         );
00206         geometry_msgs::Vector3 &v = eef_transform.transform.translation;
00207         //convert PointCloud2 to PointCloud to access the data easily
00208         sensor_msgs::PointCloud cloud;
00209         sensor_msgs::convertPointCloud2ToPointCloud(object_list_->objects[i].point_cloud, cloud);
00210         // check each point in the cloud
00211         for (size_t j = 0; j < cloud.points.size(); j++)
00212         {
00213           // euclidean distance to the point
00214           float dist = sqrt(
00215               pow(cloud.points[j].x - v.x, 2) + pow(cloud.points[j].y - v.y, 2) + pow(cloud.points[j].z - v.z, 2)
00216           );
00217           if (dist < min)
00218           {
00219             min = dist;
00220             closest = i;
00221           }
00222         }
00223       }
00224     }
00225     // check if we need to transform the point cloud
00226     rail_manipulation_msgs::SegmentedObject &object = object_list_->objects[closest];
00227     if (object.point_cloud.header.frame_id != robot_fixed_frame_id_)
00228     {
00229       try
00230       {
00231         sensor_msgs::PointCloud2 transformed_cloud = tf_buffer_.transform(object.point_cloud, robot_fixed_frame_id_,
00232                                                                           ros::Time(0),
00233                                                                           object.point_cloud.header.frame_id);
00234         object.point_cloud = transformed_cloud;
00235         object.point_cloud.header.frame_id = robot_fixed_frame_id_;
00236       } catch (tf2::TransformException &ex)
00237       {
00238         ROS_WARN("%s", ex.what());
00239         as_.setSucceeded(result, "Could not transform the segemented object to the robot fixed frame.");
00240         return;
00241       }
00242     }
00243     // check if we are going to publish some debug info
00244     if (debug_)
00245     {
00246       debug_pub_.publish(object.point_cloud);
00247     }
00248 
00249     // store the data
00250     feedback.message = "Storing grasp data...";
00251     as_.publishFeedback(feedback);
00252     graspdb::GraspDemonstration gd(goal->object_name, graspdb::Pose(grasp), eef_frame_id_, object.point_cloud,
00253                                    object.image);
00254     if (graspdb_->addGraspDemonstration(gd))
00255     {
00256       // store the ID
00257       result.id = gd.getID();
00258     } else
00259     {
00260       as_.setSucceeded(result, "Could not insert into database.");
00261       return;
00262     }
00263   }
00264 
00265   // success
00266   result.success = true;
00267   as_.setSucceeded(result, "Success!");
00268 }
00269 
00270 void GraspCollector::segmentedObjectsCallback(const rail_manipulation_msgs::SegmentedObjectList::Ptr &object_list)
00271 {
00272   ROS_INFO("Updated segmented object list received.");
00273   // lock for the vector
00274   boost::mutex::scoped_lock lock(mutex_);
00275   object_list_ = object_list;
00276 }


rail_grasp_collection
Author(s): Russell Toris , David Kent
autogenerated on Thu Jun 6 2019 19:44:05