Skip to content

Commit 696cc38

Browse files
gleichdickgleichdick
committed
[3/7] Complete rework: bullet
1 parent e61d385 commit 696cc38

File tree

3 files changed

+126
-85
lines changed

3 files changed

+126
-85
lines changed

‎tf2_bullet/CMakeLists.txt‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -28,13 +28,16 @@ install(DIRECTORY include/${PROJECT_NAME}/
2828
)
2929

3030
if(BUILD_TESTING)
31-
find_package(ament_lint_auto REQUIRED)
32-
find_package(ament_cmake_cppcheck REQUIRED)
3331
list(APPEND AMENT_LINT_AUTO_EXCLUDE
3432
ament_cmake_cppcheck
33+
ament_cmake_uncrustify
3534
)
35+
find_package(ament_lint_auto REQUIRED)
36+
find_package(ament_cmake_uncrustify REQUIRED)
37+
find_package(ament_cmake_cppcheck REQUIRED)
3638
ament_lint_auto_find_test_dependencies()
37-
ament_cppcheck(LANGUAGE c++)
39+
ament_cppcheck(LANGUAGE "c++")
40+
ament_uncrustify(LANGUAGE "c++")
3841

3942
find_package(ament_cmake_gtest REQUIRED)
4043
ament_add_gtest(test_bullet test/test_tf2_bullet.cpp)

‎tf2_bullet/include/tf2_bullet/tf2_bullet.hpp‎

Lines changed: 102 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -26,18 +26,18 @@
2626
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
2727
// POSSIBILITY OF SUCH DAMAGE.
2828

29-
/** \author Wim Meeussen */
29+
/** \author Wim Meeussen, Bjarne von Horn */
3030

3131
#ifndef TF2_BULLET__TF2_BULLET_HPP_
3232
#define TF2_BULLET__TF2_BULLET_HPP_
3333

34-
#include <tf2/convert.h>
3534
#include <LinearMath/btScalar.h>
3635
#include <LinearMath/btTransform.h>
37-
#include <geometry_msgs/msg/point_stamped.hpp>
38-
#include <tf2_ros/buffer_interface.h>
36+
#include <tf2/convert.h>
37+
#include <tf2/transform_datatypes.h>
3938

40-
#include <iostream>
39+
#include <geometry_msgs/msg/point_stamped.hpp>
40+
#include <geometry_msgs/msg/vector3.hpp>
4141

4242
#if (BT_BULLET_VERSION <= 282)
4343
// Suppress compilation warning on older versions of Bullet.
@@ -50,96 +50,116 @@ inline int bullet_btInfinityMask()
5050

5151
namespace tf2
5252
{
53-
/** \brief Convert a timestamped transform to the equivalent Bullet data type.
54-
* \param t The transform to convert, as a geometry_msgs TransformedStamped message.
55-
* \return The transform message converted to a Bullet btTransform.
56-
*/
57-
inline
58-
btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t)
53+
namespace impl
5954
{
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-
}
55+
/// \brief Default return type of tf2::toMsg() for btVector3.
56+
template<>
57+
struct DefaultMessageForDatatype<btVector3>
58+
{
59+
/// \brief Default return type of tf2::toMsg() for btVector3.
60+
using type = geometry_msgs::msg::Point;
61+
};
7162

63+
/// \brief Default return type of tf2::toMsg() for btQuaternion.
64+
template<>
65+
struct DefaultMessageForDatatype<btQuaternion>
66+
{
67+
/// \brief Default return type of tf2::toMsg() for btQuaternion.
68+
using type = geometry_msgs::msg::Quaternion;
69+
};
7270

73-
/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 type.
74-
* This function is a specialization of the doTransform template defined in tf2/convert.h
75-
* \param t_in The vector to transform, as a timestamped Bullet btVector3 data type.
76-
* \param t_out The transformed vector, as a timestamped Bullet btVector3 data type.
77-
* \param transform The timestamped transform to apply, as a TransformStamped message.
78-
*/
79-
template< >
80-
inline
81-
void doTransform(
82-
const tf2::Stamped<btVector3> & t_in, tf2::Stamped<btVector3> & t_out,
83-
const geometry_msgs::msg::TransformStamped & transform)
71+
/// \brief Default return type of tf2::toMsg() for btTransform.
72+
template<>
73+
struct DefaultMessageForDatatype<btTransform>
8474
{
85-
t_out =
86-
tf2::Stamped<btVector3>(
87-
transformToBullet(transform) * t_in,
88-
tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
89-
}
75+
/// \brief Default return type of tf2::toMsg() for btTransform.
76+
using type = geometry_msgs::msg::Transform;
77+
};
9078

91-
/** \brief Convert a stamped Bullet Vector3 type to a PointStamped message.
92-
* This function is a specialization of the toMsg template defined in tf2/convert.h
93-
* \param in The timestamped Bullet btVector3 to convert.
94-
* \return The vector converted to a PointStamped message.
95-
*/
96-
inline
97-
geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<btVector3> & in)
79+
/// \brief Conversion implementation for geometry_msgs::msg::Point and btVector3.
80+
template<>
81+
struct ConversionImplementation<btVector3, geometry_msgs::msg::Point>
82+
: DefaultVectorConversionImplementation<btVector3, geometry_msgs::msg::Point, btScalar>
9883
{
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-
}
84+
};
10785

