moped_handler_alg.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author 
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
// IMPORTANT NOTE: This code has been generated through a script from the 
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab

#ifndef _moped_handler_alg_h_
#define _moped_handler_alg_h_

#include <iri_moped_handler/MopedHandlerConfig.h>
#include "mutex.h"

// C++ includes
#include <iostream>
#include <vector>
#include <cmath>
using namespace std;

// Publisher subscriber headers
#include <pr_msgs/ObjectPoseList.h>

// PCL specific includes
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

//include moped_handler_alg main library

class MopedHandlerAlgorithm
{
  protected:
    CMutex alg_mutex_;

    // private attributes and methods

  public:
    typedef iri_moped_handler::MopedHandlerConfig Config;

    Config config_;

    MopedHandlerAlgorithm(void);

    void lock(void) { alg_mutex_.enter(); };

    void unlock(void) { alg_mutex_.exit(); };

    bool try_enter(void) { return alg_mutex_.try_enter(); };

    void config_update(Config& new_cfg, uint32_t level=0);

    // here define all moped_handler_alg interface methods to retrieve and set
    // the driver parameters

    ~MopedHandlerAlgorithm(void);

};

#endif


// DEAD CODE

/*
        // Struct for position
        typedef struct
        {
                float x, y, z;
        } pos_t;

        // Struct for orientation
        typedef struct
        {
                float x, y, z, w;
        } ori_t;

        // Struct for object pose.
        typedef struct
        {
                pos_t pos2D;
                pos_t pos3D;
                ori_t ori;
        } Op_t;

        // Attributes
        vector<Op_t> inputOpl; // Object Position List
        vector<Op_t> outputOpl; // Object Position List
        pcl::PointCloud<pcl::PointXYZ> pcl_;

        // Main functions

        //   Set data to @attribute opl from a ROS message.
        void setObjectDataFromRosMsg(pr_msgs::ObjectPoseList input);

        //   Starts to compute the input data and fills the output data.
        void compute();

        //   Creates and returns a ROS msg from the output OPL.
        pr_msgs::ObjectPoseList getRosMsgWithObjectData();

        // Other functions
        //   Given a position wrt the camera, gives the pixel showing that position.
        void getPose2Pixel(float x, float y, float z, int& xPixel, int& yPixel, pcl::PointCloud<pcl::PointXYZ> pcl);
        //   Given a pixel position (x, y) returns the distance to that pixel in the @param pcl.
//      float getDepthPixel(int x, int y, pcl::PointCloud<pcl::PointXYZ> pcl);
        //   Given a pixel, return the point in that pixel (wrt the camera), according to the input pcl.
        MopedHandlerAlgorithm::pos_t getPixel2Pose(float xPixel, float yPixel, pcl::PointCloud<pcl::PointXYZ> pcl);

*/