benchmark.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <threemxl/example.h>
00003 #include <threemxl/C3mxlROS.h>
00004 
00005 #define NUM_SAMPLES  1024
00006 #define NUM_MESSAGES 1
00007 #define NUM_BINS     64
00008 #define NUM_STARS    16
00009 
00010 void DxlROSExample::init(char *path)
00011 {
00012   CDxlConfig *config = new CDxlConfig();
00013 
00014   if (path)
00015   {
00016     ROS_INFO("Using shared_serial");
00017     motor_ = new C3mxlROS(path);
00018   }
00019   else
00020   {
00021     ROS_INFO("Using direct connection");
00022     motor_ = new C3mxl();
00023     
00024     serial_port_.port_open("/dev/ttyUSB0", LxSerial::RS485_FTDI);
00025     serial_port_.set_speed(LxSerial::S921600);
00026     motor_->setSerialPort(&serial_port_);
00027   }
00028 
00029   motor_->setConfig(config->setID(100));
00030   ROS_ASSERT(motor_->init(false) == DXL_SUCCESS);
00031   ROS_ASSERT(motor_->set3MxlMode(TORQUE_MODE) == DXL_SUCCESS);
00032 
00033   delete config;
00034 }
00035 
00036 void DxlROSExample::spin()
00037 {
00038   double samples[NUM_SAMPLES];
00039   double mi=1, ma=0, avg=0;
00040 
00041   printf("Benchmarking");
00042 
00043   // Warm up
00044   motor_->getPos();
00045   
00046   for (int ii=0; ii < NUM_SAMPLES; ++ii)
00047   {
00048     ros::Time begin = ros::Time::now();
00049     for (int jj=0; jj < NUM_MESSAGES; ++jj)
00050       motor_->getPos();
00051     samples[ii] = (ros::Time::now() - begin).toSec();
00052     mi = fmin(mi, samples[ii]);
00053     ma = fmax(ma, samples[ii]);
00054     avg += samples[ii];
00055     
00056     if (!(ii % 100))
00057     {
00058       printf(".");
00059       fflush(stdout);
00060     }
00061   }
00062   printf("\n");
00063   
00064   avg /= NUM_SAMPLES;
00065   
00066   double d = (ma-mi)/(NUM_BINS-1), std=0;
00067   int histogram[NUM_BINS];
00068   
00069   bzero(histogram, NUM_BINS*sizeof(int));
00070   
00071   for (int ii=0; ii < NUM_SAMPLES; ++ii)
00072   {
00073     int bin = std::min((int)((samples[ii]-mi)/d), NUM_BINS-1);
00074     histogram[bin]++;
00075     std += pow((samples[ii]-avg), 2);
00076   }
00077   
00078   std = sqrt(std/NUM_SAMPLES);
00079   
00080   printf("3mxl lateny report for %d messages\n", NUM_MESSAGES);
00081   printf("----------------------------------\n");
00082   printf("Average           : %f\n", avg);
00083   printf("Standard deviation: %f\n", std);
00084   printf("Minimum           : %f\n", mi);
00085   printf("Maximum           : %f\n", ma);
00086   printf("\nHistogram\n");
00087   
00088   int hmi=NUM_SAMPLES, hma=0;
00089   for (int ii=0; ii < NUM_BINS; ++ii)
00090   {
00091     hmi = std::min(hmi, histogram[ii]);
00092     hma = std::max(hma, histogram[ii]);
00093   }
00094   
00095   int hd = ceil((hma-hmi)/(NUM_STARS-1));
00096   
00097   for (int ii=NUM_STARS-1; ii >= 0; --ii)
00098   {
00099     for (int jj=0; jj < NUM_BINS; ++jj)
00100       if (histogram[jj] >= hmi+ii*hd)
00101         printf("*");
00102       else
00103         printf(" ");
00104     printf("\n");
00105   }
00106 }
00107 
00108 int main(int argc, char **argv)
00109 {
00110   ros::init(argc, argv, "dxl_ros_example");
00111 
00112   char *path=NULL;
00113   if (argc == 2)
00114     path = argv[1];
00115  
00116   DxlROSExample dxl_ros_example;
00117   
00118   dxl_ros_example.init(path);
00119   dxl_ros_example.spin();
00120   
00121   return 0;   
00122 } 


threemxl
Author(s):
autogenerated on Fri Aug 28 2015 13:21:08