From 3537f266ef12e67dadbc7b7f749ecff6ecb9ef63 Mon Sep 17 00:00:00 2001
From: Johannes Meyer <johannes@intermodalics.eu>
Date: Sat, 17 Sep 2022 11:23:02 +0200
Subject: [PATCH] Fix ROSMessage::updateMissingPkgNames() for the case that
 multiple types with the same message name are registered in different
 packages

... following the rules defined in http://wiki.ros.org/msg#Fields.
---
 src/ros_message.cpp | 33 +++++++++++++++++++++------------
 src/ros_parser.cpp  |  2 +-
 2 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/src/ros_message.cpp b/src/ros_message.cpp
index 0354977..a8fd604 100644
--- a/src/ros_message.cpp
+++ b/src/ros_message.cpp
@@ -33,6 +33,8 @@
 * *******************************************************************/
 
 
+#include <algorithm>
+
 #include <boost/algorithm/string.hpp>
 #include <boost/utility/string_ref.hpp>
 #include <boost/utility/string_ref.hpp>
@@ -76,23 +78,30 @@ ROSMessage::ROSMessage(const std::string &msg_def)
 
 void ROSMessage::updateMissingPkgNames(const std::vector<const ROSType*> &all_types)
 {
+  // From http://wiki.ros.org/msg#Fields
+  //   When embedding other Message descriptions, the type name may be relative
+  //   (e.g. "Point32") if it is in the same package; otherwise it must be the
+  //   full Message type (e.g. "std_msgs/String"). The only exception to this
+  //   rule is Header.
   for (ROSField& field: _fields)
   {
-    // if package name is missing, try to find msgName in the list of known_type
-    if( field.type().pkgName().size() == 0 )
+    // if package name is missing, try to find pkgName/msgName in the list of all_types
+    if( field.type().pkgName().size() != 0 ) continue;
+    const auto found = std::find_if(
+        all_types.begin(), all_types.end(),
+        [&](const ROSType* known_type) {
+          return ( (this->type().pkgName().compare( known_type->pkgName() ) == 0) &&
+                   (field.type().msgName().compare( known_type->msgName() ) == 0) );
+        });
+    if (found != all_types.end())
+    {
+      field._type.setPkgName( (*found)->pkgName() );
+    }
+    else if ( field.type().msgName() == "Header" )
     {
-      for (const ROSType* known_type: all_types)
-      {
-        if( field.type().msgName().compare( known_type->msgName() ) == 0  )
-        {
-          field._type.setPkgName( known_type->pkgName() );
-          break;
-        }
-      }
+      field._type.setPkgName( "std_msgs" );
     }
   }
 }
 
 } // end namespace
-
-
diff --git a/src/ros_parser.cpp b/src/ros_parser.cpp
index ce91c9c..69930c1 100644
--- a/src/ros_parser.cpp
+++ b/src/ros_parser.cpp
@@ -106,7 +106,7 @@ void Parser::registerMessage(const std::string& definition)
         next_msg = getMessageByType(field.type());
         if (next_msg == nullptr)
         {
-          throw std::runtime_error("This type was not registered ");
+          throw std::runtime_error("Type " + field.type().baseName() + " was not registered");
         }
         msg_node->addChild(next_msg);
         MessageTreeNode* new_msg_node = &(msg_node->children().back());