rtt_ros.cpp
Go to the documentation of this file.
00001 #include <cstdlib>
00002 #include <list>
00003 #include <queue>
00004 #include <sstream>
00005 #include <set>
00006 
00007 #include <boost/filesystem.hpp>
00008 #include <boost/version.hpp>
00009 
00010 #include <libxml/parser.h>
00011 #include <libxml/xpath.h>
00012 #include <libxml/xpathInternals.h>
00013 #include <libxml/tree.h>
00014 
00015 #include <rtt/RTT.hpp>
00016 #include <rtt/internal/GlobalService.hpp>
00017 
00018 #include <rtt/deployment/ComponentLoader.hpp>
00019 #include <rtt/Logger.hpp>
00020 
00021 #include <ros/package.h>
00022 #include <rospack/rospack.h>
00023 
00024 #include <rtt_ros/rtt_ros.h>
00025 
00026 
00027 bool rtt_ros::import(const std::string& package)
00028 {
00029   RTT::Logger::In in("ROSService::import(\""+package+"\")");
00030 
00031   boost::shared_ptr<RTT::ComponentLoader> loader = RTT::ComponentLoader::Instance();
00032 
00033   // List of packages which could not be loaded
00034   std::vector<std::string> missing_packages;
00035 
00036   // Get the dependencies for a given ROS package --> deps
00037   try {
00038     // Get the package paths
00039     std::vector<std::string> ros_package_paths;
00040     rospack::Rospack rpack;
00041     rpack.setQuiet(true);
00042     rpack.getSearchPathFromEnv(ros_package_paths);
00043 
00044     RTT::log(RTT::Debug) << "Loading dependencies for ROS package \""<< package << "\" from: ";
00045     for(size_t i=0; i<ros_package_paths.size(); i++) { RTT::log(RTT::Debug) << ros_package_paths[i]; }
00046     RTT::log(RTT::Debug) << RTT::endlog();
00047 
00048     rpack.crawl(ros_package_paths,true);
00049 
00050     if ( ros_package_paths.size() == 0 ) {
00051       RTT::log(RTT::Error) << "No paths in the ROS_PACKAGE_PATH environment variable! Could not load ROS package \"" << package << "\"" << RTT::endlog();
00052       return false;
00053     }
00054 
00055     // Add all rtt_ros/plugin_depend dependencies to package names
00056     std::vector<std::string> deps_to_import;
00057     std::vector<std::string> search_paths;
00058     rpack.setQuiet(true);
00059 
00060     // Read the package.xml for this package
00061     std::queue<std::string> dep_names;
00062     dep_names.push(package);
00063     deps_to_import.push_back(package);
00064 
00065     const xmlChar * rtt_plugin_depend_xpath = xmlCharStrdup("/package/export/rtt_ros/plugin_depend/text()");
00066 
00067     while(!dep_names.empty()) 
00068     {
00069       namespace fs = boost::filesystem;
00070 
00071       // Get the next dep name, store the dep path
00072       std::string dep_name, dep_path;
00073       dep_name = dep_names.front();
00074       dep_names.pop();
00075 
00076       bool dep_found = rpack.find(dep_name,dep_path);
00077 
00078       if(!dep_found) {
00079         RTT::log(RTT::Error) << "Could not find ROS package \""<< dep_name << "\" in ROS_PACKAGE_PATH environment variable." <<RTT::endlog();
00080         continue;
00081       }
00082 
00083       // Construct the package.xml path
00084       fs::path package_xml_path = fs::path(dep_path) / "package.xml";
00085       bool is_rosbuild_package = false;
00086 
00087       // Check if package.xml file exists
00088       if(!boost::filesystem::is_regular_file( package_xml_path.string() ) ) {
00089 
00090         // Fall back to manifest.xml for rosbuild packages
00091         package_xml_path = fs::path(dep_path) / "manifest.xml";
00092         is_rosbuild_package = true;
00093 
00094         if(!boost::filesystem::is_regular_file( package_xml_path.string() ) ) {
00095           RTT::log(RTT::Error) << "No package.xml or manifest.xml file for ROS package \""<< dep_name << "\" found at "<<package_xml_path.branch_path() <<RTT::endlog();
00096           continue;
00097         }
00098       }
00099 
00100       // Add package path to the list of search paths
00101       if (is_rosbuild_package) {
00102         search_paths.push_back((fs::path(dep_path) / "lib" / "orocos").string());
00103       }
00104 
00105       // Read in package.xml/manifest.xml
00106       xmlInitParser();
00107 
00108       // libxml structures
00109       xmlDocPtr package_doc;
00110       xmlXPathContextPtr xpath_ctx;
00111       xmlXPathObjectPtr xpath_obj;
00112 
00113       // Load package.xml
00114       package_doc = xmlParseFile(package_xml_path.string().c_str());
00115       xpath_ctx = xmlXPathNewContext(package_doc);
00116 
00117       // Get the text of the rtt_ros <plugin_depend>s
00118       xpath_obj = xmlXPathEvalExpression(rtt_plugin_depend_xpath, xpath_ctx);
00119 
00120       // Iterate through the nodes
00121       if(xmlXPathNodeSetIsEmpty(xpath_obj->nodesetval)) {
00122         RTT::log(RTT::Debug) << "ROS package \""<< dep_name << "\" has no RTT plugin dependencies." <<RTT::endlog();
00123       } else {
00124         RTT::log(RTT::Debug) << "ROS package \""<< dep_name << "\" has "<<xpath_obj->nodesetval->nodeNr<<" RTT plugin dependencies." <<RTT::endlog();
00125 
00126         for(int i=0; i < xpath_obj->nodesetval->nodeNr; i++) {
00127           if(xpath_obj->nodesetval->nodeTab[i]) {
00128             std::ostringstream oss;
00129             oss << xmlNodeGetContent(xpath_obj->nodesetval->nodeTab[i]);
00130             RTT::log(RTT::Debug) << "Found dependency \""<< oss.str() << "\"" <<RTT::endlog();
00131             dep_names.push(oss.str());
00132 
00133             // Add the dep to the list of deps to import
00134             deps_to_import.push_back(oss.str());
00135           }
00136         }
00137       }
00138 
00139       xmlXPathFreeObject(xpath_obj);
00140       xmlXPathFreeContext(xpath_ctx);
00141       xmlFreeDoc(package_doc);
00142       xmlCleanupParser();
00143     }
00144 
00145     // Build path list by prepending paths from search_paths list to the RTT component path in reverse order without duplicates
00146     std::set<std::string> search_paths_seen;
00147     std::string path_list = loader->getComponentPath();
00148     for(std::vector<std::string>::reverse_iterator it = search_paths.rbegin();
00149         it != search_paths.rend();
00150         ++it)
00151     {
00152       if (search_paths_seen.count(*it)) continue;
00153       path_list = *it + ":" + path_list;
00154       search_paths_seen.insert(*it);
00155     }
00156 
00157     RTT::log(RTT::Debug) << "Attempting to load RTT plugins from "<<deps_to_import.size()<<" packages..." << RTT::endlog();
00158 
00159     // Import each package dependency and the package itself (in deps_to_import[0])
00160     for(std::vector<std::string>::reverse_iterator it = deps_to_import.rbegin();
00161         it != deps_to_import.rend();
00162         ++it)
00163     {
00164       // Check if it's already been imported
00165       if(*it == "rtt_ros" || loader->isImported(*it)) {
00166         RTT::log(RTT::Debug) << "Package dependency '"<< *it <<"' already imported." << RTT::endlog();
00167         continue;
00168       }
00169 
00170       // Import the dependency
00171       if(loader->import(*it, path_list)) {
00172         RTT::log(RTT::Debug) << "Importing Orocos components from ROS package \""<<*it<<"\" SUCCEEDED.";
00173       } else {
00174         // Temporarily store the name of the missing package
00175         missing_packages.push_back(*it);
00176         RTT::log(RTT::Debug) << "Importing Orocos components from ROS package \""<<*it<<"\" FAILED.";
00177       }
00178       RTT::log(RTT::Debug) << RTT::endlog();
00179     }
00180 
00181   } catch(std::string arg) {
00182     RTT::log(RTT::Debug) << "While processing the dependencies of " << package << ": Dependency is not a ros package: " << arg << RTT::endlog();
00183     missing_packages.push_back(arg);
00184   }
00185 
00186   // Report success or failure
00187   if(missing_packages.size() == 0) { 
00188     RTT::log(RTT::Info) << "Loaded plugins from ROS package \"" << package << "\" and its dependencies." << RTT::endlog();
00189   } else {
00190     RTT::log(RTT::Warning) << "Could not load RTT plugins from the following ROS packages (they might be empty, in which case this message can be ignored): "<< RTT::endlog();
00191     for(std::vector<std::string>::iterator it = missing_packages.begin();
00192         it != missing_packages.end();
00193         ++it)
00194     {
00195       RTT::log(RTT::Warning) << " - " << *it<< RTT::endlog();
00196     }
00197   }
00198 
00199   return missing_packages.size() == 0;
00200 }


rtt_ros
Author(s): Ruben Smits
autogenerated on Thu Jun 6 2019 18:05:40