console.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <readline/readline.h>
00003 #include <readline/history.h>
00004 
00005 #include <ros/ros.h>
00006 #include <threemxl/console.h>
00007 #include <threemxl/C3mxlROS.h>
00008 #include <threemxl/CDynamixelROS.h>
00009 #include <threemxl/platform/io/configuration/XMLConfiguration.h>
00010 
00011 #ifdef __MACH__
00012 #include <mach/clock.h>
00013 #include <mach/mach.h>
00014 #endif
00015 
00016 using std::cout;
00017 using std::endl;
00018 using std::hex;
00019 using std::dec;
00020 
00021 #define DXLC_SAFE_CALL(call) \
00022   do { \
00023     int ret = call; \
00024     if (ret != DXL_SUCCESS) { \
00025       cout << "Error:" << endl << "  " << C3mxl::translateErrorCode(ret) << " (0x" << hex << ret << dec << ")" << endl; \
00026     } \
00027   } while (0)
00028 
00029 bool DxlROSCommand::execute(ArgList args)
00030 {
00031   // Sanity check
00032   if (args.size() != nargs_)
00033         return false;
00034   
00035   if (name_ == "exit")
00036   {
00037     ros::shutdown();
00038     return true;
00039   }
00040   else if (name_ == "id")
00041   {
00042     if (args.size() == 1)
00043     {
00044       int id = atoi(args[0].c_str());
00045       if (id >= 0 && id <= 255)
00046         console_->setMotor(id);
00047       else
00048         return false;
00049     }
00050     else
00051     {
00052       CDxlGeneric *motor = console_->getMotor();
00053       DxlROSConsole::MotorList &motors = console_->getMotors();
00054     
00055       cout << "Available motors:" << endl;
00056       for (size_t ii=0; ii < motors.size(); ++ii)
00057       {
00058         if (motors[ii] == motor)
00059           cout << "* ";
00060         else
00061           cout << "  ";
00062         cout << "id " << motors[ii]->getID() << endl;
00063       }
00064     }
00065   
00066     return true;
00067   }
00068   else if (name_ == "hb")
00069   {
00070     double freq = atof(args[0].c_str());
00071     if (freq < 0)
00072       return false;
00073       
00074     if (freq > 0)
00075       console_->setHeartbeatInterval(1/freq);
00076     else
00077       console_->setHeartbeatInterval(0);
00078       
00079     return true;
00080   }
00081   else if (name_ == "scan")
00082   {
00083     CDxlGeneric *motor = console_->createMotor();
00084     CDxlConfig config;
00085     std::vector<int> motors;
00086 
00087     for (int id=1; id < BROADCAST_ID && ros::ok(); ++id)
00088     {
00089       motor->setConfig(config.setID(id));
00090       if (motor->ping() == DXL_SUCCESS)
00091         motors.push_back(id);
00092         
00093       std::cout << "Scanning [";
00094       for (int ii=1; ii < BROADCAST_ID; ++ii)
00095         if (!(ii%15))
00096         {
00097           if (ii < id)
00098             cout << "=";
00099           else
00100             cout << ".";
00101         }
00102       cout << "]\r" << flush;
00103     }
00104     
00105     delete motor;
00106     
00107     if (motors.empty())
00108     {
00109       cout << "No motors found. Check the connection and power." << endl;
00110     }
00111     else
00112     {
00113       cout << "Motor controllers found on bus:" << endl;
00114       for (size_t ii=0; ii < motors.size(); ++ii)
00115         cout << "  id " << motors[ii] << endl;
00116     }
00117     
00118     return true;
00119   }
00120   
00121   CDxlGeneric *motor = console_->getMotor();
00122   if (!motor)
00123   {
00124     cout << "Error:" << endl << "  No motor specified" << endl;
00125     return true;
00126   }
00127   
00128   if (name_ == "init")
00129     DXLC_SAFE_CALL(motor->init(false));
00130   else if (name_ == "mode")
00131   {
00132     if (args[0] == "speed")
00133       DXLC_SAFE_CALL(motor->set3MxlMode(SPEED_MODE));
00134     else if (args[0] == "pos")
00135       DXLC_SAFE_CALL(motor->set3MxlMode(POSITION_MODE));
00136     else if (args[0] == "current")
00137       DXLC_SAFE_CALL(motor->set3MxlMode(CURRENT_MODE));
00138     else if (args[0] == "torque")
00139       DXLC_SAFE_CALL(motor->set3MxlMode(TORQUE_MODE));
00140     else if (args[0] == "sine")
00141       DXLC_SAFE_CALL(motor->set3MxlMode(SINUSOIDAL_POSITION_MODE));
00142     else if (args[0] == "ext_init")
00143       DXLC_SAFE_CALL(motor->set3MxlMode(EXTERNAL_INIT));
00144     else if (args[0] == "hsi_init")
00145       DXLC_SAFE_CALL(motor->set3MxlMode(HOME_SWITCH_AND_INDEX_INIT));
00146     else if (args[0] == "pwm")
00147       DXLC_SAFE_CALL(motor->set3MxlMode(PWM_MODE));
00148     else if (args[0] == "stop")
00149       DXLC_SAFE_CALL(motor->set3MxlMode(STOP_MODE));
00150     else
00151     {
00152       cout << "Unknown mode" << endl;
00153       return false;
00154     }
00155   }
00156   else if (name_ == "config")
00157   {
00158     CXMLConfiguration config_xml;
00159     
00160     if (!config_xml.loadFile(args[0]))
00161     {
00162       cout << "Couldn't read config from " << args[0] << endl;
00163       return false;
00164     }
00165     
00166     CDxlConfig config;
00167     if (args.size() == 2)
00168       config.readConfig(config_xml.root().section(args[1]));
00169     else
00170       config.readConfig(config_xml.root());
00171       
00172     if (config.mID.isSet() && config.mID != motor->getID())
00173       cout << "Config has different ID. Ignoring" << std::endl;
00174       
00175     config.setID(motor->getID());
00176     DXLC_SAFE_CALL(config.configureDynamixel(motor));
00177   }
00178   else if (name_ == "pos")
00179   {
00180     switch (args.size())
00181     {
00182       case 0:
00183         DXLC_SAFE_CALL(motor->getPos());
00184         cout << "Position:" << endl << "  " << motor->presentPos() << " rad" << endl;
00185         break;
00186       case 1:
00187         DXLC_SAFE_CALL(motor->setPos(atof(args[0].c_str())));
00188         break;
00189       case 2:
00190         DXLC_SAFE_CALL(motor->setPos(atof(args[0].c_str()), atof(args[1].c_str())));
00191         break;
00192       case 3:
00193         DXLC_SAFE_CALL(motor->setPos(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str())));
00194         break;
00195     }
00196   }
00197   else if (name_ == "speed")
00198     DXLC_SAFE_CALL(motor->setSpeed(atof(args[0].c_str())));
00199   else if (name_ == "accel")
00200   {
00201     switch (args.size())
00202     {
00203       case 0:
00204         DXLC_SAFE_CALL(motor->getAcceleration());
00205         cout << "Acceleration:" << endl << "  " << motor->presentAcceleration() << " rad/s^2" << endl;
00206         break;
00207       case 1:
00208         DXLC_SAFE_CALL(motor->setAcceleration(atof(args[0].c_str())));
00209         break;
00210     }
00211   }
00212   else if (name_ == "moffset")
00213     DXLC_SAFE_CALL(motor->setMotorOffset(atof(args[0].c_str())));
00214   else if (name_ == "joffset")
00215     DXLC_SAFE_CALL(motor->setJointOffset(atof(args[0].c_str())));
00216   else if (name_ == "jlimits")
00217     DXLC_SAFE_CALL(motor->setAngleLimits(atof(args[0].c_str()), atof(args[1].c_str())));
00218   else if (name_ == "lpos")
00219   {
00220     switch (args.size())
00221     {
00222       case 0:
00223         DXLC_SAFE_CALL(motor->getLinearPos());
00224         cout << "Position:" << endl << "  " << motor->presentLinearPos() << " m" << endl;
00225         break;
00226       case 1:
00227         DXLC_SAFE_CALL(motor->setLinearPos(atof(args[0].c_str())));
00228         break;
00229       case 2:
00230         DXLC_SAFE_CALL(motor->setLinearPos(atof(args[0].c_str()), atof(args[1].c_str())));
00231         break;
00232       case 3:
00233         DXLC_SAFE_CALL(motor->setLinearPos(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str())));
00234         break;
00235     }
00236   }
00237   else if (name_ == "lspeed")
00238     DXLC_SAFE_CALL(motor->setLinearSpeed(atof(args[0].c_str())));
00239   else if (name_ == "laccel")
00240   {
00241     switch (args.size())
00242     {
00243       case 0:
00244         DXLC_SAFE_CALL(motor->getLinearAcceleration());
00245         cout << "Acceleration:" << endl << "  " << motor->presentLinearAcceleration() << " m/s^2" << endl;
00246         break;
00247       case 1:
00248         DXLC_SAFE_CALL(motor->setLinearAcceleration(atof(args[0].c_str())));
00249         break;
00250     }
00251   }
00252   else if (name_ == "current")
00253     DXLC_SAFE_CALL(motor->setCurrent(atof(args[0].c_str())));
00254   else if (name_ == "torque")
00255     DXLC_SAFE_CALL(motor->setTorque(atof(args[0].c_str())));
00256   else if (name_ == "pwm")
00257     DXLC_SAFE_CALL(motor->setPWM(atof(args[0].c_str())));
00258   else if (name_ == "freq")
00259     DXLC_SAFE_CALL(motor->setSineFrequency(atof(args[0].c_str())));
00260   else if (name_ == "ampl")
00261     DXLC_SAFE_CALL(motor->setSineAmplitude(atof(args[0].c_str())));
00262   else if (name_ == "phase")
00263     DXLC_SAFE_CALL(motor->setSinePhase(atof(args[0].c_str())));
00264   else if (name_ == "accel")
00265     DXLC_SAFE_CALL(motor->setAcceleration(atof(args[0].c_str())));
00266   else if (name_ == "ppid")
00267   {
00268     if (args.empty())
00269     {
00270       double p, d, i, i_limit;
00271       DXLC_SAFE_CALL(motor->getPIDPosition(p, d, i, i_limit));
00272       cout << "Position gains:" << endl << "  " << std::fixed
00273            << "P  " << std::setw(8) << std::setprecision(3) << p << ", "
00274            << "D  " << std::setw(8) << std::setprecision(3) << d << ", "
00275            << "I  " << std::setw(8) << std::setprecision(3) << i << ", "
00276            << "IL " << std::setw(8) << std::setprecision(3) << i_limit << endl;
00277     }
00278     else
00279       DXLC_SAFE_CALL(motor->setPIDPosition(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str()), atof(args[3].c_str())));
00280   }
00281   else if (name_ == "spid")
00282   {
00283     if (args.empty())
00284     {
00285       double p, d, i, i_limit;
00286       DXLC_SAFE_CALL(motor->getPIDSpeed(p, d, i, i_limit));
00287       cout << "Speed gains:" << endl << "  " << std::fixed
00288            << "P  " << std::setw(8) << std::setprecision(3) << p << ", "
00289            << "D  " << std::setw(8) << std::setprecision(3) << d << ", "
00290            << "I  " << std::setw(8) << std::setprecision(3) << i << ", "
00291            << "IL " << std::setw(8) << std::setprecision(3) << i_limit << endl;
00292     }
00293     else
00294       DXLC_SAFE_CALL(motor->setPIDSpeed(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str()), atof(args[3].c_str())));
00295   }
00296   else if (name_ == "cpid")
00297   {
00298     if (args.empty())
00299     {
00300       double p, d, i, i_limit;
00301       DXLC_SAFE_CALL(motor->getPIDCurrent(p, d, i, i_limit));
00302       cout << "Current gains:" << endl << "  " << std::fixed
00303            << "P  " << std::setw(8) << std::setprecision(3) << p << ", "
00304            << "D  " << std::setw(8) << std::setprecision(3) << d << ", "
00305            << "I  " << std::setw(8) << std::setprecision(3) << i << ", "
00306            << "IL " << std::setw(8) << std::setprecision(3) << i_limit << endl;
00307     }
00308     else
00309       DXLC_SAFE_CALL(motor->setPIDCurrent(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str()), atof(args[3].c_str())));
00310   }
00311   else if (name_ == "tpid")
00312   {
00313     if (args.empty())
00314     {
00315       double p, d, i, i_limit;
00316       DXLC_SAFE_CALL(motor->getPIDTorque(p, d, i, i_limit));
00317       cout << "Torque gains:" << endl << "  " << std::fixed
00318            << "P  " << std::setw(8) << std::setprecision(3) << p << ", "
00319            << "D  " << std::setw(8) << std::setprecision(3) << d << ", "
00320            << "I  " << std::setw(8) << std::setprecision(3) << i << ", "
00321            << "IL " << std::setw(8) << std::setprecision(3) << i_limit << endl;
00322     }
00323     else
00324       DXLC_SAFE_CALL(motor->setPIDTorque(atof(args[0].c_str()), atof(args[1].c_str()), atof(args[2].c_str()), atof(args[3].c_str())));
00325   }
00326   else if (name_ == "state")
00327   {
00328     DxlROSConsole::MotorList &motors = console_->getMotors();
00329 
00330     cout << "State:" << endl << std::fixed;
00331 
00332     for (size_t ii=0; ii < motors.size(); ++ii)
00333     {
00334       if (motors[ii]->isInitialized() && motors[ii]->getID() != BROADCAST_ID)
00335       {
00336         if (motors[ii] == motor)
00337           cout << "* ";
00338         else
00339           cout << "  ";
00340         
00341         DXLC_SAFE_CALL(motors[ii]->getState());
00342         DXLC_SAFE_CALL(motors[ii]->getStatus());
00343 
00344         cout << "id " << motors[ii]->getID() << " "
00345            << std::setw(8) << std::setprecision(3) << motors[ii]->presentVoltage() << " V, "
00346            << std::setw(8) << std::setprecision(3) << motors[ii]->presentCurrent() << " A, "
00347            << std::setw(8) << std::setprecision(3) << motors[ii]->presentTorque()  << " Nm, "
00348            << std::setw(8) << std::setprecision(3) << motors[ii]->presentPos()     << " rad, "
00349            << std::setw(8) << std::setprecision(3) << motors[ii]->presentSpeed()   << " rad/s, "
00350            << C3mxl::translateErrorCode(motors[ii]->presentStatus()) << " (0x" << hex << motors[ii]->presentStatus() << dec << ")" << endl;
00351       }
00352     }
00353     
00354     cout << std::resetiosflags(std::ios_base::floatfield);
00355   }
00356   else if (name_ == "bus")
00357   {
00358     DXLC_SAFE_CALL(motor->getBusVoltage());
00359     cout << "Bus voltage:" << endl << "  " << motor->presentBusVoltage() << " V" << endl;
00360   }
00361   else if (name_ == "sensors")
00362   {
00363     DXLC_SAFE_CALL(motor->getSensorVoltages());
00364     cout << "Analog sensor voltages:" << endl << "  " << std::fixed
00365          << "Bus         " << std::setw(8) << std::setprecision(3) << motor->presentBusVoltage() << ", "
00366          << "Current ADC " << std::setw(8) << std::setprecision(3) << motor->presentCurrentADCVoltage() << ", "
00367          << "Analog 1    " << std::setw(8) << std::setprecision(3) << motor->presentAnalog1Voltage() << ", "
00368          << "Analog 2    " << std::setw(8) << std::setprecision(3) << motor->presentAnalog2Voltage() << ", "
00369          << "Analog 3    " << std::setw(8) << std::setprecision(3) << motor->presentAnalog3Voltage() << ", "
00370          << "Analog 4    " << std::setw(8) << std::setprecision(3) << motor->presentAnalog4Voltage() << endl;
00371   }
00372   else if (name_ == "log")
00373     DXLC_SAFE_CALL(motor->setLogInterval(atoi(args[0].c_str())));
00374   else if (name_ == "getlog")
00375   {
00376     DXLC_SAFE_CALL(motor->getLog());
00377     TMxlLog log = motor->presentLog();
00378     cout << "    Time      PWM  Current  Voltage  Desired   Actual" << endl;
00379     for (TMxlLog::iterator ii = log.begin(); ii != log.end(); ++ii)
00380       cout << (*ii) << endl;
00381   }
00382   else if (name_ == "savelog")
00383   {
00384     DXLC_SAFE_CALL(motor->getLog());
00385     TMxlLog log = motor->presentLog();
00386 
00387     std::ofstream logstream(args[0].c_str());
00388     logstream << "    Time      PWM  Current  Voltage  Desired   Actual" << endl;
00389     for (TMxlLog::iterator ii = log.begin(); ii != log.end(); ++ii)
00390       logstream << (*ii) << endl;
00391     logstream.close();
00392   }
00393   else if (name_ == "table")
00394   {
00395     BYTE data[256];
00396     
00397     // Read control table in chunks to stay within maximum message size
00398     for (int ii=0; ii < 256; ii += 64)
00399       DXLC_SAFE_CALL(motor->readData(ii, 64, &data[ii]));
00400 
00401     cout << "Control table:" << endl;
00402     for (int rr=0; rr != 16; rr++)
00403     {
00404       cout << "  " << hex << std::setfill('0');
00405       for (int cc=0; cc != 16; ++cc)
00406       {
00407         cout << std::setw(2) << cc*16+rr << ": " << std::setw(2) << (int)data[cc*16+rr];
00408         if (cc < 16-1) cout << ", ";
00409       }
00410       cout << dec << std::setfill(' ') << endl;
00411     }
00412   }
00413   else
00414   {
00415     cout << "Error:" << endl << "  Unknown command" << endl;
00416     return false;
00417   }
00418 
00419   return true;
00420 }
00421 
00422 void Lockable::wait(double interval)
00423 {
00424   if (interval > 0)
00425   {
00426     struct timespec spec;
00427 #ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time
00428     clock_serv_t cclock;
00429     mach_timespec_t mts;
00430     host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
00431     clock_get_time(cclock, &mts);
00432     mach_port_deallocate(mach_task_self(), cclock);
00433     spec.tv_sec = mts.tv_sec;
00434     spec.tv_nsec = mts.tv_nsec;
00435 #else
00436     clock_gettime(CLOCK_REALTIME, &spec);
00437 #endif
00438     
00439     spec.tv_sec += (time_t)interval;
00440     spec.tv_nsec += (long)((interval-(time_t)interval)*1000000000);
00441     if (spec.tv_nsec >= 1000000000)
00442     {
00443       spec.tv_nsec -= 1000000000;
00444       spec.tv_sec++;
00445     }
00446     
00447     pthread_cond_timedwait(&condition_, &mutex_, &spec);
00448   }
00449   else
00450     pthread_cond_wait(&condition_, &mutex_);
00451 }
00452 
00453 CDxlGeneric *DxlROSConsole::createMotor()
00454 {
00455   CDxlGeneric *motor;
00456   
00457   if (strncmp(path_, "/dev", 4))
00458   {
00459     motor = new C3mxlROS(path_);
00460   }
00461   else
00462   {
00463     motor = new C3mxl();
00464     motor->setSerialPort(&serial_port_);
00465   }
00466 
00467   return motor;
00468 }
00469 
00470 void DxlROSConsole::setMotor(int id)
00471 {
00472   motor_ = NULL;
00473   
00474   // See if this motor already exists
00475   for (size_t ii=0; ii < motors_.size(); ++ii)
00476     if (motors_[ii]->getID() == id)
00477       motor_ = motors_[ii];
00478   
00479   // If not, construct a new motor object
00480   if (!motor_)
00481   {
00482     motor_ = createMotor();
00483   
00484     CDxlConfig config;
00485     motor_->setConfig(config.setID(id));
00486     motors_.push_back(motor_);
00487   }
00488 }
00489 
00490 void DxlROSConsole::init(char *path)
00491 {
00492   path_ = path;
00493   
00494   if (strncmp(path_, "/dev", 4))
00495   {
00496     ROS_INFO_STREAM("Using shared_serial at " << path_);
00497   }
00498   else
00499   {
00500     ROS_INFO_STREAM("Using direct connection at " << path_);
00501     serial_port_.port_open(path_, LxSerial::RS485_FTDI);
00502     serial_port_.set_speed(LxSerial::S921600);
00503   }
00504   
00505   // Register commands
00506   commands_.push_back(DxlROSCommand(this, "exit",    0, "exit                     Quit program."));
00507   commands_.push_back(DxlROSCommand(this, "id",      0, "id                       Lists available motors."));
00508   commands_.push_back(DxlROSCommand(this, "id",      1, "id      <id>             Switches motor. id is an integer between 0 and 255."));
00509   commands_.push_back(DxlROSCommand(this, "hb",      1, "hb      <frequency>      Sets heartbeat frequency. 0 disables heartbeat."));
00510 
00511   commands_.push_back(DxlROSCommand(this, "init",    0, "init                     Initializes the motor."));
00512   commands_.push_back(DxlROSCommand(this, "mode",    1, "mode    <mode>           Sets the control mode. mode is one of {pos|speed|current|torque|sine|ext_init}."));
00513   commands_.push_back(DxlROSCommand(this, "config",  1, "config  <file>           Configure the motor with an XML file."));
00514   commands_.push_back(DxlROSCommand(this, "config",  2, "config  <file> <section> Configure the motor with a section from an XML file."));
00515 
00516   commands_.push_back(DxlROSCommand(this, "pos",     0, "pos                      Gets the current position in [rad]."));
00517   commands_.push_back(DxlROSCommand(this, "pos",     1, "pos     <position>       Sets the target position in [rad]."));
00518   commands_.push_back(DxlROSCommand(this, "pos",     2, "pos     <pos> <speed>    Sets the target position in [rad] and maximum velocity in [rad/s]."));
00519   commands_.push_back(DxlROSCommand(this, "pos",     3, "pos     <pos> <sp> <ac>  Sets the target position in [rad], velocity in [rad/s] and acceleration in [rad/s^2]."));
00520   commands_.push_back(DxlROSCommand(this, "speed",   1, "speed   <speed>          Sets the target speed in [rad/s]."));
00521   commands_.push_back(DxlROSCommand(this, "accel",   0, "accel                    Gets the current acceleration [rad/s^2]."));
00522   commands_.push_back(DxlROSCommand(this, "accel",   1, "accel   <accel>          Sets the acceleration for trajectory generation in [rad/s^2]."));
00523   
00524   commands_.push_back(DxlROSCommand(this, "lpos",    0, "lpos                     Gets the current position in [m]."));
00525   commands_.push_back(DxlROSCommand(this, "lpos",    1, "lpos    <position>       Sets the target position in [m]."));
00526   commands_.push_back(DxlROSCommand(this, "lpos",    2, "lpos    <pos> <speed>    Sets the target position in [m] and maximum velocity in [m/s]."));
00527   commands_.push_back(DxlROSCommand(this, "lpos",    3, "lpos    <pos> <sp> <ac>  Sets the target position in [m], velocity in [m/s] and acceleration in [m/s^2]."));
00528   commands_.push_back(DxlROSCommand(this, "lspeed",  1, "lspeed  <speed>          Sets the target speed in [m/s]."));
00529   commands_.push_back(DxlROSCommand(this, "laccel",  0, "laccel                   Gets the current acceleration in [m/s^2]."));
00530   commands_.push_back(DxlROSCommand(this, "laccel",  1, "laccel  <accel>          Sets the acceleration for trajectory generation in [m/s^2]."));
00531   
00532   commands_.push_back(DxlROSCommand(this, "current", 1, "current <current>        Sets the target current in [A]."));
00533   commands_.push_back(DxlROSCommand(this, "torque",  1, "torque  <torque>         Sets the target torque in [Nm]."));
00534   commands_.push_back(DxlROSCommand(this, "pwm",     1, "pwm     <dutycycle>      Sets the PWM duty cycle (between -1 and 1)."));
00535   commands_.push_back(DxlROSCommand(this, "freq",    1, "freq    <frequency>      Sets the target sine frequency in [Hz]."));
00536   commands_.push_back(DxlROSCommand(this, "ampl",    1, "ampl    <amplitude>      Sets the target sine amplitude in [rad]."));
00537   commands_.push_back(DxlROSCommand(this, "phase",   1, "phase   <phase>          Sets the target sine phase angle in [rad]."));
00538 
00539   commands_.push_back(DxlROSCommand(this, "accel",   1, "accel   <accel>          Sets the acceleration for trajectory generation in [rad/s^2]."));
00540   commands_.push_back(DxlROSCommand(this, "moffset", 1, "moffset <offset>         Sets the motor offset in [rad]."));
00541   commands_.push_back(DxlROSCommand(this, "joffset", 1, "joffset <offset>         Sets the joint offset in [rad]."));
00542 
00543   commands_.push_back(DxlROSCommand(this, "jlimits", 2, "jlimits <lower> <upper>  Sets the joint angle limits in [rad]."));
00544   
00545   commands_.push_back(DxlROSCommand(this, "ppid",    0, "ppid                     Gets the position PID gains."));
00546   commands_.push_back(DxlROSCommand(this, "ppid",    4, "ppid    <p> <d> <i> <il> Sets the position PID gains. il is the integration limit."));
00547   commands_.push_back(DxlROSCommand(this, "spid",    0, "spid                     Gets the speed PID gains."));
00548   commands_.push_back(DxlROSCommand(this, "spid",    4, "spid    <p> <d> <i> <il> Sets the speed PID gains. il is the integration limit."));
00549   commands_.push_back(DxlROSCommand(this, "cpid",    0, "cpid                     Gets the current PID gains."));
00550   commands_.push_back(DxlROSCommand(this, "cpid",    4, "cpid    <p> <d> <i> <il> Sets the current PID gains. il is the integration limit."));
00551   commands_.push_back(DxlROSCommand(this, "tpid",    0, "tpid                     Gets the torque PID gains."));
00552   commands_.push_back(DxlROSCommand(this, "tpid",    4, "tpid    <p> <d> <i> <il> Sets the torque PID gains. il is the integration limit."));
00553 
00554   commands_.push_back(DxlROSCommand(this, "bus",     0, "bus                      Displays the current bus voltage."));
00555   commands_.push_back(DxlROSCommand(this, "sensors", 0, "sensors                  Displays the current sensor voltages."));
00556   commands_.push_back(DxlROSCommand(this, "state",   0, "state                    Displays the current state of the motor board."));
00557   commands_.push_back(DxlROSCommand(this, "log",     1, "log     <interval>       Initializes logging for this motor. interval is in [ms]."));
00558   commands_.push_back(DxlROSCommand(this, "getlog",  0, "getlog                   Displays the log of the last motor command."));
00559   commands_.push_back(DxlROSCommand(this, "savelog", 1, "savelog <filename>       Saves the log of the last motor command."));
00560   
00561   commands_.push_back(DxlROSCommand(this, "table",   0, "table                    Displays the entire control table."));
00562   commands_.push_back(DxlROSCommand(this, "scan",    0, "scan                     Scan bus for motors."));
00563 }
00564 
00565 void DxlROSConsole::execute(std::string line)
00566 {
00567   // Tokenize
00568   size_t pos = line.find(' ');
00569   std::string cmd = line.substr(0, pos);
00570 
00571   std::vector<std::string> args;
00572   while (pos != line.npos)
00573   {
00574     size_t newpos = line.find(' ', pos+1);
00575     args.push_back(line.substr(pos+1, newpos-pos-1));
00576     pos = newpos;
00577   }
00578 
00579   // Deal with help separately
00580   if (cmd == "help")
00581   {
00582     cout << "Available commands:" << endl;
00583     for (CommandList::iterator ii=commands_.begin(); ii != commands_.end(); ++ii)
00584       cout << "  " << ii->getHelp() << endl;
00585     cout << endl;
00586     cout << "Recommended initialization:" << endl;
00587     cout << "  id 109; init; mode torque" << endl;
00588     return;
00589   }
00590 
00591   // Try to find a matching command
00592   bool matched = false;
00593   for (CommandList::iterator ii=commands_.begin(); ii != commands_.end(); ++ii)
00594     if (ii->match(cmd, args))
00595     {
00596       matched = true;
00597       if (!ii->execute(args))
00598         cout << "Usage:" << endl << "  " << ii->getHelp() << endl;
00599       break;
00600     }
00601 
00602   // Try to find a loosely matching command and print help
00603   if (!matched)
00604   {
00605     std::stringstream ss;
00606 
00607     for (CommandList::iterator ii=commands_.begin(); ii != commands_.end(); ++ii)
00608       if (ii->matchHelp(cmd, args))
00609       {
00610         matched = true;
00611         ss << "  " << ii->getHelp() << endl;
00612       }
00613 
00614     if (matched)
00615       cout << "Usage:" << endl << ss.str();
00616   }
00617 
00618   // Command not found
00619   if (!matched)
00620     cout << "Error: " << endl << "  Unknown command '" << cmd << "'" << endl;
00621 }
00622 
00623 void DxlROSConsole::spin()
00624 {
00625   char *buf, *home = getenv("HOME"), history_file[PATH_MAX] = {0};
00626   std::string line;
00627 
00628   // Load history
00629   strncat(history_file, home, PATH_MAX-1);
00630   strncat(history_file, "/.threemxl_console_history", PATH_MAX-1);
00631   read_history(history_file);
00632   
00633   cout << "3mxl console, type 'help' for help." << endl;
00634   while (ros::ok())
00635   {
00636     // Prompt for input
00637     buf = readline(">> ");
00638     
00639     if (!buf)
00640     {
00641       // Exit on EOF
00642       cout << "exit" << endl;
00643       break;
00644     }
00645     else if (*buf)
00646     {
00647       // Add to history buffer
00648       add_history(buf);
00649       line = buf;
00650       
00651       // Separate multiple commands
00652       std::vector<std::string> cmds;
00653       
00654       size_t pos = line.find(';');
00655       cmds.push_back(line.substr(0, pos));
00656       while (pos != line.npos)
00657       {
00658         size_t newpos = line.find(';', pos+1);
00659         cmds.push_back(line.substr(pos+1, newpos-pos-1));
00660         pos = newpos;
00661       }
00662       
00663       // Strip whitespace and execute commands
00664       for (std::vector<std::string>::iterator ii=cmds.begin(); ii != cmds.end(); ++ii)
00665       {
00666         while ((!ii->empty()) && (*ii)[0] == ' ') ii->erase(0, 1);
00667         while ((!ii->empty()) && (*ii)[ii->length()-1] == ' ') ii->erase(ii->length()-1, 1);
00668         
00669         lock();
00670         execute(*ii);
00671         unlock();
00672       }
00673     }
00674     
00675     // Process ROS stuff
00676     ros::spinOnce();
00677   }
00678   
00679   lock();
00680 
00681   // Stop the motors
00682   for (size_t ii=0; ii < motors_.size(); ++ii)
00683     motors_[ii]->set3MxlMode(STOP_MODE);
00684     
00685   unlock();
00686   
00687   // Write history
00688   write_history(history_file);
00689 }
00690 
00691 void DxlROSConsole::setHeartbeatInterval(double interval)
00692 {
00693   hb_interval_ = interval;
00694   
00695   // Wake up heartbeat thread
00696   if (hb_interval_ > 0)
00697     signal();
00698 }
00699 
00700 void *DxlROSConsole::spin_hb(void *obj)
00701 {
00702   DxlROSConsole *console = static_cast<DxlROSConsole*>(obj);
00703   if (!console)
00704   {
00705     ROS_ERROR("Couldn't start heartbeat thread");
00706     return NULL;
00707   }
00708   
00709   std::vector<int> status;
00710   
00711   while (ros::ok())
00712   {
00713     console->lock();
00714     
00715     // Wait for next update. This function may return prematurely, but we don't care.
00716     console->wait(console->hb_interval_);
00717     
00718     if (ros::ok() && console->hb_interval_ > 0)
00719     {
00720       bool error = false;
00721     
00722       // Get status of all motors
00723       DxlROSConsole::MotorList &motors = console->getMotors();
00724       for (size_t ii=0; ii < motors.size(); ++ii)
00725       {
00726         if (ii >= status.size())
00727           status.push_back(M3XL_STATUS_IDLE_STATE);
00728           
00729         // Skip broadcast id
00730         if (motors[ii]->getID() == BROADCAST_ID)
00731           continue; 
00732       
00733         motors[ii]->getStatus();
00734         
00735         // Compare current state to previous state to check for a new error
00736         if (status[ii] != motors[ii]->presentStatus() && motors[ii]->presentStatus() < 0x90 && motors[ii]->presentStatus() != DXL_SUCCESS)
00737           error = true;
00738       }
00739       
00740       // Display error if it changed
00741       if (error)
00742       {
00743         cout << endl << "Motor entered error state:" << endl;
00744       
00745         for (size_t ii=0; ii < motors.size(); ++ii)
00746           if (status[ii] != motors[ii]->presentStatus() && motors[ii]->presentStatus() < 0x90 && motors[ii]->presentStatus() != DXL_SUCCESS)
00747             cout << "  id " << motors[ii]->getID() << " " << motors[ii]->translateErrorCode(motors[ii]->presentStatus()) << endl;
00748             
00749         cout << ">> " << flush;
00750       }
00751       
00752       // Remember current error state
00753       for (size_t ii=0; ii < motors.size(); ++ii)
00754         status[ii] = motors[ii]->presentStatus();
00755     }
00756     console->unlock();
00757   }
00758 
00759   return NULL;  
00760 }
00761 
00762 int main(int argc, char **argv)
00763 {
00764   ros::init(argc, argv, "dxl_ros_console");
00765   gLogFactory().setLevel(llWarning);
00766 
00767   char *path=NULL, default_path[] = "/dev/ttyUSB0";
00768   if (argc == 2)
00769     path = argv[1];
00770   else
00771     path = default_path;
00772  
00773   DxlROSConsole dxl_ros_console;
00774   
00775   dxl_ros_console.init(path);
00776   dxl_ros_console.spin();
00777   
00778   ros::shutdown();
00779   
00780   return 0;
00781 } 


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