tfCallback

tf_callback.cpp

/*
A call back function for retriving ROS Transformation. The code hasn't been tested yet. 
*/

template <typename T>
void DynamicMotionPrimitives<T>::tfCallback(const tf2_msgs::TFMessage::Ptr & msg)
{
  //geometry_msgs::TransformStamped world_to_uav;
  bool found_transform = false;

  // Check if TF message is for world->uav/imu
  // This is output by dynamics node.
  for (auto transform : msg->transforms){
    if (transform.child_frame_id == "uav/imu"){
      //world_to_uav = transform;
      found_transform = true;
    }
  }

  // Skip if do not have transform
  if (!found_transform) return;

  // Skip every other transform to get an update rate of 60hz
  if (numSimulationStepsSinceLastRender_ >= kNumSimulationStepsBeforeRenderRequest_){

    // Get transform for left camera
    geometry_msgs::TransformStamped camLeftTransform;

    try{
      camLeftTransform = tfBuffer_.lookupTransform("world", "uav/imu", ros::Time(0));
    } catch (tf2::TransformException &ex) {
      ROS_WARN("Could NOT find transform for /uav/camera/0/internal_nwu: %s", ex.what());
    }
    Eigen::Quaternion<double> orientation;
    geometry_msgs::Quaternion tf_quat;
    tf_quat = camLeftTransform.transform.rotation;
    orientation.x() = tf_quat.x;
    orientation.y() = tf_quat.y;
    orientation.z() = tf_quat.z;
    orientation.w() = tf_quat.w;

    Eigen::Vector3d position = Eigen::Vector3d::Zero();
    geometry_msgs::Vector3 tf_vec = camLeftTransform.transform.translation;
    position.x() = tf_vec.x;
    position.y() = tf_vec.y;
    position.z() = tf_vec.z;

    // ros::Time timestamp = camLeftTransform.header.stamp;

    // estimator_.feedPOSEQueue(position, orientation, timestamp);
    numSimulationStepsSinceLastRender_ = 0;

  } else {
    numSimulationStepsSinceLastRender_++;
  }

}