From 7e6c1b7234e90e74ce30b9e694eed9713c7f2888 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 09:09:47 +0100 Subject: [PATCH 01/12] Porting tf2_bullet to ros2 --- tf2_bullet/AMENT_IGNORE | 0 tf2_bullet/CMakeLists.txt | 32 ++++++++++++++-------- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 24 ++++++++-------- tf2_bullet/test/test_tf2_bullet.cpp | 2 +- 4 files changed, 33 insertions(+), 25 deletions(-) delete mode 100644 tf2_bullet/AMENT_IGNORE diff --git a/tf2_bullet/AMENT_IGNORE b/tf2_bullet/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/tf2_bullet/CMakeLists.txt b/tf2_bullet/CMakeLists.txt index 23f1294b9..1c77f6daf 100644 --- a/tf2_bullet/CMakeLists.txt +++ b/tf2_bullet/CMakeLists.txt @@ -6,19 +6,27 @@ find_package(PkgConfig REQUIRED) set(bullet_FOUND 0) pkg_check_modules(bullet REQUIRED bullet) -find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2) +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) -include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) - -catkin_package(INCLUDE_DIRS include ${bullet_INCLUDE_DIRS}) +include_directories(include ${bullet_INCLUDE_DIRS}) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - - -if(CATKIN_ENABLE_TESTING) - -catkin_add_gtest(test_bullet test/test_tf2_bullet.cpp) -target_link_libraries(test_bullet ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - + DESTINATION include/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_bullet test/test_tf2_bullet.cpp) + ament_target_dependencies(test_bullet + tf2 + geometry_msgs + tf2_ros + ) endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_package() diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 63e88f5ac..393ef1acc 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -34,8 +34,8 @@ #include #include -#include - +#include +#include namespace tf2 { @@ -44,7 +44,7 @@ namespace tf2 * \return The transform message converted to a Bullet btTransform. */ inline -btTransform transformToBullet(const geometry_msgs::TransformStamped& t) +btTransform transformToBullet(const geometry_msgs::msg::TransformStamped& t) { return btTransform(btQuaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), @@ -60,9 +60,9 @@ btTransform transformToBullet(const geometry_msgs::TransformStamped& t) */ template <> inline - void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) + void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) { - t_out = tf2::Stamped(transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); + t_out = tf2::Stamped(transformToBullet(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); } /** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. @@ -71,10 +71,10 @@ inline * \return The vector converted to a PointStamped message. */ inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped& in) { - geometry_msgs::PointStamped msg; - msg.header.stamp = in.stamp_; + 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]; @@ -88,9 +88,9 @@ geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) * \param out The point converted to a timestamped Bullet Vector3. */ inline -void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) +void fromMsg(const geometry_msgs::msg::PointStamped& msg, tf2::Stamped& out) { - out.stamp_ = msg.header.stamp; + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); out.frame_id_ = msg.header.frame_id; out[0] = msg.point.x; out[1] = msg.point.y; @@ -106,9 +106,9 @@ void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& ou */ template <> inline - void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) + void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) { - t_out = tf2::Stamped(transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); + t_out = tf2::Stamped(transformToBullet(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); } diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index 01b1a3226..3b8a4563f 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -31,7 +31,7 @@ #include -#include +#include #include #include From 1b068dd9d1e37f3d28d23a6bc2c4a63ef1fe669e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 19:55:04 +0100 Subject: [PATCH 02/12] fixed packae.xml --- tf2_bullet/package.xml | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/tf2_bullet/package.xml b/tf2_bullet/package.xml index 489f6f3cb..1fa10da47 100644 --- a/tf2_bullet/package.xml +++ b/tf2_bullet/package.xml @@ -9,8 +9,8 @@ BSD http://www.ros.org/wiki/tf2_bullet - - catkin + + ament_cmake pkg-config tf2 @@ -21,5 +21,10 @@ bullet geometry_msgs - + bullet + + ament_cmake + + + From d45d8d5e6f95e62b2fbb9720e53007e124bad7bc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 20 Dec 2019 16:20:39 +0100 Subject: [PATCH 03/12] Fixed cmake for windows --- tf2_bullet/CMakeLists.txt | 12 ++++++------ tf2_bullet/package.xml | 11 ++++------- 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/tf2_bullet/CMakeLists.txt b/tf2_bullet/CMakeLists.txt index 1c77f6daf..0f6a22fc5 100644 --- a/tf2_bullet/CMakeLists.txt +++ b/tf2_bullet/CMakeLists.txt @@ -1,17 +1,17 @@ cmake_minimum_required(VERSION 3.5) project(tf2_bullet) -find_package(PkgConfig REQUIRED) - -set(bullet_FOUND 0) -pkg_check_modules(bullet REQUIRED bullet) - find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) -include_directories(include ${bullet_INCLUDE_DIRS}) +if(WIN32) + set(BULLET_ROOT $ENV{ChocolateyInstall}/lib/bullet) +endif() +find_package(Bullet REQUIRED) + +include_directories(include ${BULLET_INCLUDE_DIRS}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} diff --git a/tf2_bullet/package.xml b/tf2_bullet/package.xml index 1fa10da47..53fe1b6d9 100644 --- a/tf2_bullet/package.xml +++ b/tf2_bullet/package.xml @@ -1,4 +1,4 @@ - + tf2_bullet 0.9.1 @@ -11,17 +11,14 @@ http://www.ros.org/wiki/tf2_bullet ament_cmake - pkg-config tf2 bullet geometry_msgs - tf2 - bullet - geometry_msgs - - bullet + tf2 + bullet + geometry_msgs ament_cmake From 334abb46146588d1e54a84c79ca97ca196fe3cce Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 26 Feb 2020 15:48:26 +0100 Subject: [PATCH 04/12] tf2_bullet added build depend Signed-off-by: ahcorde --- tf2_bullet/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/tf2_bullet/package.xml b/tf2_bullet/package.xml index 489f6f3cb..8906f39c2 100644 --- a/tf2_bullet/package.xml +++ b/tf2_bullet/package.xml @@ -14,6 +14,7 @@ pkg-config tf2 + tf2_ros bullet geometry_msgs From 358bcb03880e19c72fe045e75ead939d612e4b79 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 26 Feb 2020 15:51:14 +0100 Subject: [PATCH 05/12] tf2_bullet alphabetize package.xml Signed-off-by: ahcorde --- tf2_bullet/package.xml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tf2_bullet/package.xml b/tf2_bullet/package.xml index 3afb818d0..c7522272e 100644 --- a/tf2_bullet/package.xml +++ b/tf2_bullet/package.xml @@ -12,14 +12,15 @@ ament_cmake - tf2 - tf2_ros bullet geometry_msgs + tf2 + tf2_ros - tf2 bullet geometry_msgs + tf2 + tf2_ros ament_cmake From f50cc45adda4705b44ea11004ec13fae25139cfc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 26 Feb 2020 18:29:34 +0100 Subject: [PATCH 06/12] Fixed licenses --- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 55 +++++++++---------- .../tf2_bullet/tf2_bullet/tf2_bullet.h | 27 +++++++++ tf2_bullet/test/test_tf2_bullet.cpp | 55 +++++++++---------- 3 files changed, 81 insertions(+), 56 deletions(-) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 393ef1acc..2aec04b50 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -1,31 +1,30 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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 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 OWNER 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. - */ +// Copyright (c) 2008, Willow Garage, Inc. 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 the Willo 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. /** \author Wim Meeussen */ diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h index 2b257ee9e..510046920 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h @@ -1,3 +1,30 @@ +// Copyright (c) 2008, Willow Garage, Inc. 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 the Willo 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. #warning This header is at the wrong path you should include #include diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index 3b8a4563f..afef797d4 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -1,31 +1,30 @@ -/* - * Copyright (c) 2008, Willow Garage, Inc. - * 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 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 OWNER 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. - */ +// Copyright (c) 2008, Willow Garage, Inc. 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 the Willo 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. /** \author Wim Meeussen */ From ba69b63176d08621f3c2ca4988765211d7bd4c03 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 26 Feb 2020 18:30:08 +0100 Subject: [PATCH 07/12] Added and fixed linters --- tf2_bullet/CMakeLists.txt | 18 ++++++ tf2_bullet/include/tf2_bullet/tf2_bullet.h | 56 +++++++++++-------- .../tf2_bullet/tf2_bullet/tf2_bullet.h | 6 ++ tf2_bullet/package.xml | 4 ++ tf2_bullet/test/test_tf2_bullet.cpp | 8 +-- 5 files changed, 66 insertions(+), 26 deletions(-) diff --git a/tf2_bullet/CMakeLists.txt b/tf2_bullet/CMakeLists.txt index 0f6a22fc5..69fd47957 100644 --- a/tf2_bullet/CMakeLists.txt +++ b/tf2_bullet/CMakeLists.txt @@ -1,6 +1,16 @@ cmake_minimum_required(VERSION 3.5) project(tf2_bullet) +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -18,6 +28,14 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_cppcheck REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cppcheck + ) + ament_lint_auto_find_test_dependencies() + ament_cppcheck(LANGUAGE c++) + find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_bullet test/test_tf2_bullet.cpp) ament_target_dependencies(test_bullet diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 2aec04b50..8ad75f469 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -28,8 +28,8 @@ /** \author Wim Meeussen */ -#ifndef TF2_BULLET_H -#define TF2_BULLET_H +#ifndef TF2_BULLET__TF2_BULLET_H_ +#define TF2_BULLET__TF2_BULLET_H_ #include #include @@ -43,12 +43,14 @@ namespace tf2 * \return The transform message converted to a Bullet btTransform. */ inline -btTransform transformToBullet(const geometry_msgs::msg::TransformStamped& t) - { - return btTransform(btQuaternion(t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), - btVector3(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); - } +btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) +{ + return btTransform( + btQuaternion( + t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + btVector3(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); +} /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 type. @@ -57,12 +59,17 @@ btTransform transformToBullet(const geometry_msgs::msg::TransformStamped& t) * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ -template <> +template<> inline - void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - t_out = tf2::Stamped(transformToBullet(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); - } +void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = + tf2::Stamped( + transformToBullet(transform) * t_in, + tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); +} /** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h @@ -70,7 +77,7 @@ inline * \return The vector converted to a PointStamped message. */ inline -geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped& in) +geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped & in) { geometry_msgs::msg::PointStamped msg; msg.header.stamp = tf2_ros::toMsg(in.stamp_); @@ -87,7 +94,7 @@ geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped& in) * \param out The point converted to a timestamped Bullet Vector3. */ inline -void fromMsg(const geometry_msgs::msg::PointStamped& msg, tf2::Stamped& out) +void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped & out) { out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); out.frame_id_ = msg.header.frame_id; @@ -103,14 +110,19 @@ void fromMsg(const geometry_msgs::msg::PointStamped& msg, tf2::Stamped +template<> inline - void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform) - { - t_out = tf2::Stamped(transformToBullet(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); - } +void doTransform( + const tf2::Stamped & t_in, tf2::Stamped & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + t_out = + tf2::Stamped( + transformToBullet(transform) * t_in, + tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); +} -} // namespace +} // namespace tf2 -#endif // TF2_BULLET_H +#endif // TF2_BULLET__TF2_BULLET_H_ diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h index 510046920..53b7297f2 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h @@ -25,6 +25,12 @@ // 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__TF2_BULLET_H_ +#define TF2_BULLET__TF2_BULLET__TF2_BULLET_H_ + #warning This header is at the wrong path you should include #include + +#endif // TF2_BULLET__TF2_BULLET__TF2_BULLET_H_ diff --git a/tf2_bullet/package.xml b/tf2_bullet/package.xml index c7522272e..d878966d0 100644 --- a/tf2_bullet/package.xml +++ b/tf2_bullet/package.xml @@ -22,6 +22,10 @@ tf2 tf2_ros + ament_cmake_gtest + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index afef797d4..65e3093af 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -39,14 +39,14 @@ static const double EPS = 1e-3; TEST(TfBullet, ConvertVector) { - btVector3 v(1,2,3); + btVector3 v(1, 2, 3); btVector3 v1 = v; tf2::convert(v1, v1); EXPECT_EQ(v, v1); - btVector3 v2(3,4,5); + btVector3 v2(3, 4, 5); tf2::convert(v1, v2); EXPECT_EQ(v, v2); @@ -54,8 +54,8 @@ TEST(TfBullet, ConvertVector) } - -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); int ret = RUN_ALL_TESTS(); From d3e6c5659df644982f951788f71ac8c1eacfb433 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 26 Feb 2020 19:32:10 +0100 Subject: [PATCH 08/12] tf2_bullet fixed uncrustify Signed-off-by: ahcorde --- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 102 ++++++++++----------- 1 file changed, 51 insertions(+), 51 deletions(-) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 8ad75f469..418a2d94a 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -42,15 +42,15 @@ namespace tf2 * \param t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to a Bullet btTransform. */ -inline -btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) -{ - return btTransform( - btQuaternion( - t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), - btVector3(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); -} + inline + btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) + { + return btTransform( + btQuaternion( + t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + btVector3(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); + } /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 type. @@ -59,49 +59,49 @@ btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ -template<> -inline -void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = - tf2::Stamped( - transformToBullet(transform) * t_in, - tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); -} + 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); + } /** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h * \param in The timestamped Bullet btVector3 to convert. * \return The vector converted to a PointStamped message. */ -inline -geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped & 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 + 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; + } /** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The PointStamped message to convert. * \param out The point converted to a timestamped Bullet Vector3. */ -inline -void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped & out) -{ - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - out[0] = msg.point.x; - out[1] = msg.point.y; - out[2] = msg.point.z; -} + 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] = msg.point.x; + out[1] = msg.point.y; + out[2] = msg.point.z; + } /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Transform data type. @@ -110,17 +110,17 @@ void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped -inline -void doTransform( - const tf2::Stamped & t_in, tf2::Stamped & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = - tf2::Stamped( - transformToBullet(transform) * t_in, - tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id); -} + 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 From 82d321786ecdb4ccc4a0fd4d46b72b2bd864eb73 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 2 Mar 2020 15:46:58 +0100 Subject: [PATCH 09/12] fixed windows compiler warnings --- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 418a2d94a..02dba83bf 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -36,6 +36,8 @@ #include #include +#include + namespace tf2 { /** \brief Convert a timestamped transform to the equivalent Bullet data type. @@ -46,10 +48,13 @@ namespace tf2 btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) { return btTransform( - btQuaternion( - t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), - btVector3(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); + btQuaternion(static_cast(t.transform.rotation.x), + static_cast(t.transform.rotation.y), + static_cast(t.transform.rotation.z), + static_cast(t.transform.rotation.w)), + btVector3(static_cast(t.transform.translation.x), + static_cast(t.transform.translation.y), + static_cast(t.transform.translation.z))); } @@ -98,9 +103,9 @@ namespace tf2 { out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); out.frame_id_ = msg.header.frame_id; - out[0] = msg.point.x; - out[1] = msg.point.y; - out[2] = msg.point.z; + out[0] = static_cast(msg.point.x); + out[1] = static_cast(msg.point.y); + out[2] = static_cast(msg.point.z); } From ef51aab7fb7ee0d8214e3374ca95e5f9c0c302de Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 2 Mar 2020 16:43:08 +0100 Subject: [PATCH 10/12] fixed windows uncrustify --- tf2_bullet/include/tf2_bullet/tf2_bullet.h | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 02dba83bf..0bcaa423b 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -48,13 +48,15 @@ namespace tf2 btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t) { return btTransform( - btQuaternion(static_cast(t.transform.rotation.x), - static_cast(t.transform.rotation.y), - static_cast(t.transform.rotation.z), - static_cast(t.transform.rotation.w)), - btVector3(static_cast(t.transform.translation.x), - static_cast(t.transform.translation.y), - static_cast(t.transform.translation.z))); + 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))); } @@ -103,9 +105,9 @@ namespace tf2 { out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); out.frame_id_ = msg.header.frame_id; - out[0] = static_cast(msg.point.x); - out[1] = static_cast(msg.point.y); - out[2] = static_cast(msg.point.z); + out[0] = static_cast < float > (msg.point.x); + out[1] = static_cast < float > (msg.point.y); + out[2] = static_cast < float > (msg.point.z); } From f9b1871b4f57c2ddc8b1e56944852a53629695c0 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 3 Mar 2020 08:21:19 +0100 Subject: [PATCH 11/12] Fixed CXX compiler --- tf2_bullet/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_bullet/CMakeLists.txt b/tf2_bullet/CMakeLists.txt index 69fd47957..8fe210460 100644 --- a/tf2_bullet/CMakeLists.txt +++ b/tf2_bullet/CMakeLists.txt @@ -2,8 +2,8 @@ cmake_minimum_required(VERSION 3.5) project(tf2_bullet) # Default to C11 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 14) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") From bf6503537225f7a33c1a012330851a87d6163ade Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 4 Mar 2020 08:45:57 +0100 Subject: [PATCH 12/12] removed unused variable tf2_bullet test --- tf2_bullet/test/test_tf2_bullet.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp index 65e3093af..08a735e51 100644 --- a/tf2_bullet/test/test_tf2_bullet.cpp +++ b/tf2_bullet/test/test_tf2_bullet.cpp @@ -34,8 +34,6 @@ #include #include -static const double EPS = 1e-3; - TEST(TfBullet, ConvertVector) {