myList<object> intersect(const myList<object> &f,const myList<object> &s) //return the insection of two increasing lists,f and s.
{
myList<object> retVal;
myList<object>::const_iterator itr2=s.begin();
for(myList<object>::const_iterator itr1=f.begin();itr1!=f.end();)
{
if(*itr1==*itr2) //find the same element.
{
retVal.push_back(*itr1);
++itr1; //move two iterators to next nodes.
++itr2;
if(itr2==s.end())
break;
}
else if(*itr1<*itr2) //The element of f is small.
{
++itr1;
}
else //The element of s is small.
{
++itr2;
if(itr2==s.end())
break;
}
}
return retVal;
}
int main()
{
ACE_OS::printf("\nHello from YARP!\n\n");
char path[255];
sprintf(path,"%s/%s", GetYarpRoot(), ConfigFilePath);
// read positions from file
_parseFile(path, posList, home);
YARPArm arm;
if (arm.initialize(path, "arm.ini") == YARP_OK)
printf("Init was OK");
else
{
printf("Init was NOT OK");
exit(-1);
}
arm.activatePID();
// go to home
ACE_OS::printf("\nGoing home...\n");
setPositions(arm, home);
YARPTime::DelayInSeconds(3.0);
myList::iterator it,end;
it = posList.begin();
end = posList.end();
while(it!=end)
{
setPositions(arm, (*it));
it++;
YARPTime::DelayInSeconds(3.0);
}
ACE_OS::printf("\nGoing back home");
setPositions(arm, home);
YARPTime::DelayInSeconds(3.0);
ACE_OS::printf("\ndone!\n");
arm.uninitialize();
return 0;
}
void _parseFile(char *path, myList &list, YVector &home)
{
YARPConfigFile cfg(path, "demo.cfg");
printf("%s\n", path);
int n = __nj;
int length;
home.Resize(__nj);
if (cfg.get("[POS]", "N=", &length)==YARP_FAIL)
printf("Error reading file\n");
cfg.get("[POS]", "Home=", home.data(),__nj);
YVector tmp(__nj);
for(int i = 1; i <= length; i++)
{
char k[80];
sprintf(k, "WayPoint%d=", i);
cfg.get("[POS]", k, tmp.data(),__nj);
list.push_back(tmp);
}
/// BEGIN DEBUG
printf("N=%d\n", length);
printf("Home: ");
for(int i = 1; i<=__nj; i++)
printf("%lf\t", home(i));
printf("\n");
myList::iterator it,end;
it = list.begin();
end = list.end();
while(it!=end)
{
for(int i = 1; i<=__nj; i++)
printf("%lf\t", (*it)(i));
printf("\n");
it++;
}
}