zyonz_nbv_geometric_simple_alg.cpp

Go to the documentation of this file.

#include "zyonz_nbv_geometric_simple_alg.h"

ZyonzNbvGeometricSimpleAlgorithm::ZyonzNbvGeometricSimpleAlgorithm(void):
nbv_ptr_(boost::make_shared<geometry_msgs::PoseStamped> ())
{
}

ZyonzNbvGeometricSimpleAlgorithm::~ZyonzNbvGeometricSimpleAlgorithm(void)
{
}

void ZyonzNbvGeometricSimpleAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// ZyonzNbvGeometricSimpleAlgorithm Public API
bool ZyonzNbvGeometricSimpleAlgorithm::isRunning(geometry_msgs::PoseStamped& viewpoint, geometry_msgs::PoseStamped& viewpoint_roi)
{
    // NBV Transform definition
    tf::Quaternion tf_quat(tf::Quaternion::getIdentity());

    if (viewpoint_roi.pose.position.x < 0.0)
        tf_quat.setEuler(M_PI_4/2,0.0, 0.0 ); // (YAW, PITCH, ROLL) -> (Y, X, Z)
    else 
        tf_quat.setEuler(-M_PI_4/2,0.0, 0.0 ); // (YAW, PITCH, ROLL) -> (Y, X, Z)

    tf::Transform TF(tf_quat);

    // Poses conversions
    tf::Stamped<tf::Transform> w_tf_viewpoint;
    tf::poseStampedMsgToTF (viewpoint, w_tf_viewpoint); 
    
    tf::Stamped<tf::Transform> viewpoint_tf_roi;
    tf::poseStampedMsgToTF (viewpoint_roi, viewpoint_tf_roi); 
    
    // Compute NBV pose
    tf::Stamped<tf::Transform> w_tf_nbv (w_tf_viewpoint * viewpoint_tf_roi * TF * viewpoint_tf_roi.inverse(), ros::Time::now(), w_tf_viewpoint.frame_id_);

    tf::poseStampedTFToMsg(w_tf_nbv, *nbv_ptr_);

    return true;
}

geometry_msgs::PoseStamped::Ptr ZyonzNbvGeometricSimpleAlgorithm::get_nbv () const
{
    return nbv_ptr_; 
}