Move definitions in tf_help to separate src cpp file (#1890)

* Move definitions of functions in tf_help to separate src cpp file to avoid multiple definition error

* Fix linting issues

* Make uncrustify happier

* More format fixing

nav2_dwb_controller/nav_2d_utils/CMakeLists.txt

@@ -43,7 +43,15 @@ ament_target_dependencies(path_ops
   ${dependencies}
 )
 
-install(TARGETS conversions path_ops
+add_library(tf_help SHARED
+  src/tf_help.cpp
+)
+
+ament_target_dependencies(tf_help
+  ${dependencies}
+)
+
+install(TARGETS conversions path_ops tf_help
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib
         RUNTIME DESTINATION bin
@@ -60,7 +68,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(conversions path_ops)
+ament_export_libraries(conversions path_ops tf_help)
 ament_export_dependencies(${dependencies})
 
 ament_package()

nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp

@@ -37,6 +37,7 @@
 
 #include <string>
 #include <memory>
+#include "tf2_ros/buffer.h"
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
@@ -55,46 +56,12 @@ namespace nav_2d_utils
  * @return True if successful transform
  */
 bool transformPose(
-  const std::shared_ptr<tf2_ros::Buffer> tf, const std::string frame,
-  const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose,
-  rclcpp::Duration & transform_tolerance)
-{
-  if (in_pose.header.frame_id == frame) {
-    out_pose = in_pose;
-    return true;
-  }
-
-  try {
-    tf->transform(in_pose, out_pose, frame);
-    return true;
-  } catch (tf2::ExtrapolationException & ex) {
-    auto transform = tf->lookupTransform(frame, in_pose.header.frame_id, tf2::TimePointZero);
-    if ((rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) >
-      transform_tolerance)
-    {
-      RCLCPP_ERROR(
-        rclcpp::get_logger("tf_help"),
-        "Transform data too old when converting from %s to %s",
-        in_pose.header.frame_id.c_str(),
-        frame.c_str());
-      RCLCPP_ERROR(
-        rclcpp::get_logger("tf_help"),
-        "Data time: %ds %uns, Transform time: %ds %uns",
-        in_pose.header.stamp.sec, in_pose.header.stamp.nanosec,
-        transform.header.stamp.sec, transform.header.stamp.nanosec);
-      return false;
-    } else {
-      tf2::doTransform(in_pose, out_pose, transform);
-      return true;
-    }
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("tf_help"),
-      "Exception in transformPose: %s", ex.what());
-    return false;
-  }
-  return false;
-}
+  const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::string frame,
+  const geometry_msgs::msg::PoseStamped & in_pose,
+  geometry_msgs::msg::PoseStamped & out_pose,
+  rclcpp::Duration & transform_tolerance
+);
 
 /**
  * @brief Transform a Pose2DStamped from one frame to another while catching exceptions
@@ -107,19 +74,12 @@ bool transformPose(
  * @return True if successful transform
  */
 bool transformPose(
-  const std::shared_ptr<tf2_ros::Buffer> tf, const std::string frame,
-  const nav_2d_msgs::msg::Pose2DStamped & in_pose, nav_2d_msgs::msg::Pose2DStamped & out_pose,
-  rclcpp::Duration & transform_tolerance)
-{
-  geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
-  geometry_msgs::msg::PoseStamped out_3d_pose;
-
-  bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance);
-  if (ret) {
-    out_pose = poseStampedToPose2D(out_3d_pose);
-  }
-  return ret;
-}
+  const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::string frame,
+  const nav_2d_msgs::msg::Pose2DStamped & in_pose,
+  nav_2d_msgs::msg::Pose2DStamped & out_pose,
+  rclcpp::Duration & transform_tolerance
+);
 
 }  // namespace nav_2d_utils
 

nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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.
+ */
+
+#include <memory>
+#include <string>
+#include "nav_2d_utils/tf_help.hpp"
+
+namespace nav_2d_utils
+{
+
+bool transformPose(
+  const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::string frame,
+  const geometry_msgs::msg::PoseStamped & in_pose,
+  geometry_msgs::msg::PoseStamped & out_pose,
+  rclcpp::Duration & transform_tolerance
+)
+{
+  if (in_pose.header.frame_id == frame) {
+    out_pose = in_pose;
+    return true;
+  }
+
+  try {
+    tf->transform(in_pose, out_pose, frame);
+    return true;
+  } catch (tf2::ExtrapolationException & ex) {
+    auto transform = tf->lookupTransform(
+      frame,
+      in_pose.header.frame_id,
+      tf2::TimePointZero
+    );
+    if (
+      (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) >
+      transform_tolerance)
+    {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("tf_help"),
+        "Transform data too old when converting from %s to %s",
+        in_pose.header.frame_id.c_str(),
+        frame.c_str()
+      );
+      RCLCPP_ERROR(
+        rclcpp::get_logger("tf_help"),
+        "Data time: %ds %uns, Transform time: %ds %uns",
+        in_pose.header.stamp.sec,
+        in_pose.header.stamp.nanosec,
+        transform.header.stamp.sec,
+        transform.header.stamp.nanosec
+      );
+      return false;
+    } else {
+      tf2::doTransform(in_pose, out_pose, transform);
+      return true;
+    }
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("tf_help"),
+      "Exception in transformPose: %s",
+      ex.what()
+    );
+    return false;
+  }
+  return false;
+}
+
+bool transformPose(
+  const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::string frame,
+  const nav_2d_msgs::msg::Pose2DStamped & in_pose,
+  nav_2d_msgs::msg::Pose2DStamped & out_pose,
+  rclcpp::Duration & transform_tolerance
+)
+{
+  geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
+  geometry_msgs::msg::PoseStamped out_3d_pose;
+
+  bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance);
+  if (ret) {
+    out_pose = poseStampedToPose2D(out_3d_pose);
+  }
+  return ret;
+}
+}  // namespace nav_2d_utils