ObjectRecognizer.cpp
Go to the documentation of this file.
00001 
00012 // RAIL Recognition
00013 #include "rail_recognition/ObjectRecognizer.h"
00014 #include "rail_recognition/PointCloudRecognizer.h"
00015 
00016 using namespace std;
00017 using namespace rail::pick_and_place;
00018 
00019 ObjectRecognizer::ObjectRecognizer()
00020     : private_node_("~"),
00021       as_(private_node_, "recognize_object", boost::bind(&ObjectRecognizer::recognizeObjectCallback, this, _1), false)
00022 {
00023   // set defaults
00024   int port = graspdb::Client::DEFAULT_PORT;
00025   string host("127.0.0.1");
00026   string user("ros");
00027   string password("");
00028   string db("graspdb");
00029 
00030   // grab any parameters we need
00031   node_.getParam("/graspdb/host", host);
00032   node_.getParam("/graspdb/port", port);
00033   node_.getParam("/graspdb/user", user);
00034   node_.getParam("/graspdb/password", password);
00035   node_.getParam("/graspdb/db", db);
00036 
00037   // connect to the grasp database
00038   graspdb_ = new graspdb::Client(host, port, user, password, db);
00039   okay_ = graspdb_->connect();
00040 
00041   // start the action server
00042   as_.start();
00043 
00044   if (okay_)
00045   {
00046     ROS_INFO("Object Recognizer Successfully Initialized");
00047   }
00048 }
00049 
00050 ObjectRecognizer::~ObjectRecognizer()
00051 {
00052   // cleanup
00053   as_.shutdown();
00054   graspdb_->disconnect();
00055   delete graspdb_;
00056 }
00057 
00058 bool ObjectRecognizer::okay() const
00059 {
00060   return okay_;
00061 }
00062 
00063 void ObjectRecognizer::recognizeObjectCallback(const rail_manipulation_msgs::RecognizeObjectGoalConstPtr &goal)
00064 {
00065   ROS_INFO("Recognize Object Request Received.");
00066 
00067   rail_manipulation_msgs::RecognizeObjectFeedback feedback;
00068   feedback.message = "Loading candidate models...";
00069   as_.publishFeedback(feedback);
00070 
00071   // populate candidates based on the name if it exists
00072   vector<graspdb::GraspModel> candidates;
00073   if (goal->name.size() > 0)
00074   {
00075     graspdb_->loadGraspModelsByObjectName(goal->name, candidates);
00076   }
00077   else
00078   {
00079     graspdb_->loadGraspModels(candidates);
00080   }
00081 
00082   // convert to PCL grasp models
00083   vector<PCLGraspModel> pcl_candidates;
00084   for (size_t i = 0; i < candidates.size(); i++)
00085   {
00086     pcl_candidates.push_back(PCLGraspModel(candidates[i]));
00087   }
00088 
00089   // copy the information to the result
00090   rail_manipulation_msgs::RecognizeObjectResult result;
00091   result.object = goal->object;
00092 
00093   // perform recognition
00094   feedback.message = "Running recognition...";
00095   as_.publishFeedback(feedback);
00096   PointCloudRecognizer recognizer;
00097   if (!recognizer.recognizeObject(result.object, pcl_candidates))
00098   {
00099     as_.setSucceeded(result, "Object could not be recognized.");
00100   }
00101   else
00102   {
00103     as_.setSucceeded(result, "Object successfully recognized.");
00104   }
00105 }


rail_recognition
Author(s): David Kent , Russell Toris , bhetherman
autogenerated on Thu Jun 6 2019 19:44:08