moped_handler_alg.cpp
Go to the documentation of this file.
00001 #include "moped_handler_alg.h"
00002 
00003 MopedHandlerAlgorithm::MopedHandlerAlgorithm(void)
00004 {
00005 }
00006 
00007 MopedHandlerAlgorithm::~MopedHandlerAlgorithm(void)
00008 {
00009 }
00010 
00011 void MopedHandlerAlgorithm::config_update(Config& new_cfg, uint32_t level)
00012 {
00013   this->lock();
00014 
00015   // save the current configuration
00016   this->config_=new_cfg;
00017   
00018   this->unlock();
00019 }
00020 
00021 // MopedHandlerAlgorithm Public API
00022 
00023 
00024 
00025 
00026 // DEAD CODE
00027 
00028 /*
00029 void MopedHandlerAlgorithm::setObjectDataFromRosMsg(pr_msgs::ObjectPoseList input)
00030 {
00031         // Clear previous data
00032         this->inputOpl.clear();
00033 
00034         // For every object pose in the input msg...
00035         for(size_t i=0; i<input.object_list.size(); ++i)
00036         {
00037                 // Fill attributes with data.
00038                 Op_t current;
00039 
00040                 current.pos3D.x = input.object_list[i].pose.position.x;
00041                 current.pos3D.y = input.object_list[i].pose.position.y;
00042                 current.pos3D.z = input.object_list[i].pose.position.z;
00043 
00044                 current.pos2D.x = input.object_list[i].pose2D.x;
00045                 current.pos2D.y = input.object_list[i].pose2D.y;
00046 
00047                 current.ori.x = input.object_list[i].pose.orientation.x;
00048                 current.ori.y = input.object_list[i].pose.orientation.y;
00049                 current.ori.z = input.object_list[i].pose.orientation.z;
00050                 current.ori.w = input.object_list[i].pose.orientation.w;
00051 
00052                 this->inputOpl.push_back(current);
00053         }
00054 }
00055 
00056 //   Starts to compute the input data and fills the output data.
00057 void MopedHandlerAlgorithm::compute()
00058 {
00059         // For every object...
00060         for(int i=0; i < this->inputOpl.size(); ++i)
00061         {
00062                 // Get object
00063                 Op_t objectPose = this->inputOpl[i];
00064 
00065                 // Get pixels of object position
00066                 int xPixel, yPixel;
00067                 getPose2Pixel(objectPose.pos3D.x, objectPose3D.pos.y, objectPose3D.pos.z, xPixel, yPixel, this->pcl_);
00068 
00069                 // Get position of point with new depth.
00070                 pos_t refinedPosition = getPixel2Pose(xPixel, yPixel);
00071                 Op_t refinedOp(objectPose);
00072                 refinedOp.pos = refinedPosition;
00073 
00074                 // Save refined object position to the attribute.
00075                 this->outputOpl.push_back(refinedOp);
00076         }
00077 }
00078 
00079 //   Creates and returns a ROS msg from the output OPL.
00080 pr_msgs::ObjectPoseList MopedHandlerAlgorithm::getRosMsgWithObjectData()
00081 {
00082         size_t n = this->outputOpl.size();
00083 
00084         pr_msgs::ObjectPoseList result;
00085         result.object_list.resize(n);
00086 
00087         // For every object pose in the output attribute...
00088         for(size_t i=0; i<n; ++i)
00089         {
00090                 result.object_list[i].pose.position.x = this->outputOpl[i].pos.x;
00091                 result.object_list[i].pose.position.y = this->outputOpl[i].pos.y;
00092                 result.object_list[i].pose.position.z = this->outputOpl[i].pos.z;
00093 
00094                 result.object_list[i].pose.orientation.x = this->outputOpl[i].ori.x;
00095                 result.object_list[i].pose.orientation.y = this->outputOpl[i].ori.y;
00096                 result.object_list[i].pose.orientation.z = this->outputOpl[i].ori.z;
00097                 result.object_list[i].pose.orientation.w = this->outputOpl[i].ori.w;
00098         }
00099 
00100         return result;
00101 }
00102 
00103 void MopedHandlerAlgorithm::getPose2Pixel(float x, float y, float z, int& xPixel, int& yPixel, pcl::PointCloud<pcl::PointXYZ> pcl)
00104 {
00105         for(int col=0; col<640; ++col)
00106         for(int row=0; row<480; ++row)
00107         {
00108                 pcl::PointXYZ p;
00109                 p = pcl.at(col, row);
00110 
00111                 if( p.x == x and p.y == y and p.z == z)
00112                 {
00113                         xPixel = row;
00114                         yPixel = col;
00115                         return;
00116                 }
00117         }
00118 
00119         cout << "MopedHandlerAlgorithm::getPose2Pixel: Error! Point not found." << endl;
00120 }
00121 
00122 //float MopedHandlerAlgorithm::getDepthPixel(int x, int y, pcl::PointCloud<pcl::PointXYZ> pcl)
00123 //{
00124 //      pcl::PointXYZ p = pcl.at(x, y);
00125 
00126 //      float aux1 = p.x * p.x;
00127 //      float aux2 = p.y * p.y;
00128 //      float aux3 = p.z * p.z;
00129 
00130 //      return sqrt(aux1 + aux2 + aux3);
00131 //}
00132 
00133 MopedHandlerAlgorithm::pos_t MopedHandlerAlgorithm::getPixel2Pose(float xPixel, float yPixel, pcl::PointCloud<pcl::PointXYZ> pcl);
00134 {
00135         pcl::PointXYZ p = pcl.at(x, y);
00136 
00137         // Check p correctness.
00138         // TODO: Complete.
00139 
00140         MopedHandlerAlgorithm::pos_t result;
00141         result.x = p.x;
00142         result.y = p.y;
00143         result.z = p.z;
00144         return result;
00145 }
00146 
00147 */
00148 


iri_moped_handler
Author(s): frigual
autogenerated on Fri Dec 6 2013 20:55:45