Update topic joining and includes

Signed-off-by: methylDragon <methylDragon@gmail.com>
locusrobotics · Jan 8, 2023 · 166b39a · 166b39a
1 parent b1fbcab
commit 166b39a
Show file tree

Hide file tree

Showing 6 changed files with 21 additions and 16 deletions.
diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp
@@ -166,13 +166,15 @@ std::string joinTopicName(std::string a, std::string b)
   if (b.empty()) {
     return a;
   }
+  if (b.front() == '/' || b.front() == '~') {
+    RCLCPP_WARN(
+      rclcpp::get_logger("fuse"), "Second argument to joinTopicName is absolute! Returning it.");
+    return b;
+  }
 
   if (a.back() == '/') {
     a.pop_back();
   }
-  if (b.front() == '/') {
-    b.erase(b.begin());
-  }
 
   return a + "/" + b;
 }

diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp
@@ -75,7 +75,9 @@ TEST(Util, wrapAngle2D)
     EXPECT_EQ("a/b", fuse_core::joinTopicName("a", "b"));
     EXPECT_EQ("/a/b", fuse_core::joinTopicName("/a", "b"));
     EXPECT_EQ("a/b", fuse_core::joinTopicName("a/", "b"));
-    EXPECT_EQ("a/b", fuse_core::joinTopicName("a", "/b"));
-    EXPECT_EQ("a/b", fuse_core::joinTopicName("a/", "/b"));
+    EXPECT_EQ("/b", fuse_core::joinTopicName("a", "/b"));
+    EXPECT_EQ("/b", fuse_core::joinTopicName("a/", "/b"));
+    EXPECT_EQ("~/b", fuse_core::joinTopicName("a/", "~/b"));
+    EXPECT_EQ("~b", fuse_core::joinTopicName("a/", "~b"));
   }
 }
diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp
@@ -93,9 +93,9 @@ void Path2DPublisher::onInit()
   pub_options.callback_group = cb_group_;
 
   path_publisher_ = rclcpp::create_publisher<nav_msgs::msg::Path>(
-    interfaces_, fuse_core::joinTopicName(name_, "/path"), 1, pub_options);
+    interfaces_, fuse_core::joinTopicName(name_, "path"), 1, pub_options);
   pose_array_publisher_ = rclcpp::create_publisher<geometry_msgs::msg::PoseArray>(
-    interfaces_, fuse_core::joinTopicName(name_, "/pose_array"), 1, pub_options);
+    interfaces_, fuse_core::joinTopicName(name_, "pose_array"), 1, pub_options);
 }
 
 void Path2DPublisher::notifyCallback(

diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp
@@ -195,10 +195,10 @@ void Pose2DPublisher::onInit()
   pub_options.callback_group = cb_group_;
 
   pose_publisher_ = rclcpp::create_publisher<geometry_msgs::msg::PoseStamped>(
-    interfaces_, fuse_core::joinTopicName(name_, "/pose"), 1, pub_options);
+    interfaces_, fuse_core::joinTopicName(name_, "pose"), 1, pub_options);
   pose_with_covariance_publisher_ =
     rclcpp::create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    interfaces_, fuse_core::joinTopicName(name_, "/pose_with_covariance"), 1, pub_options);
+    interfaces_, fuse_core::joinTopicName(name_, "pose_with_covariance"), 1, pub_options);
 }
 
 void Pose2DPublisher::onStart()

diff --git a/fuse_publishers/test/test_path_2d_publisher.cpp b/fuse_publishers/test/test_path_2d_publisher.cpp
@@ -32,12 +32,14 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-// Workaround ros2/geometry2#242
 #include <gtest/gtest.h>
-#include <tf2/utils.h>
 
 #include <vector>
 
+// Workaround ros2/geometry2#242
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2/utils.h>  // NOLINT(build/include_order)
+
 #include <fuse_constraints/absolute_pose_2d_stamped_constraint.hpp>
 #include <fuse_core/eigen.hpp>
 #include <fuse_core/transaction.hpp>
@@ -51,8 +53,6 @@
 #include <nav_msgs/msg/path.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>  // NOLINT(build/include_order)
-
 
 /**
  * @brief Test fixture for the Path2DPublisher

diff --git a/fuse_publishers/test/test_pose_2d_publisher.cpp b/fuse_publishers/test/test_pose_2d_publisher.cpp
@@ -32,13 +32,15 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-// Workaround ros2/geometry2#242
 #include <gtest/gtest.h>
-#include <tf2/utils.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 
 #include <vector>
 
+// Workaround ros2/geometry2#242
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2/utils.h>  // NOLINT(build/include_order)
+
 #include <fuse_constraints/absolute_pose_2d_stamped_constraint.hpp>
 #include <fuse_core/eigen.hpp>
 #include <fuse_core/transaction.hpp>
@@ -51,7 +53,6 @@
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>  // NOLINT(build/include_order)
 #include <tf2_msgs/msg/tf_message.hpp>