kinton_vs_control_sim_alg_node.cpp

Go to the documentation of this file.

#include "kinton_vs_control_sim_alg_node.h"

KintonVsControlAlgNode::KintonVsControlAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<KintonVsControlAlgorithm>()
{
  //init class attributes if necessary
  this->loop_rate_ = 100;//in [Hz]

  this->quad_twist_ = Eigen::MatrixXd::Zero(6,1);
  this->cam_twist_ = Eigen::MatrixXd::Zero(6,1);

  //Get fixed transforms between the IMU data and the robot frame
  tf::TransformListener listener;
  tf::StampedTransform tf_quad_to_cam;

  //wait 5.0 secs waiting for the other nodes to start
  sleep(5.0);

  try {
    listener.waitForTransform("/base_link","/front_cam_optical_frame", ros::Time::now(), ros::Duration(1.0));
    listener.lookupTransform("/base_link","/front_cam_optical_frame", ros::Time::now(), tf_quad_to_cam);
  } 
  catch (tf::TransformException ex) {
    ROS_ERROR("Quad_to_cam Transform error: %s",ex.what());
  }

  //Homogenous transform between camera and quadrotor
  this->T_quad_to_cam_ = getTransform(tf_quad_to_cam);

  // [init publishers]
  this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  // [init subscribers]
  this->cam_vel_subscriber_ = this->public_node_handle_.subscribe("cam_vel", 100, &KintonVsControlAlgNode::cam_vel_callback, this);
  this->odom_subscriber_ = this->public_node_handle_.subscribe("odom", 100, &KintonVsControlAlgNode::odom_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

KintonVsControlAlgNode::~KintonVsControlAlgNode(void)
{
  // [free dynamic memory]
}

void KintonVsControlAlgNode::mainNodeThread(void)
{

  Eigen::MatrixXd quad_vel;
  quad_vel = cam_to_quad_vel();

  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]

  this->cmd_vel_.linear.x = quad_vel(0,0);
  this->cmd_vel_.linear.y = quad_vel(1,0); 
  this->cmd_vel_.linear.z = quad_vel(2,0); 
  // this->cmd_vel_.angular.x = quad_vel(3,0);
  // this->cmd_vel_.angular.y = quad_vel(4,0);
  this->cmd_vel_.angular.x = 0.0;
  this->cmd_vel_.angular.y = 0.0;
  this->cmd_vel_.angular.z = quad_vel(5,0);

  this->cmd_vel_publisher_.publish(this->cmd_vel_);
}

/*  [subscriber callbacks] */
void KintonVsControlAlgNode::cam_vel_callback(const geometry_msgs::TwistWithCovariance::ConstPtr& msg) 
{ 
  //ROS_INFO("KintonVsControlAlgNode::twist_cov_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->cam_vel_mutex_.enter(); 


  if (msg->covariance[0]<1000 && this->activate_)
  {
    //To drive the quadrotor, the twist on x and y will be achieved by pitch and roll movements respectively
    //thus both linear velocities should be exchanged
    this->cam_twist_(0,0) = msg->twist.linear.x;
    this->cam_twist_(1,0) = msg->twist.linear.y;
    this->cam_twist_(2,0) = msg->twist.linear.z;
    this->cam_twist_(3,0) = msg->twist.angular.x;
    this->cam_twist_(4,0) = msg->twist.angular.y;
    this->cam_twist_(5,0) = msg->twist.angular.z;
  }
  else this->cam_twist_ = Eigen::MatrixXd::Zero(6,1);

  //std::cout << msg->data << std::endl; 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->cam_vel_mutex_.exit(); 
}
void KintonVsControlAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg) 
{ 
  //ROS_INFO("KintonVsControlAlgNode::odom_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  this->odom_mutex_.enter(); 

    this->roll_ = msg->pose.pose.orientation.x;
    this->pitch_ = msg->pose.pose.orientation.y;

    this->quad_twist_(0,0) = msg->twist.twist.linear.x;
    this->quad_twist_(1,0) = msg->twist.twist.linear.y;
    this->quad_twist_(2,0) = msg->twist.twist.linear.z;
    this->quad_twist_(3,0) = msg->twist.twist.angular.x;
    this->quad_twist_(4,0) = msg->twist.twist.angular.y;
    this->quad_twist_(5,0) = msg->twist.twist.angular.z;

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  this->odom_mutex_.exit(); 
}

