StaticTransformsTf.cpp

Go to the documentation of this file.

/*
Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich.
All rights reserved.
This file is released under the "BSD-3-Clause License".
Please see the LICENSE file that has been included as part of this package.
 */
 
// Implementation
#include "holistic_fusion_ros/extrinsics/StaticTransformsTf.h"
 
// ROS
#include <ros/ros.h>
 
// Workspace
#include "holistic_fusion_ros/constants.h"
#include "holistic_fusion_ros/util/conversions.h"
 
namespace holistic_fusion {
 
bool StaticTransformsTf::findTransformations() {
  // Print to console --------------------------
  REGULAR_COUT << " Looking up transforms in TF-tree." << std::endl;
  REGULAR_COUT << " Transforms between the following frames are required: " << imuFrame_ << ", " << baseLinkFrame_ << std::endl;
  REGULAR_COUT << " Waiting for up to 100 seconds until they arrive..." << std::endl;
 
  // Temporary variable
  static tf::StampedTransform transform;
 
  // Look up transforms ----------------------------
  // Sleep before subscribing, otherwise sometimes dying in the beginning of rosbag
  ros::Rate rosRate(10);
  rosRate.sleep();
 
  // Imu to Base ---
  REGULAR_COUT << " Looking up transform from " << imuFrame_ << " to " << baseLinkFrame_ << std::endl;
  listener_.waitForTransform(imuFrame_, baseLinkFrame_, ros::Time(0), ros::Duration(100.0));
  listener_.lookupTransform(imuFrame_, baseLinkFrame_, ros::Time(0), transform);
  // I_Base
  holistic_fusion::tfToIsometry3(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, baseLinkFrame_));
  REGULAR_COUT << " Translation I_Base: " << rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).translation() << std::endl;
  // Base_I
  lv_T_frame1_frame2(baseLinkFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).inverse();
 
  // Wrapping up --------------------------
  REGULAR_COUT << " Transforms looked up successfully." << std::endl;
  return true;
}
 
}  // namespace holistic_fusion