bool urdf_traverser::helpers::getCommonParentPath(const std::string& p1, const std::string& p2, std::string& result)
{
if (p1.empty() || p2.empty())
{
ROS_ERROR("Both p1 and p2 have to be set in getCommonParentPath()");
return false;
}
boost::filesystem::path __p1(p1);
boost::filesystem::path __p2(p2);
bool correctAbsolute = __p1.is_relative() || __p2.is_relative();
boost::filesystem::path _p1(boost::filesystem::absolute(__p1));
boost::filesystem::path _p2(boost::filesystem::absolute(__p2));
boost::filesystem::path buildPath;
boost::filesystem::path::iterator it1(_p1.begin());
boost::filesystem::path::iterator it1_end(_p1.end());
boost::filesystem::path::iterator it2(_p2.begin());
boost::filesystem::path::iterator it2_end(_p2.end());
int p1Len = numDirectories(p1);
int p2Len = numDirectories(p2);
// ROS_INFO_STREAM("p1: "<<p1<<", p2: "<<p2);
// ROS_INFO_STREAM("p1: "<<p1Len<<", p2: "<<p2Len);
if (p2Len > p1Len)
{
//swap around paths
boost::filesystem::path::iterator tmp(it2);
it2 = it1;
it1 = tmp;
tmp = it2_end;
it2_end = it1_end;
it1_end = tmp;
}
--it1_end; // make sure the iteration goes only until the previous-last
// entry, because the last entry is either '.' or a filename
for (; it1 != it1_end; ++it1, ++it2)
{
// ROS_INFO_STREAM("Comp "<<it1->string()<<", 2 "<<it2->string());
if ((it2 == it2_end)
|| (*it1 != *it2))
{
break;
}
// append the directory
buildPath /= *it1;
// std::cout << buildPath.string() << std::endl;
}
if (!buildPath.empty())
{
result = buildPath.string();
// make sure notation ends with separator.
// Last element *will* be directory as no files
// have been added in the loop.
enforceDirectory(result, false);
// If the current directory was used to build an absolute path, remove it again.
if (correctAbsolute)
{
std::string currDir = boost::filesystem::current_path().string();
urdf_traverser::helpers::enforceDirectory(currDir, false);
if (!urdf_traverser::helpers::getSubdirPath(currDir, result, result))
{
ROS_ERROR_STREAM("Could not remove temporarily created current directory for absolute path");
return false;
}
}
return true;
}
return false;
}
/** This is a generic routine. It creates a new alignment by making a copy of the old one.
*/
void ImplAlignment::map(
const HAlignment & other,
const CombinationMode & mode )
{
debug_func_cerr(5);
const HAlignment copy = getClone();
clear();
AlignmentIterator it1(copy->begin());
AlignmentIterator it1_end(copy->end());
AlignmentIterator it2(other->begin());
AlignmentIterator it2_end(other->end());
while ( it1 != it1_end && it2 != it2_end )
{
const ResiduePair & x_pair = *it1;
const ResiduePair & y_pair = *it2;
Position map1 = NO_POS;
Position value1 = NO_POS;
Position map2 = NO_POS;
Position value2 = NO_POS;
switch (mode)
{
case RR:
map1 = x_pair.mRow; value2 = x_pair.mCol;
map2 = y_pair.mRow; value1 = y_pair.mCol;
break;
case CR:
map1 = x_pair.mCol; value1 = x_pair.mRow;
map2 = y_pair.mRow; value2 = y_pair.mCol;
break;
case RC:
map1 = x_pair.mRow; value2 = x_pair.mCol;
map2 = y_pair.mCol; value1 = y_pair.mRow;
break;
case CC:
map1 = x_pair.mCol; value1 = x_pair.mRow;
map2 = y_pair.mCol; value2 = y_pair.mRow;
break;
}
assert( value1 != NO_POS);
assert( value2 != NO_POS);
debug_cerr( 5, "map1:" << map1 << " value1:" << value1 << " map2:" << map2 << " value2:" << value2 );
if (map1 == map2)
{
addPair( ResiduePair(value1, value2, 0));
++it1;
++it2;
}
else
{
if (map1 < map2)
++it1;
else
++it2;
}
}
return;
}
