Debug

src/nodeletserver.cc

@@ -30,48 +30,13 @@ bool nodelet_exists_service (lrs_msgs_common::NodeletExists::Request &req,
     return true;
   }
 
+  std::string nodelet_type;
+  bool res = basic_nodelet_exists(req.pkg, req.tst_type, nodelet_type);
+  response.nodelet_type = nodelet_type;
+
   std::string package = req.pkg;
   std::string name = req.tst_type;
 
-  std::string path = ros::package::getPath(package);
-
-  //  ROS_ERROR("PATH: %s", path.c_str());
-
-  std::string filename = path + "/nodelet_plugins.xml";
-
-  TiXmlDocument doc(filename);
-  doc.LoadFile();
-
-  TiXmlNode* node = &doc;
-  node = node->FirstChild(); // library level
-  TiXmlNode* child;
-
-  bool res = false;
-
-  for (child = node->FirstChild(); child != 0; child = child->NextSibling()) {
-    // child id class element
-    TiXmlAttribute* attrib = child->ToElement()->FirstAttribute();
-    while (attrib) {
-      std::string elname = std::string(attrib->Name());
-      std::string value = std::string(attrib->Value());
-      if (elname == "name") {
-        std::string suffix = value.substr(value.find("/")+1);
-        // ROS_INFO("CLASS ATTR: %s - %s - %s", attrib->Name(), attrib->Value(), suffix.c_str());
-
-        if (suffix == name) {
-          res = true;
-          response.nodelet_type = value;
-          break;
-        }
-      }
-     
-      attrib = attrib->Next();
-    }
-    if (res) {
-      break;
-    }
-  }
-
   response.success = res;
   
   return true;

src/nodeletutil.cc

@@ -36,6 +36,49 @@ bool nodelet_exists(std::string package, std::string name, std::string & nodelet
   return res;
 }
 
+bool basic_nodelet_exists(std::string package, std::string name, std::string & nodelet_name) {
+
+  std::string path = ros::package::getPath(package);
+
+  //  ROS_ERROR("PATH: %s", path.c_str());
+
+  std::string filename = path + "/nodelet_plugins.xml";
+
+  TiXmlDocument doc(filename);
+  doc.LoadFile();
+
+  TiXmlNode* node = &doc;
+  node = node->FirstChild(); // library level
+  TiXmlNode* child;
+
+  bool res = false;
+
+  for (child = node->FirstChild(); child != 0; child = child->NextSibling()) {
+    // child id class element
+    TiXmlAttribute* attrib = child->ToElement()->FirstAttribute();
+    while (attrib) {
+      std::string elname = std::string(attrib->Name());
+      std::string value = std::string(attrib->Value());
+      if (elname == "name") {
+        std::string suffix = value.substr(value.find("/")+1);
+        // ROS_INFO("CLASS ATTR: %s - %s - %s", attrib->Name(), attrib->Value(), suffix.c_str());
+
+        if (suffix == name) {
+          res = true;
+          nodelet_name = value;
+          break;
+        }
+      }
+     
+      attrib = attrib->Next();
+    }
+    if (res) {
+      break;
+    }
+  }
+  return res;
+}
+
 void load_nodelet_for_service(ros::ServiceClient client, std::string tst_type) {
   ROS_INFO("nodeletutil load_nodelet_for_service: %s", tst_type.c_str());
   if (!client.exists()) {
@@ -91,7 +134,7 @@ bool load_nodelet(std::vector<std::string> pkgs, std::string tst_type) {
 bool load_nodelet(std::string pkg, std::string tst_type) {
   ROS_ERROR("nodeletutil load_nodelet3: %s - %s", pkg.c_str(), tst_type.c_str());  
   std::string nodelet_type;
-  if (nodelet_exists(pkg, tst_type, nodelet_type)) {
+  if (basic_nodelet_exists(pkg, tst_type, nodelet_type)) {
     std::string ns = ros::names::clean (ros::this_node::getNamespace());    
     std::string nodelet_name = ns + "/" + tst_type;
     std::string manager = pkg + "_manager";

src/nodeletutil.h

@@ -7,6 +7,8 @@
 
 bool nodelet_exists(std::string package, std::string name, std::string & nodelet_name);
 
+bool basic_nodelet_exists(std::string package, std::string name, std::string & nodelet_name);
+
 bool load_nodelet(std::string tst_type);
 bool load_nodelet(std::vector<std::string> pkgs, std::string tst_type);
 bool load_nodelet(std::string pkg, std::string tst_type);