ObjectTransformation.cpp

Go to the documentation of this file.

#include "helper/ObjectTransformation.h"

namespace ProbabilisticSceneRecognition {
 
  ObjectTransformation::ObjectTransformation()
  : mTfListener(ros::Duration(5184000.0))
  {
  }
  
  ObjectTransformation::~ObjectTransformation()
  {
  }
  
  void ObjectTransformation::setBaseFrame(std::string pBaseFrame)
  {
    mBaseFrame = pBaseFrame;
  }

  void ObjectTransformation::transform(const boost::shared_ptr<asr_msgs::AsrObject>& pObject)
  {
      if (mBaseFrame == pObject->header.frame_id) return;   // shortcut when both frames are equal.

    // Create everything required for the transformation
    geometry_msgs::PoseStamped input, output;
    input.header = pObject->header;

    if(!pObject->sampledPoses.size()){
      std::cerr << "Got a AsrObject without poses." << std::endl;
      std::exit(1);    
    }
    input.pose = pObject->sampledPoses.front().pose;
    
    // If no transformation from source to target frame possible, drop object.
    if(!mTfListener.waitForTransform(mBaseFrame, input.header.frame_id, pObject->header.stamp, ros::Duration(1.0)))
      throw std::runtime_error("Unable to resolve transformation in target coordinate frame.");
    
    // Do the transformation.
    mTfListener.transformPose(mBaseFrame, input, output);
    
    // Write the results back.
    pObject->header = output.header;

    pObject->sampledPoses.pop_back();
    geometry_msgs::PoseWithCovariance output_pose_with_c;
    output_pose_with_c.pose = output.pose;
    pObject->sampledPoses.push_back(output_pose_with_c);
  }
  
}