tf2_bullet  master
Bullet binding for tf2.
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator
tf2_bullet.h
Go to the documentation of this file.
1 // Copyright (c) 2008, Willow Garage, Inc. All rights reserved.
2 //
3 // Redistribution and use in source and binary forms, with or without
4 // modification, are permitted provided that the following conditions are met:
5 //
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 //
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 //
13 // * Neither the name of the Willo Garage, Inc nor the names of its
14 // contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 
31 #ifndef TF2_BULLET__TF2_BULLET_H_
32 #define TF2_BULLET__TF2_BULLET_H_
33 
34 #include <tf2/convert.h>
35 #include <LinearMath/btScalar.h>
36 #include <LinearMath/btTransform.h>
37 #include <geometry_msgs/msg/point_stamped.hpp>
39 
40 #include <iostream>
41 
42 #if (BT_BULLET_VERSION <= 282)
43 // Suppress compilation warning on older versions of Bullet.
44 // TODO(mjcarroll): Remove this when all platforms have the fix upstream.
46 {
47  return btInfinityMask;
48 }
49 #endif
50 
51 namespace tf2
52 {
57  inline
58  btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t)
59  {
60  return btTransform(
61  btQuaternion(
62  static_cast < float > (t.transform.rotation.x),
63  static_cast < float > (t.transform.rotation.y),
64  static_cast < float > (t.transform.rotation.z),
65  static_cast < float > (t.transform.rotation.w)),
66  btVector3(
67  static_cast < float > (t.transform.translation.x),
68  static_cast < float > (t.transform.translation.y),
69  static_cast < float > (t.transform.translation.z)));
70  }
71 
72 
79  template < >
80  inline
83  const geometry_msgs::msg::TransformStamped & transform)
84  {
85  t_out =
87  transformToBullet(transform) * t_in,
88  tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
89  }
90 
96  inline
97  geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped < btVector3 > & in)
98  {
99  geometry_msgs::msg::PointStamped msg;
100  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
101  msg.header.frame_id = in.frame_id_;
102  msg.point.x = in[0];
103  msg.point.y = in[1];
104  msg.point.z = in[2];
105  return msg;
106  }
107 
113  inline
114  void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped < btVector3 > & out)
115  {
116  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
117  out.frame_id_ = msg.header.frame_id;
118  out[0] = static_cast < float > (msg.point.x);
119  out[1] = static_cast < float > (msg.point.y);
120  out[2] = static_cast < float > (msg.point.z);
121  }
122 
123 
130  template < >
131  inline
134  const geometry_msgs::msg::TransformStamped & transform)
135  {
136  t_out =
138  transformToBullet(transform) * t_in,
139  tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
140  }
141 
142 
143 } // namespace tf2
144 
145 #endif // TF2_BULLET__TF2_BULLET_H_
tf2::Stamped::stamp_
TimePoint stamp_
tf2::fromMsg
void fromMsg(const A &, B &b)
tf2::transformToBullet
btTransform transformToBullet(const geometry_msgs::msg::TransformStamped &t)
Convert a timestamped transform to the equivalent Bullet data type.
Definition: tf2_bullet.h:58
tf2::Stamped
tf2::Stamped::frame_id_
std::string frame_id_
tf2_ros::fromMsg
tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time &time_msg)
tf2
tf2::toMsg
B toMsg(const A &a)
convert.h
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::msg::TransformStamped &transform)
bullet_btInfinityMask
int bullet_btInfinityMask()
Definition: tf2_bullet.h:45
tf2_ros::toMsg
builtin_interfaces::msg::Time toMsg(const tf2::TimePoint &t)
buffer_interface.h