Skip to content

Commit e43068a

Browse files
gleichdickgleichdick
committed
[3/7] Complete rework: bullet
1 parent 110b96b commit e43068a

File tree

3 files changed

+129
-85
lines changed

3 files changed

+129
-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: 105 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -26,19 +26,19 @@
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/btQuaternion.h>
3635
#include <LinearMath/btScalar.h>
3736
#include <LinearMath/btTransform.h>
38-
#include <geometry_msgs/msg/point_stamped.hpp>
39-
#include <tf2_ros/buffer_interface.h>
37+
#include <tf2/convert.h>
38+
#include <tf2/transform_datatypes.h>
4039

41-
#include <iostream>
40+
#include <geometry_msgs/msg/point_stamped.hpp>
41+
#include <geometry_msgs/msg/vector3.hpp>
4242

4343
#if (BT_BULLET_VERSION <= 282)
4444
// Suppress compilation warning on older versions of Bullet.
@@ -51,96 +51,119 @@ inline int bullet_btInfinityMask()
5151

5252
namespace tf2
5353
{
54-
/** \brief Convert a timestamped transform to the equivalent Bullet data type.
55-
* \param t The transform to convert, as a geometry_msgs TransformedStamped message.
56-
* \return The transform message converted to a Bullet btTransform.
57-
*/
58-
inline
59-
btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t)
54+
namespace impl
6055
{
61-
return btTransform(
62-
btQuaternion(
63-
static_cast<float>(t.transform.rotation.x),
64-
static_cast<float>(t.transform.rotation.y),
65-
static_cast<float>(t.transform.rotation.z),
66-
static_cast<float>(t.transform.rotation.w)),
67-
btVector3(
68-
static_cast<float>(t.transform.translation.x),
69-
static_cast<float>(t.transform.translation.y),
70-
static_cast<float>(t.transform.translation.z)));
71-
}
56+
/// \brief Default return type of tf2::toMsg() for btVector3.
57+
template<>
58+
struct DefaultMessageForDatatype<btVector3>
59+
{
60+
/// \brief Default return type of tf2::toMsg() for btVector3.
61+
using type = geometry_msgs::msg::Point;
62+
};
7263

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

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

92-
/** \brief Convert a stamped Bullet Vector3 type to a PointStamped message.
93-
* This function is a specialization of the toMsg template defined in tf2/convert.h
94-
* \param in The timestamped Bullet btVector3 to convert.
95-
* \return The vector converted to a PointStamped message.
96-
*/
97-
inline
98-
geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<btVector3> & in)
80+
/// \brief Conversion implementation for geometry_msgs::msg::Point and btVector3.
81+
template<>
82+
struct ConversionImplementation<btVector3, geometry_msgs::msg::Point>
83+
: DefaultVectorConversionImplementation<btVector3, geometry_msgs::msg::Point, btScalar>
9984
{
100-
geometry_msgs::msg::PointStamped msg;
101-
msg.header.stamp = tf2_ros::toMsg(in.stamp_);
102-
msg.header.frame_id = in.frame_id_;
103-
msg.point.x = in[0];
104-
msg.point.y = in[1];
105-
msg.point.z = in[2];
106-
return msg;
107-
}
85+
};
10886

109-
/** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type.
110-
* This function is a specialization of the fromMsg template defined in tf2/convert.h
111-
* \param msg The PointStamped message to convert.
112-
* \param out The point converted to a timestamped Bullet Vector3.
113-
*/
114-
inline
115-
void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped<btVector3> & out)
87+
/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and btVector3.
88+
template<>
89+
struct ConversionImplementation<btVector3, geometry_msgs::msg::Vector3>
90+
: DefaultVectorConversionImplementation<btVector3, geometry_msgs::msg::Vector3, btScalar>
11691
{
117-
out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
118-
out.frame_id_ = msg.header.frame_id;
119-
out[0] = static_cast<float>(msg.point.x);
120-
out[1] = static_cast<float>(msg.point.y);
121-
out[2] = static_cast<float>(msg.point.z);
122-
}
92+
};
12393

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

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

129+
/// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
130+
template<>
131+
struct DefaultTransformType<btTransform>
132+
{
133+
/// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
134+
using type = btTransform;
135+
};
136+
137+
/// \brief Default Type for automatic tf2::doTransform() implementation for btVector3.
138+
template<>
139+
struct DefaultTransformType<btVector3>
140+
{
141+
/// \brief Default Type for automatic tf2::doTransform() implementation for btVector3.
142+
using type = btTransform;
143+
};
143144

145+
/// \brief Default Type for automatic tf2::doTransform() implementation for btQuaternion.
146+
template<>
147+
struct DefaultTransformType<btQuaternion>
148+
{
149+
/// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
150+
using type = btTransform;
151+
};
152+
} // namespace impl
153+
154+
/**
155+
* \brief Convert a timestamped transform to the equivalent Bullet data type.
156+
* \param[in] t The transform to convert, as a geometry_msgs TransformedStamped message.
157+
* \return The transform message converted to a Bullet btTransform.
158+
*/
159+
[[deprecated("Please use tf2::fromMsg()")]]
160+
inline btTransform transformToBullet(
161+
const geometry_msgs::msg::TransformStamped & t)
162+
{
163+
btTransform ans;
164+
fromMsg<>(t.transform, ans);
165+
return ans;
166+
}
144167
} // namespace tf2
145168

146169
#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)