StaticTransformsUrdf.h

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.
 */
 
#ifndef StaticTransformsUrdf_H
#define StaticTransformsUrdf_H
 
// ROS
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include "urdf/model.h"
 
// Workspace
#include "holistic_fusion/config/StaticTransforms.h"
#include "holistic_fusion_ros/extrinsics/ElementToRoot.h"
 
namespace holistic_fusion {
 
class StaticTransformsUrdf : public StaticTransforms {
 public:
  StaticTransformsUrdf(const std::shared_ptr<ros::NodeHandle> privateNodePtr);
  void setup();
 
 protected:
  // Names
  std::string urdfDescriptionName_;
 
  // Robot Models
  urdf::Model urdfModel_;
  std::unique_ptr<urdf::Model> model_;
  KDL::Tree tree_;

  std::map<std::string, ElementToRoot> segments_;
 
  // Methods
  void getRootTransformations(const KDL::SegmentMap::const_iterator element, std::string rootName = "");
  tf::Transform kdlToTransform(const KDL::Frame& k);
 
  // ROS
  ros::NodeHandle privateNode_;
 
 private:
  virtual bool findTransformations() = 0;
};
}  // namespace holistic_fusion
#endif  // end StaticTransformsUrdf_H