segwayrmp.cc
Go to the documentation of this file.
00001 #include <algorithm>
00002 #include <iostream>
00003 
00004 #include <segwayrmp/segwayrmp.h>
00005 #include <segwayrmp/impl/rmp_io.h>
00006 #include <segwayrmp/impl/rmp_ftd2xx.h>
00007 #if defined(SEGWAYRMP_USE_SERIAL)
00008 # include <segwayrmp/impl/rmp_serial.h>
00009 #endif
00010 
00011 #if !HAS_CLOCK_GETTIME
00012 # include <sys/time.h>
00013 #endif
00014 
00015 inline void
00016 defaultSegwayStatusCallback(segwayrmp::SegwayStatus::Ptr &segway_status)
00017 {
00018   std::cout << "Segway Status:" << std::endl << std::endl
00019             << segway_status->str() << std::endl << std::endl;
00020 }
00021 
00022 inline void defaultDebugMsgCallback(const std::string &msg)
00023 {
00024   std::cerr << "SegwayRMP Debug: " << msg << std::endl;
00025 }
00026 
00027 inline void defaultInfoMsgCallback(const std::string &msg)
00028 {
00029   std::cerr << "SegwayRMP Info: " << msg << std::endl;
00030 }
00031 
00032 inline void defaultErrorMsgCallback(const std::string &msg)
00033 {
00034   std::cerr << "SegwayRMP Error: " << msg << std::endl;
00035 }
00036 
00037 // This is from ROS's walltime function
00038 // http://www.ros.org/doc/api/rostime/html/time_8cpp_source.html
00039 inline segwayrmp::SegwayTime defaultTimestampCallback()
00040 #ifndef WIN32
00041 throw(segwayrmp::NoHighPerformanceTimersException)
00042 #endif
00043 {
00044   segwayrmp::SegwayTime st;
00045 #ifndef WIN32
00046 # if HAS_CLOCK_GETTIME
00047   timespec start;
00048   clock_gettime(CLOCK_REALTIME, &start);
00049   st.sec  = start.tv_sec;
00050   st.nsec = start.tv_nsec;
00051 # else
00052   struct timeval timeofday;
00053   gettimeofday(&timeofday, NULL);
00054   st.sec  = timeofday.tv_sec;
00055   st.nsec = timeofday.tv_usec * 1000;
00056 # endif
00057 #else
00058   // Win32 implementation
00059   // unless I've missed something obvious, the only way to get high-precision
00060   // time on Windows is via the QueryPerformanceCounter() call. However,
00061   // this is somewhat problematic in Windows XP on some processors,
00062   // especially AMD, because the Windows implementation can freak out when
00063   // the CPU clocks down to save power. Time can jump or even go backwards.
00064   // Microsoft has fixed this bug for most systems now, but it can still
00065   // show up if you have not installed the latest CPU drivers (an oxymoron).
00066   // They fixed all these problems in Windows Vista, and this API is by far
00067   // the most accurate that I know of in Windows, so I'll use it here
00068   // despite all these caveats
00069   static LARGE_INTEGER cpu_freq, init_cpu_time;
00070   uint32_t start_sec = 0;
00071   uint32_t start_nsec = 0;
00072   if ( ( start_sec == 0 ) && ( start_nsec == 0 ) ) {
00073     QueryPerformanceFrequency(&cpu_freq);
00074     if (cpu_freq.QuadPart == 0) {
00075       throw segwayrmp::NoHighPerformanceTimersException();
00076     }
00077     QueryPerformanceCounter(&init_cpu_time);
00078     // compute an offset from the Epoch using the lower-performance
00079     // timer API
00080     FILETIME ft;
00081     GetSystemTimeAsFileTime(&ft);
00082     LARGE_INTEGER start_li;
00083     start_li.LowPart = ft.dwLowDateTime;
00084     start_li.HighPart = ft.dwHighDateTime;
00085     // why did they choose 1601 as the time zero, instead of 1970?
00086     // there were no outstanding hard rock bands in 1601.
00087 # ifdef _MSC_VER
00088     start_li.QuadPart -= 116444736000000000Ui64;
00089 # else
00090     start_li.QuadPart -= 116444736000000000ULL;
00091 # endif
00092     // 100-ns units. odd.
00093     start_sec = (uint32_t)(start_li.QuadPart / 10000000);
00094     start_nsec = (start_li.LowPart % 10000000) * 100;
00095   }
00096   LARGE_INTEGER cur_time;
00097   QueryPerformanceCounter(&cur_time);
00098   LARGE_INTEGER delta_cpu_time;
00099   delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time.QuadPart;
00100   // todo: how to handle cpu clock drift. not sure it's a big deal for us.
00101   // also, think about clock wraparound. seems extremely unlikey,
00102   // but possible
00103   double d_delta_cpu_time =
00104     delta_cpu_time.QuadPart / (double) cpu_freq.QuadPart;
00105   uint32_t delta_sec = (uint32_t) floor(d_delta_cpu_time);
00106   uint32_t delta_nsec =
00107     (uint32_t) boost::math::round((d_delta_cpu_time - delta_sec) * 1e9);
00108 
00109   int64_t sec_sum  = (int64_t)start_sec  + (int64_t)delta_sec;
00110   int64_t nsec_sum = (int64_t)start_nsec + (int64_t)delta_nsec;
00111 
00112   // Throws an exception if we go out of 32-bit range
00113   normalizeSecNSecUnsigned(sec_sum, nsec_sum);
00114 
00115   st.sec = sec_sum;
00116   st.nsec = nsec_sum;
00117 #endif
00118   return st;
00119 }
00120 
00121 inline void defaultExceptionCallback(const std::exception &error)
00122 {
00123   std::cerr << "SegwayRMP Unhandled Exception: " << error.what()
00124             << std::endl;
00125 }
00126 
00127 inline void printHex(char *data, int length)
00128 {
00129   for (int i = 0; i < length; ++i) {
00130     printf("0x%.2X ", (unsigned)(unsigned char)data[i]);
00131   }
00132   printf("\n");
00133 }
00134 
00135 inline void printHexFromString(std::string str)
00136 {
00137   printHex(const_cast<char *>(str.c_str()), str.length());
00138 }
00139 
00140 using namespace segwayrmp;
00141 
00142 SegwayStatus::SegwayStatus()
00143   : timestamp(SegwayTime(0, 0)), pitch(0.0f), pitch_rate(0.0f), roll(0.0f),
00144     roll_rate(0.0f), left_wheel_speed(0.0f), right_wheel_speed(0.0f),
00145     yaw_rate(0.0f), servo_frames(0.0f), integrated_left_wheel_position(0.0f),
00146     integrated_right_wheel_position(0.0f), integrated_forward_position(0.0f),
00147     integrated_turn_position(0.0f), left_motor_torque(0.0f),
00148     right_motor_torque(0.0f), ui_battery_voltage(0.0f),
00149     powerbase_battery_voltage(0.0f), commanded_velocity(0.0f),
00150     commanded_yaw_rate(0.0f), operational_mode(disabled),
00151     controller_gain_schedule(light), motor_status(0), touched(false)
00152 {}
00153 
00154 std::string SegwayStatus::str()
00155 {
00156   std::stringstream ss;
00157   ss << "Time Stamp: "
00158      << "\n  Seconds: " << timestamp.sec
00159      << "\n  Nanoseconds: " << timestamp.nsec
00160      << "\nPitch: " << pitch << "\nPitch Rate: " << pitch_rate
00161      << "\nRoll: " << roll  << "\nRoll Rate: " << roll_rate
00162      << "\nLeft Wheel Speed: " << left_wheel_speed
00163      << "\nRight Wheel Speed: " << right_wheel_speed
00164      << "\nYaw Rate: " << yaw_rate
00165      << "\nServo Frames: " << servo_frames
00166      << "\nIntegrated Left Wheel Position: " << integrated_left_wheel_position
00167      << "\nIntegrated Right Wheel Position: "
00168      << integrated_right_wheel_position
00169      << "\nIntegrated Forward Displacement: " << integrated_forward_position
00170      << "\nIntegrated Turn Position: " << integrated_turn_position
00171      << "\nLeft Motor Torque: " << left_motor_torque
00172      << "\nRight Motor Torque: " << right_motor_torque
00173      << "\nUI Battery Voltage: " << ui_battery_voltage
00174      << "\nPowerbase Battery Voltage: " << powerbase_battery_voltage
00175      << "\nOperational Mode: ";
00176   switch (operational_mode) {
00177     case disabled:
00178       ss << "disabled";
00179       break;
00180     case tractor:
00181       ss << "tractor";
00182       break;
00183     case balanced:
00184       ss << "balanced";
00185       break;
00186     case power_down:
00187       ss << "power down";
00188       break;
00189     default:
00190       ss << "unknown";
00191       break;
00192   }
00193   ss << "\nController Gain Schedule: ";
00194   switch (controller_gain_schedule) {
00195     case light:
00196       ss << "light";
00197       break;
00198     case tall:
00199       ss << "tall";
00200       break;
00201     case heavy:
00202       ss << "heavy";
00203       break;
00204     default:
00205       ss << "unknown";
00206       break;
00207   }
00208   ss << "\nCommanded Velocity: " << commanded_velocity
00209      << "\nCommanded Yaw Rate: " << commanded_yaw_rate
00210      << "\nMotor Status: ";
00211   if (motor_status) {
00212     ss << "Motors Enabled";
00213   } else {
00214     ss << "E-Stopped";
00215   }
00216   return ss.str();
00217 }
00218 
00219 SegwayRMP::SegwayRMP(InterfaceType interface_type,
00220                      SegwayRMPType segway_rmp_type)
00221 : interface_type_(no_interface), segway_rmp_type_(segway_rmp_type),
00222   connected_(false),
00223   continuously_reading_(false),
00224   status_callback_(defaultSegwayStatusCallback),
00225   get_time_(defaultTimestampCallback),
00226   debug_(defaultDebugMsgCallback),
00227   info_(defaultInfoMsgCallback),
00228   error_(defaultErrorMsgCallback),
00229   handle_exception_(defaultExceptionCallback)
00230 {
00231   this->segway_status_ = SegwayStatus::Ptr(new SegwayStatus());
00232   this->interface_type_ = interface_type;
00233   switch (interface_type) {
00234     case can:
00235       RMP_THROW_MSG(ConfigurationException, "CAN is not supported currently");
00236       break;
00237     case usb:
00238       this->rmp_io_ = new FTD2XXRMPIO();
00239       break;
00240     case serial:
00241 #if defined(SEGWAYRMP_USE_SERIAL)
00242       this->rmp_io_ = new SerialRMPIO();
00243 #else
00244       RMP_THROW_MSG(ConfigurationException, "Library is not built with Serial "
00245         "support");
00246 #endif
00247       break;
00248     case ethernet:
00249       RMP_THROW_MSG(ConfigurationException, "Ethernet is not currently "
00250         "supported");
00251       break;
00252     case no_interface:
00253       // do nothing
00254       break;
00255     default:
00256       RMP_THROW_MSG(ConfigurationException, "Invalid InterfaceType specified");
00257       break;
00258   }
00259 
00260   // Set the constants based on the segway type
00261   this->SetConstantsBySegwayType_(this->segway_rmp_type_);
00262 }
00263 
00264 SegwayRMP::~SegwayRMP()
00265 {
00266   if (this->continuously_reading_) {
00267     this->StopReadingContinuously_();
00268   }
00269   if (this->interface_type_ == serial) {
00270 #if defined(SEGWAYRMP_USE_SERIAL)
00271     SerialRMPIO * ptr = (SerialRMPIO *)(this->rmp_io_);
00272     delete ptr;
00273 #endif
00274   }
00275   if (this->interface_type_ == usb) {
00276     FTD2XXRMPIO * ptr = (FTD2XXRMPIO *)(this->rmp_io_);
00277     delete ptr;
00278   }
00279 }
00280 
00281 void SegwayRMP::configureSerial(std::string port, int baudrate)
00282 {
00283 #if SEGWAYRMP_USE_SERIAL
00284   if (this->interface_type_ == serial) {
00285     SerialRMPIO *serial_rmp = (SerialRMPIO *)(this->rmp_io_);
00286     serial_rmp->configure(port, baudrate);
00287   } else {
00288     RMP_THROW_MSG(ConfigurationException, "configureSerial: Cannot configure "
00289       "serial when the InterfaceType is not serial.");
00290   }
00291 #else
00292   RMP_THROW_MSG(ConfigurationException, "configureSerial: The segwayrmp "
00293     "library is not build with serial support, not implemented.");
00294 #endif
00295 }
00296 
00297 void SegwayRMP::configureUSBBySerial(std::string serial_number, int baudrate)
00298 {
00299   if (this->interface_type_ == usb) {
00300     FTD2XXRMPIO *ftd2xx_rmp = (FTD2XXRMPIO *)(this->rmp_io_);
00301     ftd2xx_rmp->configureUSBBySerial(serial_number, baudrate);
00302   } else {
00303     RMP_THROW_MSG(ConfigurationException, "configureUSBBySerial: Cannot "
00304       "configure ftd2xx usb when the InterfaceType is not usb.");
00305   }
00306 }
00307 
00308 void SegwayRMP::configureUSBByDescription(std::string description,
00309                                           int baudrate)
00310 {
00311   if (this->interface_type_ == usb) {
00312     FTD2XXRMPIO *ftd2xx_rmp = (FTD2XXRMPIO *)(this->rmp_io_);
00313     ftd2xx_rmp->configureUSBByDescription(description, baudrate);
00314   } else {
00315     RMP_THROW_MSG(ConfigurationException, "configureUSBByDescription: "
00316       "Cannot configure ftd2xx usb when the InterfaceType is not usb.");
00317   }
00318 }
00319 
00320 void SegwayRMP::configureUSBByIndex(int device_index, int baudrate)
00321 {
00322   if (this->interface_type_ == usb) {
00323     FTD2XXRMPIO *ftd2xx_rmp = (FTD2XXRMPIO *)(this->rmp_io_);
00324     ftd2xx_rmp->configureUSBByIndex(device_index, baudrate);
00325   } else {
00326     RMP_THROW_MSG(ConfigurationException, "configureUSBByIndex: "
00327       "Cannot configure ftd2xx usb when the InterfaceType is not usb.");
00328   }
00329 }
00330 
00331 void SegwayRMP::connect(bool reset_integrators)
00332 {
00333   // Connect to the interface
00334   this->rmp_io_->connect();
00335 
00336   this->connected_ = true;
00337 
00338   if (reset_integrators) {
00339     // Reset all the integrators
00340     this->resetAllIntegrators();
00341   }
00342 
00343   // Kick off the read thread
00344   this->StartReadingContinuously_();
00345 
00346 }
00347 
00348 void SegwayRMP::shutdown()
00349 {
00350   // Ensure we are connected
00351   if (!this->connected_)
00352       RMP_THROW_MSG(ConfigurationException, "Cannot send shutdown: "
00353         "Not Connected.");
00354   try {
00355     Packet packet;
00356 
00357     packet.id = 0x0412;
00358 
00359     this->rmp_io_->sendPacket(packet);
00360   } catch (std::exception &e) {
00361       std::stringstream ss;
00362       ss << "Cannot send shutdown: " << e.what();
00363       RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00364   }
00365 }
00366 
00367 void SegwayRMP::moveCounts(short int linear_counts, short int angular_counts)
00368 {
00369   // Ensure we are connected
00370   if (!this->connected_)
00371     RMP_THROW_MSG(MoveFailedException, "Not Connected.");
00372   try {
00373     short int lc = linear_counts;
00374     short int ac = angular_counts;
00375     Packet packet;
00376 
00377     packet.id = 0x0413;
00378 
00379     packet.data[0] = (unsigned char)((lc & 0xFF00) >> 8);
00380     packet.data[1] = (unsigned char)(lc & 0x00FF);
00381     packet.data[2] = (unsigned char)((ac & 0xFF00) >> 8);
00382     packet.data[3] = (unsigned char)(ac & 0x00FF);
00383     packet.data[4] = 0x00;
00384     packet.data[5] = 0x00;
00385     packet.data[6] = 0x00;
00386     packet.data[7] = 0x00;
00387 
00388     this->rmp_io_->sendPacket(packet);
00389   } catch (std::exception &e) {
00390     RMP_THROW_MSG(MoveFailedException, e.what());
00391   }
00392 }
00393 
00394 void SegwayRMP::move(float linear_velocity, float angular_velocity)
00395 {
00396   // Ensure we are connected
00397   if (!this->connected_)
00398     RMP_THROW_MSG(MoveFailedException, "Not Connected.");
00399   try {
00400     short int lv = (short int)(linear_velocity * this->mps_to_counts_);
00401     short int av = (short int)(angular_velocity * this->dps_to_counts_);
00402 
00403     Packet packet;
00404 
00405     packet.id = 0x0413;
00406 
00407     packet.data[0] = (unsigned char)((lv & 0xFF00) >> 8);
00408     packet.data[1] = (unsigned char)(lv & 0x00FF);
00409     packet.data[2] = (unsigned char)((av & 0xFF00) >> 8);
00410     packet.data[3] = (unsigned char)(av & 0x00FF);
00411     packet.data[4] = 0x00;
00412     packet.data[5] = 0x00;
00413     packet.data[6] = 0x00;
00414     packet.data[7] = 0x00;
00415 
00416     this->rmp_io_->sendPacket(packet);
00417   } catch (std::exception &e) {
00418     RMP_THROW_MSG(MoveFailedException, e.what());
00419   }
00420 }
00421 
00422 void SegwayRMP::setOperationalMode(OperationalMode operational_mode)
00423 {
00424   // Ensure we are connected
00425   if (!this->connected_)
00426     RMP_THROW_MSG(ConfigurationException, "Cannot set operational mode: "
00427       "Not Connected.");
00428   try {
00429     Packet packet;
00430 
00431     packet.id = 0x0413;
00432 
00433     packet.data[0] = 0x00;
00434     packet.data[1] = 0x00;
00435     packet.data[2] = 0x00;
00436     packet.data[3] = 0x00;
00437     packet.data[4] = 0x00;
00438     packet.data[5] = 0x10;
00439     packet.data[6] = 0x00;
00440     packet.data[7] = (unsigned char)operational_mode;
00441 
00442     this->rmp_io_->sendPacket(packet);
00443 
00444 //    while(this->segway_status_->operational_mode != operational_mode) {
00445 //      // Check again in 10 ms
00446 //      boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00447 //    }
00448   } catch (std::exception &e) {
00449     std::stringstream ss;
00450     ss << "Cannot set operational mode: " << e.what();
00451     RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00452   }
00453 }
00454 
00455 void SegwayRMP::setControllerGainSchedule(
00456   ControllerGainSchedule controller_gain_schedule)
00457 {
00458   // Ensure we are connected
00459   if (!this->connected_)
00460     RMP_THROW_MSG(ConfigurationException, "Cannot set controller gain "
00461       "schedule: Not Connected.");
00462   try {
00463     Packet packet;
00464 
00465     packet.id = 0x0413;
00466 
00467     packet.data[0] = 0x00;
00468     packet.data[1] = 0x00;
00469     packet.data[2] = 0x00;
00470     packet.data[3] = 0x00;
00471     packet.data[4] = 0x00;
00472     packet.data[5] = 0x0D;
00473     packet.data[6] = 0x00;
00474     packet.data[7] = (unsigned char)controller_gain_schedule;
00475 
00476     this->rmp_io_->sendPacket(packet);
00477 
00478 //    while(this->segway_status_->controller_gain_schedule
00479 //          != controller_gain_schedule)
00480 //    {
00481 //      // Check again in 10 ms
00482 //      boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00483 //    }
00484   } catch (std::exception &e) {
00485     std::stringstream ss;
00486     ss << "Cannot set controller gain schedule: " << e.what();
00487     RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00488   }
00489 }
00490 
00491 void SegwayRMP::setBalanceModeLocking(bool state)
00492 {
00493   // Ensure we are connected
00494   if (!this->connected_)
00495     RMP_THROW_MSG(ConfigurationException, "Cannot set balance mode lock: "
00496       "Not Connected.");
00497   try {
00498     Packet packet;
00499 
00500     packet.id = 0x0413;
00501 
00502     packet.data[0] = 0x00;
00503     packet.data[1] = 0x00;
00504     packet.data[2] = 0x00;
00505     packet.data[3] = 0x00;
00506     packet.data[4] = 0x00;
00507     packet.data[5] = 0x0F;
00508     packet.data[6] = 0x00;
00509     if (state)
00510       packet.data[7] = 0x01;
00511     else
00512       packet.data[7] = 0x00;
00513 
00514     this->rmp_io_->sendPacket(packet);
00515   } catch (std::exception &e) {
00516     std::stringstream ss;
00517     ss << "Cannot set balance mode lock: " << e.what();
00518     RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00519   }
00520 }
00521 
00522 void SegwayRMP::resetAllIntegrators()
00523 {
00524   // Ensure we are connected
00525   if (!this->connected_)
00526     RMP_THROW_MSG(ConfigurationException, "Cannot reset Integrators: Not "
00527       "Connected.");
00528   try {
00529     Packet packet;
00530 
00531     packet.id = 0x0413;
00532 
00533     packet.data[0] = 0x00;
00534     packet.data[1] = 0x00;
00535     packet.data[2] = 0x00;
00536     packet.data[3] = 0x00;
00537     packet.data[4] = 0x00;
00538     packet.data[5] = 0x32;
00539     packet.data[6] = 0x00;
00540     packet.data[7] = 0x01;
00541 
00542     this->rmp_io_->sendPacket(packet);
00543 
00544     packet.data[7] = 0x02;
00545 
00546     this->rmp_io_->sendPacket(packet);
00547 
00548     packet.data[7] = 0x04;
00549 
00550     this->rmp_io_->sendPacket(packet);
00551 
00552     packet.data[7] = 0x08;
00553 
00554     this->rmp_io_->sendPacket(packet);
00555 
00556   } catch (std::exception &e) {
00557     std::stringstream ss;
00558     ss << "Cannot reset Integrators: " << e.what();
00559     RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00560   }
00561 }
00562 
00563 void SegwayRMP::setMaxVelocityScaleFactor(double scalar)
00564 {
00565   // Ensure we are connected
00566   if (!this->connected_)
00567     RMP_THROW_MSG(ConfigurationException, "Cannot set max velocity scale "
00568       "factor: Not Connected.");
00569   try {
00570     Packet packet;
00571 
00572     packet.id = 0x0413;
00573 
00574     packet.data[0] = 0x00;
00575     packet.data[1] = 0x00;
00576     packet.data[2] = 0x00;
00577     packet.data[3] = 0x00;
00578     packet.data[4] = 0x00;
00579     packet.data[5] = 0x0A;
00580     packet.data[6] = 0x00;
00581 
00582     if (scalar < 0.0)
00583       scalar = 0.0;
00584     if (scalar > 1.0)
00585       scalar = 1.0;
00586     scalar *= 16.0;
00587     scalar = floor(scalar);
00588 
00589     short int scalar_int = (short int)scalar;
00590 
00591     packet.data[7] = (unsigned char)(scalar_int & 0x00FF);
00592 
00593     this->rmp_io_->sendPacket(packet);
00594   } catch (std::exception &e) {
00595     std::stringstream ss;
00596     ss << "Cannot set max velocity scale factor: " << e.what();
00597     RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00598   }
00599 }
00600 
00601 void SegwayRMP::setMaxAccelerationScaleFactor(double scalar)
00602 {
00603   // Ensure we are connected
00604   if (!this->connected_)
00605     RMP_THROW_MSG(ConfigurationException, "Cannot set max acceleration scale "
00606       "factor: Not Connected.");
00607   try {
00608     Packet packet;
00609 
00610     packet.id = 0x0413;
00611 
00612     packet.data[0] = 0x00;
00613     packet.data[1] = 0x00;
00614     packet.data[2] = 0x00;
00615     packet.data[3] = 0x00;
00616     packet.data[4] = 0x00;
00617     packet.data[5] = 0x0B;
00618     packet.data[6] = 0x00;
00619 
00620     if (scalar < 0.0)
00621       scalar = 0.0;
00622     if (scalar > 1.0)
00623       scalar = 1.0;
00624     scalar *= 16.0;
00625     scalar = floor(scalar);
00626 
00627     short int scalar_int = (short int)scalar;
00628 
00629     packet.data[7] = (unsigned char)(scalar_int & 0x00FF);
00630 
00631     this->rmp_io_->sendPacket(packet);
00632   } catch (std::exception &e) {
00633     std::stringstream ss;
00634     ss << "Cannot set max acceleration scale factor: " << e.what();
00635     RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00636   }
00637 }
00638 
00639 void SegwayRMP::setMaxTurnScaleFactor(double scalar)
00640 {
00641   // Ensure we are connected
00642   if (!this->connected_)
00643     RMP_THROW_MSG(ConfigurationException, "Cannot set max turn scale factor: "
00644       "Not Connected.");
00645   try {
00646     Packet packet;
00647 
00648     packet.id = 0x0413;
00649 
00650     packet.data[0] = 0x00;
00651     packet.data[1] = 0x00;
00652     packet.data[2] = 0x00;
00653     packet.data[3] = 0x00;
00654     packet.data[4] = 0x00;
00655     packet.data[5] = 0x0C;
00656     packet.data[6] = 0x00;
00657 
00658     if (scalar < 0.0)
00659       scalar = 0.0;
00660     if (scalar > 1.0)
00661       scalar = 1.0;
00662     scalar *= 16.0;
00663     scalar = floor(scalar);
00664 
00665     short int scalar_int = (short int)scalar;
00666 
00667     packet.data[7] = (unsigned char)(scalar_int & 0x00FF);
00668 
00669     this->rmp_io_->sendPacket(packet);
00670   } catch (std::exception &e) {
00671     std::stringstream ss;
00672     ss << "Cannot set max turn scale factor: " << e.what();
00673     RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00674   }
00675 }
00676 
00677 void SegwayRMP::setCurrentLimitScaleFactor(double scalar)
00678 {
00679   // Ensure we are connected
00680   if (!this->connected_)
00681     RMP_THROW_MSG(ConfigurationException, "Cannot set current limit scale "
00682       "factor: Not Connected.");
00683   try {
00684     Packet packet;
00685 
00686     packet.id = 0x0413;
00687 
00688     packet.data[0] = 0x00;
00689     packet.data[1] = 0x00;
00690     packet.data[2] = 0x00;
00691     packet.data[3] = 0x00;
00692     packet.data[4] = 0x00;
00693     packet.data[5] = 0x0E;
00694     packet.data[6] = 0x00;
00695 
00696     if (scalar < 0.0)
00697       scalar = 0.0;
00698     if (scalar > 1.0)
00699       scalar = 1.0;
00700     scalar *= 256.0;
00701     scalar = floor(scalar);
00702 
00703     short int scalar_int = (short int)scalar;
00704 
00705     packet.data[7] = (unsigned char)(scalar_int & 0x00FF);
00706 
00707     this->rmp_io_->sendPacket(packet);
00708   } catch (std::exception &e) {
00709     std::stringstream ss;
00710     ss << "Cannot set current limit scale factor: " << e.what();
00711     RMP_THROW_MSG(ConfigurationException, ss.str().c_str());
00712   }
00713 }
00714 
00715 void SegwayRMP::setStatusCallback(SegwayStatusCallback callback) {
00716   this->status_callback_ = callback;
00717 }
00718 
00719 void SegwayRMP::setLogMsgCallback(std::string log_level,
00720                                     LogMsgCallback callback)
00721 {
00722   // Convert to lower case
00723   std::transform(log_level.begin(), log_level.end(),
00724                  log_level.begin(), ::tolower);
00725   if (log_level == "debug") {
00726     this->debug_ = callback;
00727   }
00728   if (log_level == "info") {
00729     this->info_ = callback;
00730   }
00731   if (log_level == "error") {
00732     this->error_ = callback;
00733   }
00734 }
00735 
00736 void SegwayRMP::setTimestampCallback(GetTimeCallback callback) {
00737   this->get_time_ = callback;
00738 }
00739 
00740 void SegwayRMP::setExceptionCallback(ExceptionCallback exception_callback) {
00741   this->handle_exception_ = exception_callback;
00742 }
00743 
00744 void SegwayRMP::ReadContinuously_() {
00745   Packet packet;
00746   while (this->continuously_reading_) {
00747     try {
00748       this->rmp_io_->getPacket(packet);
00749       this->ProcessPacket_(packet);
00750     } catch (PacketRetrievalException &e) {
00751       if (e.error_number() == 2) // Failed Checksum
00752         this->error_("Checksum mismatch...");
00753       else if (e.error_number() == 3) // No packet received
00754         this->error_("No data from Segway...");
00755       else
00756         this->handle_exception_(e);
00757     }
00758   }
00759 }
00760 
00761 void SegwayRMP::ExecuteCallbacks_() {
00762   while (this->continuously_reading_) {
00763     SegwayStatus::Ptr ss = this->ss_queue_.dequeue();
00764     if (this->continuously_reading_) {
00765       try {
00766         if (ss) {
00767           if (this->status_callback_) {
00768             this->status_callback_(ss);
00769           } // if this->status_callback_
00770         } // if ss
00771       } catch (std::exception &e) {
00772         this->handle_exception_(e);
00773       }// try callback
00774     }// if continuous
00775   }// while continuous
00776 }
00777 
00778 void SegwayRMP::StartReadingContinuously_() {
00779   this->continuously_reading_ = true;
00780   this->read_thread_ =
00781     boost::thread(&SegwayRMP::ReadContinuously_, this);
00782   this->callback_execution_thread_ =
00783     boost::thread(&SegwayRMP::ExecuteCallbacks_, this);
00784 }
00785 
00786 void SegwayRMP::StopReadingContinuously_()
00787 {
00788   this->continuously_reading_ = false;
00789   this->rmp_io_->cancel();
00790   this->read_thread_.join();
00791   this->ss_queue_.cancel();
00792   this->callback_execution_thread_.join();
00793 }
00794 
00795 void SegwayRMP::SetConstantsBySegwayType_(SegwayRMPType &rmp_type) {
00796   if (rmp_type == rmp200 || rmp_type == rmp400) {
00797     this->dps_to_counts_ = 7.8;
00798     this->mps_to_counts_ = 332.0;
00799     this->meters_to_counts_ = 33215.0;
00800     this->rev_to_counts_ = 112644.0;
00801     this->torque_to_counts_ = 1094.0;
00802   } else
00803   if (rmp_type == rmp50 || rmp_type == rmp100) {
00804     this->dps_to_counts_ = 7.8;
00805     this->mps_to_counts_ = 401.0;
00806     this->meters_to_counts_ = 40181.0;
00807     this->rev_to_counts_ = 117031.0;
00808     this->torque_to_counts_ = 1463.0;
00809   } else {
00810     RMP_THROW_MSG(ConfigurationException, "Invalid Segway RMP Type");
00811   }
00812 }
00813 
00814 inline short int getShortInt(unsigned char high, unsigned char low)
00815 {
00816   return (short int)(((unsigned short int)high << 8)
00817                    | (unsigned short int)low);
00818 }
00819 
00820 inline int
00821 getInt(unsigned char lhigh, unsigned char llow,
00822        unsigned char hhigh, unsigned char hlow)
00823 {
00824   int result = 0;
00825   char data[4] = {llow, lhigh, hlow, hhigh};
00826   memcpy(&result, data, 4);
00827   return result;
00828 }
00829 
00830 bool SegwayRMP::ParsePacket_(Packet &packet, SegwayStatus::Ptr &ss_ptr)
00831 {
00832   bool status_updated = false;
00833   if (packet.channel == 0xBB) // Ignore Channel B messages
00834     return status_updated;
00835 
00836   // This section comes largerly from the Segway example code
00837   switch (packet.id) {
00838   case 0x0400: // COMMAND REQUEST
00839     // This is the first packet of a msg series, timestamp here.
00840     ss_ptr->timestamp  = this->get_time_();
00841     break;
00842   case 0x0401:
00843     ss_ptr->pitch      = getShortInt(packet.data[0], packet.data[1])
00844                        / this->dps_to_counts_;
00845     ss_ptr->pitch_rate = getShortInt(packet.data[2], packet.data[3])
00846                        / this->dps_to_counts_;
00847     ss_ptr->roll       = getShortInt(packet.data[4], packet.data[5])
00848                        / this->dps_to_counts_;
00849     ss_ptr->roll_rate  = getShortInt(packet.data[6], packet.data[7])
00850                        / this->dps_to_counts_;
00851     ss_ptr->touched = true;
00852     break;
00853   case 0x0402:
00854     ss_ptr->left_wheel_speed  = getShortInt(packet.data[0], packet.data[1])
00855                               / this->mps_to_counts_;
00856     ss_ptr->right_wheel_speed = getShortInt(packet.data[2], packet.data[3])
00857                               / this->mps_to_counts_;
00858     ss_ptr->yaw_rate          = getShortInt(packet.data[4], packet.data[5])
00859                               / this->dps_to_counts_;
00860     ss_ptr->servo_frames      = (
00861                                  (((short unsigned int)packet.data[6]) << 8)
00862                                | ((short unsigned int)packet.data[7])
00863                                 ) * 0.01;
00864     ss_ptr->touched = true;
00865     break;
00866   case 0x0403:
00867     ss_ptr->integrated_left_wheel_position  =
00868       getInt(packet.data[0], packet.data[1], packet.data[2], packet.data[3])
00869     / this->meters_to_counts_;
00870     ss_ptr->integrated_right_wheel_position =
00871       getInt(packet.data[4], packet.data[5], packet.data[6], packet.data[7])
00872     / this->meters_to_counts_;
00873     ss_ptr->touched = true;
00874     break;
00875   case 0x0404:
00876     ss_ptr->integrated_forward_position =
00877       getInt(packet.data[0], packet.data[1], packet.data[2], packet.data[3])
00878     / this->meters_to_counts_;
00879     ss_ptr->integrated_turn_position    =
00880       getInt(packet.data[4], packet.data[5], packet.data[6], packet.data[7])
00881     / this->rev_to_counts_;
00882     // convert from revolutions to degrees
00883     ss_ptr->integrated_turn_position *= 360.0;
00884     ss_ptr->touched = true;
00885     break;
00886   case 0x0405:
00887     ss_ptr->left_motor_torque  = getShortInt(packet.data[0], packet.data[1])
00888                                / this->torque_to_counts_;
00889     ss_ptr->right_motor_torque = getShortInt(packet.data[2], packet.data[3])
00890                                / this->torque_to_counts_;
00891     ss_ptr->touched = true;
00892     break;
00893   case 0x0406:
00894     ss_ptr->operational_mode          =
00895       OperationalMode(getShortInt(packet.data[0], packet.data[1]));
00896     ss_ptr->controller_gain_schedule  =
00897       ControllerGainSchedule(getShortInt(packet.data[2], packet.data[3]));
00898     ss_ptr->ui_battery_voltage        =
00899       (
00900         (((short unsigned int)packet.data[4]) << 8)
00901       | ((short unsigned int)packet.data[5])
00902       ) * 0.0125 + 1.4;
00903     ss_ptr->powerbase_battery_voltage =
00904       (
00905         (((short unsigned int)packet.data[6]) << 8)
00906       | ((short unsigned int)packet.data[7])
00907       ) / 4.0;
00908     ss_ptr->touched = true;
00909     break;
00910   case 0x0407:
00911     ss_ptr->commanded_velocity =
00912       (float)getShortInt(packet.data[0], packet.data[1])
00913     / this->mps_to_counts_;
00914     ss_ptr->commanded_yaw_rate =
00915       (float)getShortInt(packet.data[2], packet.data[3])
00916     / 1024.0;
00917     status_updated = true;
00918     ss_ptr->touched = true;
00919     break;
00920   case 0x0680:
00921     if (packet.data[3] == 0x80) // Motors Enabled
00922       ss_ptr->motor_status = 1;
00923     else // E-Stopped
00924       ss_ptr->motor_status = 0;
00925     ss_ptr->touched = true;
00926     break;
00927   default: // Unknown/Unhandled Message
00928     break;
00929   };
00930   return status_updated;
00931 }
00932 
00933 void SegwayRMP::ProcessPacket_(Packet &packet)
00934 {
00935   bool status_updated = false;
00936 
00937   status_updated = this->ParsePacket_(packet, this->segway_status_);
00938 
00939   // Messages come in order 0x0400, 0x0401, ... 0x0407 so a
00940   //  complete "cycle" of information has been sent every
00941   //  time we get an 0x0407
00942   if (status_updated) {
00943     if (this->ss_queue_.enqueue(this->segway_status_)) {
00944       this->error_("Falling behind, SegwayStatus Queue Full, skipping packet "
00945         "report...");
00946     }
00947     this->segway_status_ = SegwayStatus::Ptr(new SegwayStatus());
00948   }
00949 }
00950 


libsegwayrmp
Author(s): William Woodall
autogenerated on Wed Aug 26 2015 12:23:18