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();
}
// 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();
}
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());
}
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;
}
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;
}