Eigen::MatrixXd KintonVsControlAlgNode::cam_to_quad_vel() 
{

  Eigen::MatrixXd cam_lin = Eigen::MatrixXd::Zero(3,1);
  Eigen::MatrixXd cam_ang = Eigen::MatrixXd::Zero(3,1);

  //Camera desired velocities
  this->cam_vel_mutex_.enter();
    cam_lin = this->cam_twist_.block(0,0,3,1);
    cam_ang = this->cam_twist_.block(3,0,3,1);
  this->cam_vel_mutex_.exit();

  //Transform angular velocities from cam_optical_frame to base_link
  //In a rigid body the values does not change, only frame changes
  Eigen::MatrixXd quad_ang = Eigen::MatrixXd::Zero(3,1);
  quad_ang = this->T_quad_to_cam_.block(0,0,3,3) * cam_ang;

  //In a rigid body, the angular velocities add linear velocities
  //Without considering it negligible:
  //Skew symmetric to perform cross product
  // Eigen::Matrix3d skew;
  // skew(0,0) = 0.0;
  // skew(0,1) = -quad_ang(2,0);
  // skew(0,2) = quad_ang(1,0);
  // skew(1,0) = quad_ang(2,0);
  // skew(1,1) = 0.0;
  // skew(1,2) = -quad_ang(0,0);
  // skew(2,0) = -quad_ang(1,0);
  // skew(2,1) = quad_ang(0,0);
  // skew(2,2) = 0.0;

  // Eigen::MatrixXd quad_lin = Eigen::MatrixXd::Zero(3,1);
  // quad_lin = (this->T_quad_to_cam_.block(0,0,3,3) * cam_lin) + (skew * T_quad_to_cam_.block(0,3,3,1));

  //Considering cam and quad frames witht he same origin point
  Eigen::MatrixXd quad_lin = Eigen::MatrixXd::Zero(3,1);
  quad_lin = this->T_quad_to_cam_.block(0,0,3,3) * cam_lin;


  Eigen::MatrixXd quad_vel = Eigen::MatrixXd::Zero(6,1);
  quad_vel.block(0,0,3,1) = quad_lin;
  quad_vel.block(3,0,3,1) = quad_ang;

  //Translate to Quadrotor inertial frame with quadrotor jacobian
  //Eigen::MatrixXd Jq(6,6);
  //Jq = quad_jacobian();

  //ROS_INFO_STREAM(Jq);

  //Eigen::MatrixXd vel_quad_inertial(4,1);
  //vel_quad_inertial = vel_4DOF(Jq,quad_vel);

  //quad_vel = Eigen::MatrixXd::Zero(6,1);
  //quad_vel.block(0,0,3,1) = vel_quad_inertial.block(0,0,3,1);
  //quad_vel(5,0) = vel_quad_inertial(3,0);

  return quad_vel;
}

Eigen::Matrix4d KintonVsControlAlgNode::getTransform(const tf::StampedTransform& transform)
{

  double roll,pitch,yaw;
  tf::Quaternion q =  transform.getRotation();
  tf::Matrix3x3(q).getRPY(roll,pitch,yaw); 

  Eigen::Matrix3d Rot;
  Rot = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) \
      * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) \
      * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
  Eigen::Matrix4d T = Eigen::Matrix4d::Zero();

  T.block(0,0,3,3) = Rot;
  T(0,3) = transform.getOrigin().x();
  T(1,3) = transform.getOrigin().y();
  T(2,3) = transform.getOrigin().z();
  T(3,3) = 1.0;  

  //ROS_INFO("x: %f y: %f z: %f roll: %f, pitch: %f, yaw: %f",transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z(),roll,pitch,yaw);

  return T;
}

       
/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void KintonVsControlAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->activate_=config.sim_activate;
  this->alg_.unlock();
}

void KintonVsControlAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<KintonVsControlAlgNode>(argc, argv, "kinton_vs_control_alg_node");
}