Program Listing for File transform.h

↰ Return to documentation for file (/tmp/ws/src/marti_common/swri_transform_util/include/swri_transform_util/transform.h)

// *****************************************************************************
//
// Copyright (c) 2014, Southwest Research Institute® (SwRI®)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************

#ifndef TRANSFORM_UTIL_TRANSFORM_H_
#define TRANSFORM_UTIL_TRANSFORM_H_

#include <boost/shared_ptr.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>
#ifdef USE_TF2_H_FILES
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

#include <rclcpp/rclcpp.hpp>

#include <tf2/transform_datatypes.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>

namespace swri_transform_util
{
  class StampInterface
  {
  public:
    virtual tf2::TimePoint GetStamp() const = 0;

    virtual void SetStamp(const tf2::TimePoint&) = 0;
  };

  class StampedTransformStampInterface : virtual public StampInterface
  {
  public:
    tf2::TimePoint GetStamp() const final
    {
      return tf2_ros::fromMsg(transform_.header.stamp);
    }

    void SetStamp(const tf2::TimePoint& time) final
    {
      transform_.header.stamp = tf2_ros::toMsg(time);
    };

    tf2::Stamped<tf2::Transform> GetStampedTransform() const
    {
      tf2::Stamped<tf2::Transform> tf;
      tf2::fromMsg(transform_, tf);
      return tf;
    }

  protected:
    geometry_msgs::msg::TransformStamped transform_;
  };

  class Tf2StampStampInterface : virtual public StampInterface
  {
  public:
    tf2::TimePoint GetStamp() const final
    {
      return stamp_;
    }

    void SetStamp(const tf2::TimePoint& time) final
    {
      stamp_ = time;
    };

  protected:
    tf2::TimePoint stamp_;
  };

  class TransformImpl : virtual public StampInterface
  {
  public:
    explicit TransformImpl(const rclcpp::Logger& logger = rclcpp::get_logger("swri_transform_util::TransformImpl")) :
      logger_(logger)
    {};
    virtual ~TransformImpl() = default;

    virtual void Transform(
      const tf2::Vector3& v_in, tf2::Vector3& v_out) const = 0;

    virtual tf2::Quaternion GetOrientation() const
    {
      tf2::Vector3 offset;
      Transform(tf2::Vector3(1, 0, 0), offset);

      tf2::Vector3 origin;
      Transform(tf2::Vector3(0, 0, 0), origin);

      tf2::Vector3 vector = offset - origin;

      // Use the "half-way quaternion method" of summing and normalizing a
      // quaternion with twice the rotation between the vector and the x-axis and
      // the zero rotation.

      tf2::Vector3 cross = tf2::Vector3(1, 0, 0).cross(vector);
      double w = vector.length() + tf2::Vector3(1, 0, 0).dot(vector);
      return tf2::Quaternion(cross.x(), cross.y(), cross.z(), w).normalized();
    }

    virtual std::shared_ptr<TransformImpl> Inverse() const = 0;

  protected:

    rclcpp::Logger logger_;
  };
  typedef std::shared_ptr<TransformImpl> TransformImplPtr;

  class Transform
  {
  public:
    Transform();

    explicit Transform(const tf2::Transform& transform);

    explicit Transform(const tf2::Stamped<tf2::Transform>& transform);

    explicit Transform(std::shared_ptr<TransformImpl> transform);

    Transform& operator=(const tf2::Transform transform);

    Transform& operator=(std::shared_ptr<TransformImpl> transform);

    tf2::Vector3 operator()(const tf2::Vector3& v) const;

    tf2::Vector3 operator*(const tf2::Vector3& v) const;

    tf2::Quaternion operator*(const tf2::Quaternion& q) const;

    tf2::Transform GetTF() const;

    Transform Inverse() const;

    tf2::Vector3 GetOrigin() const;

    tf2::Quaternion GetOrientation() const;

    tf2::TimePoint GetStamp() { return transform_->GetStamp(); }

  private:
    std::shared_ptr<TransformImpl> transform_;
  };

  class IdentityTransform : public TransformImpl, public Tf2StampStampInterface
  {
  public:
    IdentityTransform() { Tf2StampStampInterface::SetStamp(tf2::TimePointZero); }

    void Transform(const tf2::Vector3& v_in, tf2::Vector3& v_out) const override;
    TransformImplPtr Inverse() const override;
  };

  class TfTransform : public TransformImpl, public Tf2StampStampInterface
  {
  public:
    explicit TfTransform(const tf2::Transform& transform);

    explicit TfTransform(const tf2::Stamped<tf2::Transform>& transform);

    void Transform(const tf2::Vector3& v_in, tf2::Vector3& v_out) const override;

    tf2::Quaternion GetOrientation() const override;
    TransformImplPtr Inverse() const override;

  protected:
    tf2::Transform transform_;
  };
}

#endif  // TRANSFORM_UTIL_TRANSFORM_H_