Program Listing for File tf2_bullet.hpp

↰ Return to documentation for file (include/tf2_bullet/tf2_bullet.hpp)

// Copyright 2008 Willow Garage, Inc.
//
// 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 the Willow Garage, Inc. 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 TF2_BULLET__TF2_BULLET_HPP_
#define TF2_BULLET__TF2_BULLET_HPP_

#include <iostream>

#include "tf2/convert.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btTransform.h"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "tf2_ros/buffer_interface.h"

#if (BT_BULLET_VERSION <= 282)
// Suppress compilation warning on older versions of Bullet.
// TODO(mjcarroll): Remove this when all platforms have the fix upstream.
inline int bullet_btInfinityMask()
{
  return btInfinityMask;
}
#endif

namespace tf2
{
inline
btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t)
{
  return btTransform(
    btQuaternion(
      static_cast<float>(t.transform.rotation.x),
      static_cast<float>(t.transform.rotation.y),
      static_cast<float>(t.transform.rotation.z),
      static_cast<float>(t.transform.rotation.w)),
    btVector3(
      static_cast<float>(t.transform.translation.x),
      static_cast<float>(t.transform.translation.y),
      static_cast<float>(t.transform.translation.z)));
}


template< >
inline
void doTransform(
  const tf2::Stamped<btVector3> & t_in, tf2::Stamped<btVector3> & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out =
    tf2::Stamped<btVector3>(
    transformToBullet(transform) * t_in,
    tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
}

inline
geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<btVector3> & in)
{
  geometry_msgs::msg::PointStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.point.x = in[0];
  msg.point.y = in[1];
  msg.point.z = in[2];
  return msg;
}

inline
void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped<btVector3> & out)
{
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  out.frame_id_ = msg.header.frame_id;
  out[0] = static_cast<float>(msg.point.x);
  out[1] = static_cast<float>(msg.point.y);
  out[2] = static_cast<float>(msg.point.z);
}


template< >
inline
void doTransform(
  const tf2::Stamped<btTransform> & t_in, tf2::Stamped<btTransform> & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out =
    tf2::Stamped<btTransform>(
    transformToBullet(transform) * t_in,
    tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
}


}  // namespace tf2

#endif  // TF2_BULLET__TF2_BULLET_HPP_