From 301864fb7bb24fa577128e8763c229139c84795b Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Sun, 24 Nov 2024 14:51:31 +0100
Subject: [PATCH] cras_cpp_common: node_from_nodelet: Worked around the bug
 where remapping private topics was impossible for anonymous nodes.

Works around https://github.com/ros/ros_comm/issues/2324 .
---
 cras_cpp_common/cmake/node_from_nodelet.cpp.in | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/cras_cpp_common/cmake/node_from_nodelet.cpp.in b/cras_cpp_common/cmake/node_from_nodelet.cpp.in
index 2034df2..09e2b0c 100644
--- a/cras_cpp_common/cmake/node_from_nodelet.cpp.in
+++ b/cras_cpp_common/cmake/node_from_nodelet.cpp.in
@@ -9,6 +9,7 @@
 #include <ros/datatypes.h>
 #include <ros/duration.h>
 #include <ros/init.h>
+#include <ros/names.h>
 #include <ros/node_handle.h>
 #include <ros/this_node.h>
 #include <rosconsole/macros_generated.h>
@@ -30,6 +31,14 @@
 #include <@NODELET_INCLUDE_FILE@>
 #endif
 
+namespace ros
+{
+namespace names
+{
+extern void init(const ros::M_string& remappings);
+}
+}
+
 int main(int argc, char** argv)
 {
   // Remove the program name from argv because the nodelet handling code does not expect it
@@ -41,6 +50,11 @@ int main(int argc, char** argv)
   if (CRAS_NODE_ANONYMOUS)
     initOptions = ros::init_options::AnonymousName;
   ros::init(argc, argv, "@NODE_NAME@", initOptions);
+  // Anonymous nodes have a problem that topic remappings of type ~a:=b are resolved against the node name without the
+  // anonymous part. Fix that by running names::init() again after ros::init() finishes and the full node name is known.
+  // This was reported and a fix provided in https://github.com/ros/ros_comm/issues/2324, but the fix never landed.
+  if (CRAS_NODE_ANONYMOUS)
+    ros::names::init(ros::names::getUnresolvedRemappings());
 
   ros::NodeHandle pnh("~");
   nodelet::detail::CallbackQueueManager manager(pnh.param("num_worker_threads", @CRAS_NODE_DEFAULT_NUM_THREADS@));