Beispiel #1
0
void TFDisplay::updateFrames()
{
  typedef std::vector<std::string> V_string;
  V_string frames;
  tf_->getFrameStrings( frames );

  S_FrameInfo current_frames;

  {
    V_string::iterator it = frames.begin();
    V_string::iterator end = frames.end();
    for ( ; it != end; ++it )
    {
      const std::string& frame = *it;

      if ( frame.empty() )
      {
        continue;
      }

      FrameInfo* info = getFrameInfo( frame );
      if (!info)
      {
        info = createFrame(frame);
      }
      else
      {
        updateFrame(info);
      }

      current_frames.insert( info );
    }
  }

  {
    S_FrameInfo to_delete;
    M_FrameInfo::iterator frame_it = frames_.begin();
    M_FrameInfo::iterator frame_end = frames_.end();
    for ( ; frame_it != frame_end; ++frame_it )
    {
      if ( current_frames.find( frame_it->second ) == current_frames.end() )
      {
        to_delete.insert( frame_it->second );
      }
    }

    S_FrameInfo::iterator delete_it = to_delete.begin();
    S_FrameInfo::iterator delete_end = to_delete.end();
    for ( ; delete_it != delete_end; ++delete_it )
    {
      deleteFrame( *delete_it );
    }
  }

  causeRender();
}
Beispiel #2
0
// This should be folded into RenderSystem, but it should work fine as
// is, so I'm leaving it for now.
void initializeResources( const V_string& resource_paths )
{
  V_string::const_iterator path_it = resource_paths.begin();
  V_string::const_iterator path_end = resource_paths.end();
  for( ; path_it != path_end; ++path_it )
  {
    Ogre::ResourceGroupManager::getSingleton().addResourceLocation( *path_it, "FileSystem", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
  }

  Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
}
Beispiel #3
0
TEST(RoscppHandles, publisherMultiple)
{
  ros::NodeHandle n;

  g_recv_count = 0;

  {
    ros::Publisher pub1 = n.advertise<test_roscpp::TestArray>("/test", 0);

    {
      ros::Publisher pub2 = n.advertise<test_roscpp::TestArray>("/test", 0);

      ASSERT_TRUE(pub1 != pub2);

      V_string topics;
      this_node::getAdvertisedTopics(topics);
      ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
    }

    V_string topics;
    this_node::getAdvertisedTopics(topics);
    ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
  }

  V_string topics;
  this_node::getAdvertisedTopics(topics);
  ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") == topics.end());
}
Beispiel #4
0
TEST(RoscppHandles, subscriberCopy)
{
  ros::NodeHandle n;

  g_recv_count = 0;

  {
    ros::Subscriber sub1 = n.subscribe("/test", 0, subscriberCallback);

    {
      ros::Subscriber sub2 = sub1;

      {
        ros::Subscriber sub3(sub2);

        ASSERT_TRUE(sub3 == sub2);

        V_string topics;
        this_node::getSubscribedTopics(topics);
        ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
      }

      ASSERT_TRUE(sub2 == sub1);

      V_string topics;
      this_node::getSubscribedTopics(topics);
      ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
    }

    V_string topics;
    this_node::getSubscribedTopics(topics);
    ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
  }

  V_string topics;
  this_node::getSubscribedTopics(topics);
  ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") == topics.end());
}
// this is the smart import function that tries to guess where 'package' lives in path_list or
// the search path.
bool ComponentLoader::import( std::string const& package, std::string const& path_list )
{
    // check first for exact match to *file*:
    path arg( package );
    if (is_regular_file(arg)) {
#if BOOST_VERSION >= 104600
	    return loadInProcess(arg.string(), makeShortFilename( arg.filename().string() ), true);
#else
	    return loadInProcess(arg.string(), makeShortFilename( arg.filename() ), true);
#endif
    }

    // check for absolute path:
    if ( arg.is_complete() ) {
        // plain import
        bool ret = import(package);
        // if not yet given, test for target subdir:
        if ( arg.parent_path().leaf() != OROCOS_TARGET_NAME )
            ret = import( (arg / OROCOS_TARGET_NAME).string() ) || ret;
        // if something found, return true:
        if (ret)
            return true;
        // both failed:
        log(Error) << "Could not import absolute path '"<<package << "': nothing found."<<endlog();
        return false;
    }

    if ( isImported(package) ) {
        log(Info) <<"Component package '"<< package <<"' already imported." <<endlog();
        return true;
    }

    // from here on: it's a package name or a path.
    // package names must be found to return true. 
    // path will be scanned and will always return true, but will warn when invalid.
    // a package name 

    // check for rospack
#ifdef HAS_ROSLIB
    using namespace ros::package;
    try {
        bool all_good = true, found = false;
        string ppath = getPath( package );
        if ( !ppath.empty() ) {
            path rospath = path(ppath) / "lib" / "orocos";
            path rospath_target = rospath / OROCOS_TARGET_NAME;
            // + add all dependencies to paths:
            V_string rospackresult;
            command("depends " + package, rospackresult);
            for(V_string::iterator it = rospackresult.begin(); it != rospackresult.end(); ++it) {
                if ( isImported(*it) ) {
                    log(Debug) <<"Package dependency '"<< *it <<"' already imported." <<endlog();
                    continue;
                }
                ppath = getPath( *it );
                path deppath = path(ppath) / "lib" / "orocos";
                path deppath_target = path(deppath) / OROCOS_TARGET_NAME;
                // if orocos directory exists and we could import it, mark it as loaded.
                if ( is_directory( deppath_target ) ) {
                    log(Debug) << "Ignoring files under " << deppath.string() << " since " << deppath_target.string() << " was found."<<endlog();
                    found = true;
                    if ( import( deppath_target.string() ) ) {
                        loadedPackages.push_back( *it );
                    } else
                        all_good = false;
                }
                else if ( is_directory( deppath ) ) {
                    found = true;
                    if ( import( deppath.string() ) ) {
                        loadedPackages.push_back( *it );
                    } else
                        all_good = false;
                }
            }
            // now that all deps are done, import the package itself:
            if ( is_directory( rospath_target ) ) {
                log(Debug) << "Ignoring files under " << rospath.string() << " since " << rospath_target.string() << " was found."<<endlog();
                found = true;
                if ( import( rospath_target.string() ) ) {
                    loadedPackages.push_back( package );
                } else
                    all_good = false;
            } else if ( is_directory( rospath ) ) {
                found = true;
                if ( import( rospath.string() ) ) {
                    loadedPackages.push_back( package );
                } else
                    all_good = false;
            }
            // since it was a ROS package, we exit here.
            if (!found) {
                log(Error) <<"The ROS package '"<< package <<"' in '"<< ppath << "' nor its dependencies contained a lib/orocos directory."<<endlog();
            }
            return all_good && found;
        } else
            log(Info) << "Not a ros package: " << package << endlog();
    } catch(...) {
        log(Info) << "Not a ros package: " << package << endlog();
    }
#endif

    string paths;
    string trypaths;
    vector<string> tryouts;
    if (path_list.empty())
        paths = component_path + default_delimiter + ".";
    else
        paths = path_list;

    bool path_found = false;

    // if ros did not find anything, split the paths above.
    // set vpaths from (default) search paths.
    vector<string> vpaths;
    vpaths = splitPaths(paths);
    trypaths = paths; // store for error reporting below.
    paths.clear();
    // Detect absolute/relative import:
    path p( package );
    if (is_directory( p )) {
        path_found = true;
        // search in dir + dir/TARGET
        paths += p.string() + default_delimiter + (p / OROCOS_TARGET_NAME).string() + default_delimiter;
        if ( p.is_complete() ) {
            // 2.2.x: path may be absolute or relative to search path.
            //log(Warning) << "You supplied an absolute directory to the import directive. Use 'path' to set absolute directories and 'import' only for packages (sub directories)."<<endlog();
            //log(Warning) << "Please modify your XML file or script. I'm importing it now for the sake of backwards compatibility."<<endlog();
        } // else: we allow to import a subdirectory of '.'.
    }
    // append '/package' or 'target/package' to each plugin path in order to search all of them:
    for(vector<string>::iterator it = vpaths.begin(); it != vpaths.end(); ++it) {
        p = *it;
        p = p / package;
        // we only search in existing directories:
        if (is_directory( p )) {
            path_found = true;
            paths += p.string() + default_delimiter ;
        } else
            tryouts.push_back( p.string() );
        p = *it;
        p = p / OROCOS_TARGET_NAME / package;
        // we only search in existing directories:
        if (is_directory( p )) {
            path_found = true;
            paths += p.string() + default_delimiter ;
        } else
            tryouts.push_back( p.string() );
    }
    if ( path_found )
        paths.erase( paths.size() - 1 ); // remove trailing delimiter ';'

    // when at least one directory exists:
    if (path_found) {
        if ( import(paths) ) {
            loadedPackages.push_back( package );
            return true;
        } else {
            log(Error) << "Failed to import components, types or plugins from package or directory '"<< package <<"' found in:"<< endlog();
            log(Error) << paths << endlog();
            return false;
        }
    }
    log(Error) << "No such package or directory found in search path: " << package << ". Search path is: "<< endlog();
    log(Error) << trypaths << endlog();
    for(vector<string>::iterator it=tryouts.begin(); it != tryouts.end(); ++it)
        log(Error) << *it << endlog();

#ifdef HAS_ROSLIB
    log(Error) << "Package also not found in ROS_PACKAGE_PATH paths." <<endlog();
#endif
    return false;

}
Beispiel #6
-1
bool getNodes(V_string& nodes)
{
  XmlRpc::XmlRpcValue args, result, payload;
  args[0] = this_node::getName();

  if (!execute("getSystemState", args, result, payload, true))
  {
    return false;
  }

  S_string node_set;
  for (int i = 0; i < payload.size(); ++i)
  {
    for (int j = 0; j < payload[i].size(); ++j)
    {
      XmlRpc::XmlRpcValue val = payload[i][j][1];
      for (int k = 0; k < val.size(); ++k)
      {
        std::string name = payload[i][j][1][k];
        node_set.insert(name);
      }
    }
  }

  nodes.insert(nodes.end(), node_set.begin(), node_set.end());

  return true;
}