ism_helper.hpp

Go to the documentation of this file.

#ifndef _ISM_HELPER_HPP_
#define _ISM_HELPER_HPP_

//Pkg includes
#include <ros/ros.h>
#include <tf/transform_listener.h>

#include <asr_msgs/AsrObject.h>
#include <asr_object_database/ObjectMetaData.h>

//Local includes
#include <ISM/recognizer/Recognizer.hpp>

namespace IH {
  
  ISM::PosePtr rosPoseToISMPose(const geometry_msgs::Pose p);

  class ObjectConverter
  {
  public:

    ObjectConverter(const std::string& pBaseFrame, const bool pIgnoreTypes, const bool pIgnoreIds, const int pEnableRotationMode, const std::string pRotationFrame, const std::string pRotationObjType, const std::string pRotationObjId);
    ObjectConverter(const std::string& pBaseFrame, const bool pIgnoreTypes, const bool pIgnoreIds, const int pEnableRotationMode, const std::string pRotationFrame, const std::string pRotationObjType, const std::string pRotationObjId, const bool pUseConfidenceFromMsg);

    virtual ~ObjectConverter() {};

    ISM::ObjectPtr pbdObjectToISMObject(const asr_msgs::AsrObject& o);
    void normalizeRotationInvariantObjects(ISM::ObjectSetPtr objectSet);
    
  private:
    //Some consts for orientation normalization.
    const int ROTATION_MODE_DEACTIVATED = 0;
    const int ROTATION_MODE_FRAME = 1;
    const int ROTATION_MODE_OBJECT = 2;

    tf::TransformListener mTfListener;

    const std::string mBaseFrame;

    const bool mIgnoreTypes;
    const bool mIgnoreIds;

    const int mEnableRotationMode;

    const std::string mRotationFrame;
    const std::string mRotationObjType;
    const std::string mRotationObjId;
    ISM::ObjectPtr mRotationRefObject;

    const bool mUseConfidenceFromMsg;

    std::vector<asr_msgs::AsrObject> transformBuffer;

    ros::NodeHandle nodehandle;

    ros::ServiceClient objectMetaDataClient;

    Eigen::AngleAxisd calculateTransform(const Eigen::Vector3d& objectAxis, const Eigen::Vector3d& referenceAxis, const Eigen::Vector3d& unitAxis);
  }; 

  typedef boost::shared_ptr<ObjectConverter> ObjectConverterPtr;

}

#endif /* _ISM_HELPER_HPP_ */