void
write_count (char *searchFor, char *buffer, long buflen)
{
struct ht_ht *ht;
ht = generate_frequencies (strlen (searchFor), buffer, buflen);
printf ("%d\t%s\n", ht_find_new (ht, searchFor)->val, searchFor);
ht_destroy (ht);
}
void
write_frequencies (int fl, char *buffer, long buflen)
{
struct ht_ht *ht;
long total, i, j, size;
struct ht_node *nd;
sorter *s;
sorter tmp;
ht = generate_frequencies (fl, buffer, buflen);
total = 0;
size = 0;
for (nd = ht_first (ht); nd != NULL; nd = ht_next (ht))
{
total = total + nd->val;
size++;
}
s = calloc (size, sizeof (sorter));
i = 0;
for (nd = ht_first (ht); nd != NULL; nd = ht_next (ht))
{
s[i].string = nd->key;
s[i++].num = nd->val;
}
for (i = 0; i < size - 1; i++)
for (j = i + 1; j < size; j++)
if (s[i].num < s[j].num)
{
memcpy (&tmp, &(s[i]), sizeof (sorter));
memcpy (&(s[i]), &(s[j]), sizeof (sorter));
memcpy (&(s[j]), &tmp, sizeof (sorter));
}
for (i = 0; i < size; i++)
printf ("%s %.3f\n", s[i].string, 100 * (float) s[i].num / total);
printf ("\n");
ht_destroy (ht);
free (s);
}
int main(int argc, char** argv){
std::ofstream outfile("training.data");
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
double frequencies[72];
float armlength=1.0, armunits=0.25,clearence =0.0;
int i = 0;
unsigned int freq[40][2][3];
unsigned int a=0,b=0,c=0;
tf::TransformListener listener1;
tf::TransformListener listener2;
tf::TransformListener listener3;
ros::Rate rate(8);
while (node.ok()&&(i<49)){
tf::StampedTransform transform1;
tf::StampedTransform transform2;
tf::StampedTransform transform3;
try{
listener1.lookupTransform("/left_hand" , "/left_shoulder",
ros::Time(0), transform1);
listener2.lookupTransform("/right_shoulder" , "/right_hip",
ros::Time(0), transform2);
listener3.lookupTransform("/right_hand" , "/right_shoulder",
ros::Time(0), transform3);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
if(i<9){
i++;
armlength = 0.55;
clearence = 0.2;
armunits = 0.1 ;
rate.sleep();
continue;
}
a = (int) ((transform3.getOrigin().x()+armlength)/armunits);
b = (int) ((transform3.getOrigin().y()+armlength)/armunits);
c = (int) (transform3.getOrigin().z()/armunits);
//freq[i-9][0] = a + b*8 + c*8*8;
std::cerr << (i-9) << ' '<<a<< ' '<<b<< ' '<<c ; //<< ' '<<freq[i-9][0]<< "\n" ;
freq[i-9][0][0] = a;
freq[i-9][0][1] = b;
freq[i-9][0][2] = c;
a = (int) ((transform1.getOrigin().x()+armlength)/armunits);
b = (int) ((transform1.getOrigin().y()+armlength)/armunits);
c = (int) (transform1.getOrigin().z()/armunits);
//freq[i-9][1] = a + b*8 + c*8*8;
freq[i-9][1][0] = a;
freq[i-9][1][1] = b;
freq[i-9][1][2] = c;
std::cerr << (i-9) << ' '<<a<< ' '<<b<< ' '<<c ; //<< ' '<<freq[i-9][1]<< "\n" ;
i++;
rate.sleep();
}
generate_frequencies(freq , frequencies);
outfile << 3 << ' ' << 72 << ' ' << 3 << "\n" ;
for(unsigned int p = 0; p != 72; p++){
outfile << frequencies[p] << ' ';
}
outfile << std::endl;
outfile << 1 << ' ' << 0 << ' ' << 0 << "\n" ;
getchar();
i=0;
while (node.ok()&&(i<49)){
tf::StampedTransform transform1;
tf::StampedTransform transform2;
tf::StampedTransform transform3;
try{
listener1.lookupTransform("/left_hand" , "/left_shoulder",
ros::Time(0), transform1);
listener2.lookupTransform("/right_shoulder" , "/right_hip",
ros::Time(0), transform2);
listener3.lookupTransform("/right_hand" , "/right_shoulder",
ros::Time(0), transform3);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
if(i<9){
i++;
armlength = 0.55;
clearence = 0.2;
armunits = 0.1 ;
rate.sleep();
continue;
}
a = (int) ((transform3.getOrigin().x()+armlength)/armunits);
b = (int) ((transform3.getOrigin().y()+armlength)/armunits);
c = (int) (transform3.getOrigin().z()/armunits);
//freq[i-9][0] = a + b*8 + c*8*8;
std::cerr << (i-9) << ' '<<a<< ' '<<b<< ' '<<c ; //<< ' '<<freq[i-9][0]<< "\n" ;
freq[i-9][0][0] = a;
freq[i-9][0][1] = b;
freq[i-9][0][2] = c;
a = (int) ((transform1.getOrigin().x()+armlength)/armunits);
b = (int) ((transform1.getOrigin().y()+armlength)/armunits);
c = (int) (transform1.getOrigin().z()/armunits);
//freq[i-9][1] = a + b*8 + c*8*8;
freq[i-9][1][0] = a;
freq[i-9][1][1] = b;
freq[i-9][1][2] = c;
std::cerr << (i-9) << ' '<<a<< ' '<<b<< ' '<<c ; //<< ' '<<freq[i-9][1]<< "\n" ;
i++;
rate.sleep();
}
generate_frequencies(freq , frequencies);
for(unsigned int p = 0; p != 72; p++){
outfile << frequencies[p] << ' ';
}
outfile << std::endl;
outfile << 0 << ' ' << 1 << ' ' << 0 << "\n" ;
getchar();
i=0;
while (node.ok()&&(i<49)){
tf::StampedTransform transform1;
tf::StampedTransform transform2;
tf::StampedTransform transform3;
try{
listener1.lookupTransform("/left_hand" , "/left_shoulder",
ros::Time(0), transform1);
listener2.lookupTransform("/right_shoulder" , "/right_hip",
ros::Time(0), transform2);
listener3.lookupTransform("/right_hand" , "/right_shoulder",
ros::Time(0), transform3);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
if(i<9){
i++;
armlength = 0.55;
clearence = 0.2;
armunits = 0.1 ;
rate.sleep();
continue;
}
a = (int) ((transform3.getOrigin().x()+armlength)/armunits);
b = (int) ((transform3.getOrigin().y()+armlength)/armunits);
c = (int) (transform3.getOrigin().z()/armunits);
//freq[i-9][0] = a + b*8 + c*8*8;
std::cerr << (i-9) << ' '<<a<< ' '<<b<< ' '<<c ; //<< ' '<<freq[i-9][0]<< "\n" ;
freq[i-9][0][0] = a;
freq[i-9][0][1] = b;
freq[i-9][0][2] = c;
a = (int) ((transform1.getOrigin().x()+armlength)/armunits);
b = (int) ((transform1.getOrigin().y()+armlength)/armunits);
c = (int) (transform1.getOrigin().z()/armunits);
//freq[i-9][1] = a + b*8 + c*8*8;
freq[i-9][1][0] = a;
freq[i-9][1][1] = b;
freq[i-9][1][2] = c;
std::cerr << (i-9) << ' '<<a<< ' '<<b<< ' '<<c ; //<< ' '<<freq[i-9][1]<< "\n" ;
i++;
rate.sleep();
}
generate_frequencies(freq , frequencies);
for(unsigned int p = 0; p != 72; p++){
outfile << frequencies[p] << ' ';
}
outfile << std::endl;
outfile << 0 << ' ' << 0<< ' ' << 1 << "\n" ;
outfile.close();
return 0;
};