108-
/** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type.
109-
* This function is a specialization of the fromMsg template defined in tf2/convert.h
110-
* \param msg The PointStamped message to convert.
111-
* \param out The point converted to a timestamped Bullet Vector3.
112-
*/
113-
inline
114-
void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped<btVector3> & out)
86+
/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and btVector3.
87+
template<>
88+
struct ConversionImplementation<btVector3, geometry_msgs::msg::Vector3>
89+
: DefaultVectorConversionImplementation<btVector3, geometry_msgs::msg::Vector3, btScalar>
11590
{
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-
}
91+
};
12292

93+
/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond.
94+
template<>
95+
struct ConversionImplementation<btQuaternion, geometry_msgs::msg::Quaternion>
96+
: DefaultQuaternionConversionImplementation<btQuaternion, btScalar> {};
12397

124-
/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Transform data type.
125-
* This function is a specialization of the doTransform template defined in tf2/convert.h
126-
* \param t_in The frame to transform, as a timestamped Bullet btTransform.
127-
* \param t_out The transformed frame, as a timestamped Bullet btTransform.
128-
* \param transform The timestamped transform to apply, as a TransformStamped message.
129-
*/
130-
template< >
131-
inline
132-
void doTransform(
133-
const tf2::Stamped<btTransform> & t_in, tf2::Stamped<btTransform> & t_out,
134-
const geometry_msgs::msg::TransformStamped & transform)
98+
/// \brief Conversion implementation for geometry_msgs::msg::Transform and btTransform.
99+
template<>
100+
struct ConversionImplementation<btTransform, geometry_msgs::msg::Transform>
135101
{
136-
t_out =
137-
tf2::Stamped<btTransform>(
138-
transformToBullet(transform) * t_in,
139-
tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
140-
}
102+
/** \brief Convert a Transform message to a btTransform type.
103+
* \param[in] in The message to convert.
104+
* \param[out] out The converted message, as a btTransform type.
105+
*/
106+
static void fromMsg(geometry_msgs::msg::Transform const & in, btTransform & out)
107+
{
108+
btVector3 trans;
109+
btQuaternion rot;
110+
tf2::fromMsg<>(in.rotation, rot);
111+
tf2::fromMsg<>(in.translation, trans);
112+
out = btTransform(rot, trans);
113+
}
114+
115+
/** \brief Convert a btTransform to a Transform message.
116+
* \param[in] in The btTransform to convert.
117+
* \param[out] out The converted Transform, as a message.
118+
*/
119+
static void toMsg(btTransform const & in, geometry_msgs::msg::Transform & out)
120+
{
121+
tf2::toMsg<>(in.getRotation(), out.rotation);
122+
tf2::toMsg<>(in.getOrigin(), out.translation);
123+
}
124+
};
125+
126+
/// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
127+
template<>
128+
struct DefaultTransformType<btTransform>
129+
{
130+
/// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
131+
using type = btTransform;
132+
};
133+
134+
/// \brief Default Type for automatic tf2::doTransform() implementation for btVector3.
135+
template<>
136+
struct DefaultTransformType<btVector3>
137+
{
138+
/// \brief Default Type for automatic tf2::doTransform() implementation for btVector3.
139+
using type = btTransform;
140+
};
141141

142+
/// \brief Default Type for automatic tf2::doTransform() implementation for btQuaternion.
143+
template<>
144+
struct DefaultTransformType<btQuaternion>
145+
{
146+
/// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
147+
using type = btTransform;
148+
};
149+
} // namespace impl
142150

151+
/** \brief Convert a timestamped transform to the equivalent Bullet data type.
152+
* \param[in] t The transform to convert, as a geometry_msgs TransformedStamped message.
153+
* \return The transform message converted to a Bullet btTransform.
154+
*/
155+
[[deprecated("Please use tf2::fromMsg()")]]
156+
inline btTransform transformToBullet(
157+
const geometry_msgs::msg::TransformStamped & t)
158+
{
159+
btTransform ans;
160+
fromMsg<>(t.transform, ans);
161+
return ans;
162+
}
143163
} // namespace tf2
144164

145165
#endif // TF2_BULLET__TF2_BULLET_HPP_

‎tf2_bullet/test/test_tf2_bullet.cpp‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,24 @@ TEST(TfBullet, ConvertVector)
5151
EXPECT_EQ(v1, v2);
5252
}
5353

54+
TEST(TfBullet, ConvertTransform)
55+
{
56+
geometry_msgs::msg::Transform transform;
57+
transform.rotation.x = 1.0;
58+
transform.rotation.w = 0.0;
59+
transform.translation.z = 2.0;
60+
61+
btTransform transform_bt;
62+
tf2::convert(transform, transform_bt);
63+
EXPECT_EQ(transform_bt.getRotation(), btQuaternion(1, 0, 0, 0));
64+
// vector has 4 entries, set last one to 0 to make comparsion more stable
65+
transform_bt.getOrigin().setW(0);
66+
EXPECT_EQ(transform_bt.getOrigin(), btVector3(0, 0, 2));
67+
68+
geometry_msgs::msg::Transform transform2;
69+
tf2::convert(transform_bt, transform2);
70+
EXPECT_EQ(transform2, transform);
71+
}
5472

5573
int main(int argc, char ** argv)
5674
{

0 commit comments

Comments
 (0)