syncreadtest.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  10000
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   ROS_INFO("Using direct connection");
00015 
00016   config->mDxlTypeStr = "3MXL";
00017   motors_ = new CDxlGroup();
00018   motors_->addNewDynamixel(config->setID(100));
00019   motors_->addNewDynamixel(config->setID(101));
00020   motors_->addNewDynamixel(config->setID(102));
00021   motors_->addNewDynamixel(config->setID(103));
00022 
00023   serial_port_.port_open("/dev/ttyUSB0", LxSerial::RS485_FTDI);
00024   serial_port_.set_speed(LxSerial::S921600);
00025   motors_->setSerialPort(&serial_port_);
00026 
00027   ROS_ASSERT(motors_->init());
00028   
00029   ROS_ASSERT(motors_->setupSyncReadChain() == DXL_SUCCESS);
00030   ROS_ASSERT(motors_->getDynamixel(0)->set3MxlMode(TORQUE_MODE) == DXL_SUCCESS);
00031   ROS_ASSERT(motors_->getDynamixel(1)->set3MxlMode(TORQUE_MODE) == DXL_SUCCESS);
00032   ROS_ASSERT(motors_->getDynamixel(2)->set3MxlMode(TORQUE_MODE) == DXL_SUCCESS);
00033   ROS_ASSERT(motors_->getDynamixel(3)->set3MxlMode(TORQUE_MODE) == DXL_SUCCESS);
00034 
00035   delete config;
00036 }
00037 
00038 void DxlROSExample::spin()
00039 {
00040   double samples[NUM_SAMPLES];
00041   double mi=1, ma=0, avg=0;
00042 
00043   printf("Benchmarking");
00044 
00045   // Warm up
00046   motors_->getStateAll();
00047   
00048   for (int ii=0; ii < NUM_SAMPLES; ++ii)
00049   {
00050     ros::Time begin = ros::Time::now();
00051     for (int jj=0; jj < NUM_MESSAGES; ++jj)
00052       motors_->getStateAll();
00053     samples[ii] = (ros::Time::now() - begin).toSec();
00054     mi = fmin(mi, samples[ii]);
00055     ma = fmax(ma, samples[ii]);
00056     avg += samples[ii];
00057     
00058     if (!(ii % 100))
00059     {
00060       printf(".");
00061       fflush(stdout);
00062     }
00063   }
00064   printf("\n");
00065   
00066   avg /= NUM_SAMPLES;
00067   
00068   double d = (ma-mi)/(NUM_BINS-1), std=0;
00069   int histogram[NUM_BINS];
00070   
00071   bzero(histogram, NUM_BINS*sizeof(int));
00072   
00073   for (int ii=0; ii < NUM_SAMPLES; ++ii)
00074   {
00075     int bin = std::min((int)((samples[ii]-mi)/d), NUM_BINS-1);
00076     histogram[bin]++;
00077     std += pow((samples[ii]-avg), 2);
00078   }
00079   
00080   std = sqrt(std/NUM_SAMPLES);
00081   
00082   printf("3mxl lateny report for %d messages\n", NUM_MESSAGES);
00083   printf("----------------------------------\n");
00084   printf("Average           : %f\n", avg);
00085   printf("Standard deviation: %f\n", std);
00086   printf("Minimum           : %f\n", mi);
00087   printf("Maximum           : %f\n", ma);
00088   printf("\nHistogram\n");
00089   
00090   int hmi=NUM_SAMPLES, hma=0;
00091   for (int ii=0; ii < NUM_BINS; ++ii)
00092   {
00093     hmi = std::min(hmi, histogram[ii]);
00094     hma = std::max(hma, histogram[ii]);
00095   }
00096   
00097   int hd = ceil((hma-hmi)/(NUM_STARS-1));
00098   
00099   for (int ii=NUM_STARS-1; ii >= 0; --ii)
00100   {
00101     for (int jj=0; jj < NUM_BINS; ++jj)
00102       if (histogram[jj] >= hmi+ii*hd)
00103         printf("*");
00104       else
00105         printf(" ");
00106     printf("\n");
00107   }
00108 }
00109 
00110 int main(int argc, char **argv)
00111 {
00112   ros::init(argc, argv, "dxl_ros_example");
00113 
00114   char *path=NULL;
00115   if (argc == 2)
00116     path = argv[1];
00117  
00118   DxlROSExample dxl_ros_example;
00119   
00120   dxl_ros_example.init(path);
00121   dxl_ros_example.spin();
00122   
00123   return 0;   
00124 } 


threemxl
Author(s):
autogenerated on Thu Jun 6 2019 21:10:52