testObjectTurnaroundDetector.cpp
Go to the documentation of this file.
00001 /* -*- coding:utf-8-unix; mode:c++; -*- */
00002 
00003 #include "ObjectTurnaroundDetector.h"
00004 /* samples */
00005 #include <stdio.h>
00006 #include <cstdio>
00007 #include <iostream>
00008 #include <vector>
00009 
00010 #ifndef rad2deg
00011 #define rad2deg(rad) (rad * 180 / M_PI)
00012 #endif
00013 #ifndef deg2rad
00014 #define deg2rad(deg) (deg * M_PI / 180)
00015 #endif
00016 
00017 class testObjectTurnaroundDetector
00018 {
00019 protected:
00020     double dt; /* [s] */
00021     ObjectTurnaroundDetector otd;
00022     bool use_gnuplot;
00023     void gen_pattern_and_plot (const std::vector<double>& force_vec,
00024                                const std::vector<double>& time_vec)
00025     {
00026         parse_params();
00027         std::string fname("/tmp/plot-otd.dat");
00028         FILE* fp = fopen(fname.c_str(), "w");
00029         double detect_time;
00030         bool detected = false;
00031         for (size_t i = 0; i < time_vec.size();i++) {
00032             bool tmp_detected = otd.checkDetection(force_vec[i]);
00033             if (tmp_detected && !detected) {
00034                 detect_time = time_vec[i];
00035                 detected = true;
00036             }
00037             fprintf(fp, "%f %f %f %f\n", time_vec[i], force_vec[i], otd.getFilteredWrench(), otd.getFilteredDwrench(), detected);
00038         }
00039         fclose(fp);
00040         if (use_gnuplot) {
00041             // plot
00042             std::string titles[2] = {"Wrench", "Dwrench"};
00043             //   plot pos
00044             FILE* gp = popen("gnuplot", "w");
00045             fprintf(gp, "set multiplot layout 2, 1 title 'Results'\n");
00046             fprintf(gp, "set xlabel 'Time [s]'\n");
00047             fprintf(gp, "set ylabel 'Wrench'\n");
00048             fprintf(gp, "set arrow from %f,%f to %f,%f\n", detect_time, force_vec.front(), detect_time, force_vec.back());
00049             fprintf(gp, "plot '/tmp/plot-otd.dat' using 1:2 with lines title 'Wrench' lw 4, '/tmp/plot-otd.dat' using 1:3 with lines title 'FilteredWrench' lw 4\n");
00050             fprintf(gp, "set xlabel 'Time [s]'\n");
00051             fprintf(gp, "set ylabel 'Dwrench'\n");
00052             fprintf(gp, "plot '/tmp/plot-otd.dat' using 1:4 with lines title 'Dwrench' lw 4\n");
00053             fflush(gp);
00054             double tmp;
00055             std::cin >> tmp;
00056             pclose(gp);
00057         }
00058     };
00059 public:
00060     std::vector<std::string> arg_strs;
00061     testObjectTurnaroundDetector (const double _dt = 0.004) : dt(_dt), otd(_dt), use_gnuplot(true) {};
00062     void test0 ()
00063     {
00064         std::cerr << "test0 : Set" << std::endl;
00065         double tm = 0.0, total_tm = 2.0, df = 10;
00066         std::vector<double> time_vec, force_vec;
00067         for (size_t i = 0; i < static_cast<size_t>(total_tm/dt);i++) {
00068             time_vec.push_back(tm);
00069             if (i*dt < total_tm*0.2) {
00070                 force_vec.push_back(0);
00071             } else if (i*dt > total_tm*0.8) {
00072                 force_vec.push_back(df*total_tm*(0.8-0.2));
00073             } else {
00074                 force_vec.push_back(df*(i*dt-total_tm*0.2));
00075             }
00076             tm += dt;
00077         }
00078         otd.startDetection(df, total_tm);
00079         gen_pattern_and_plot (force_vec, time_vec);
00080     };
00081     void parse_params ()
00082     {
00083       for (unsigned int i = 0; i < arg_strs.size(); ++ i) {
00084           if ( arg_strs[i]== "--use-gnuplot" ) {
00085               if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true");
00086           }
00087       }
00088     };
00089 };
00090 
00091 void print_usage ()
00092 {
00093     std::cerr << "Usage : testObjectTurnaroundDetector [option]" << std::endl;
00094     std::cerr << " [option] should be:" << std::endl;
00095     std::cerr << "  --test0 : Set ref force" << std::endl;
00096     // std::cerr << "  --test1 : Move pos and rot" << std::endl;
00097 };
00098 
00099 int main(int argc, char* argv[])
00100 {
00101     int ret = 0;
00102     if (argc >= 2) {
00103         testObjectTurnaroundDetector totd;
00104         for (int i = 1; i < argc; ++ i) {
00105             totd.arg_strs.push_back(std::string(argv[i]));
00106         }
00107         if (std::string(argv[1]) == "--test0") {
00108             totd.test0();
00109         // } else if (std::string(argv[1]) == "--test1") {
00110         //     tiog.test1();
00111         } else {
00112             print_usage();
00113             ret = 1;
00114         }
00115     } else {
00116         print_usage();
00117         ret = 1;
00118     }
00119     return ret;
00120 }
00121 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56