zyonz_camera_pose_around_point_alg.cpp

Go to the documentation of this file.

#include "zyonz_camera_pose_around_point_alg.h"

ZyonzCameraPoseAroundPointAlgorithm::ZyonzCameraPoseAroundPointAlgorithm(void):
camera_pose_(boost::make_shared<geometry_msgs::PoseStamped>())
{
}

ZyonzCameraPoseAroundPointAlgorithm::~ZyonzCameraPoseAroundPointAlgorithm(void)
{
}

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

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

// ZyonzCameraPoseAroundPointAlgorithm Public API
bool  ZyonzCameraPoseAroundPointAlgorithm::isRunning(geometry_msgs::PoseStamped& view_point, double& dist_m, double& rad_x_rot, double& rad_y_rot)
{
    // Pose of the viewpoint
    // (Its rotation part will usually be equal to the one of the reference robot frame)
    tf::Stamped<tf::Transform> w_tf_viewpoint;
    tf::poseStampedMsgToTF (view_point, w_tf_viewpoint);

    // Zenital reference pose of the robot
    tf::Transform zenital_view(tf::Transform::getIdentity());
    zenital_view.setOrigin(tf::Vector3(0, 0, dist_m));
    tf::Quaternion zenital_z_quat(tf::Quaternion::getIdentity());
    zenital_z_quat.setEuler(0.0, 0.0, -M_PI_2); // (yaw, pitch, roll) -> (Y, X, Z)
    tf::Transform zenital_z_tf(zenital_z_quat);
    tf::Quaternion zenital_x_quat(tf::Quaternion::getIdentity());
    zenital_x_quat.setEuler(0.0, -M_PI, 0.0); // (yaw, pitch, roll) -> (Y, X, Z)
    tf::Transform zenital_x_tf(zenital_x_quat);
    tf::Stamped<tf::Transform> w_tf_zenital (zenital_view * zenital_z_tf * zenital_x_tf, ros::Time::now(), w_tf_viewpoint.frame_id_); 

    // Requested pose
    tf::Quaternion nextview_y_quat(tf::Quaternion::getIdentity());
    nextview_y_quat.setEuler(rad_y_rot, 0.0, 0.0); // (yaw, pitch, roll) -> (Y, X, Z)
    tf::Transform nextview_y_tf(nextview_y_quat);
    tf::Quaternion nextview_x_quat(tf::Quaternion::getIdentity());
    nextview_x_quat.setEuler(0.0, rad_x_rot, 0.0); // (yaw, pitch, roll) -> (Y, X, Z)
    tf::Transform nextview_x_tf(nextview_x_quat);
    tf::Stamped<tf::Transform> w_tf_nbv ( w_tf_viewpoint * nextview_y_tf * nextview_x_tf * w_tf_zenital , ros::Time::now(), w_tf_viewpoint.frame_id_); 
    tf::poseStampedTFToMsg( w_tf_nbv, *camera_pose_);

    return true;
}
    
geometry_msgs::PoseStamped::Ptr ZyonzCameraPoseAroundPointAlgorithm::get_camera_pose() const
{
    return camera_pose_;
}