segwayrmp.h
Go to the documentation of this file.
00001 
00039 #ifndef SEGWAYRMP_H
00040 #define SEGWAYRMP_H
00041 
00042 #include <exception>
00043 #include <sstream>
00044 #include <queue>
00045 #include <typeinfo>
00046 
00047 #include <boost/function.hpp>
00048 #include <boost/thread.hpp>
00049 
00050 /* Setup (u)int*_t types if not UNIX. */
00051 #if defined(_WIN32) && !defined(__MINGW32__)
00052   typedef unsigned int uint32_t;
00053 #else
00054 # include <stdint.h>
00055 #endif
00056 
00057 // ClassName - The name of the exception class
00058 // Prefix    - The common prefix
00059 // Message   - The default message, if one is not specified at Throw
00060 // The message takes the form of:
00061 // ClassName occurred at line # of file `<file>`: Prefix+Message
00062 #define DEFINE_EXCEPTION(ClassName, Prefix, Message) \
00063   class ClassName : public std::exception { \
00064     void operator=(const ClassName &); \
00065     const ClassName & operator=( ClassName ); \
00066     const char * what_; \
00067     const int id_; \
00068   public: \
00069     ClassName(const char * file, const int ln, \
00070               const char * msg = Message, const int id = -1) : id_(id) { \
00071       std::stringstream ss; \
00072       ss << #ClassName " occurred at line " << ln \
00073          << " of `" << file << "`: " << Prefix << msg; \
00074       what_ = ss.str().c_str(); \
00075     } \
00076     virtual const char* what () const throw () { return what_; } \
00077     const int error_number() { return this->id_; } \
00078   };
00079 
00080 #define RMP_THROW(ExceptionClass) \
00081   throw ExceptionClass(__FILE__, __LINE__)
00082 
00083 #define RMP_THROW_MSG(ExceptionClass, Message) \
00084   throw ExceptionClass(__FILE__, __LINE__, (Message) )
00085 
00086 #define RMP_THROW_MSG_AND_ID(ExceptionClass, Message, Id) \
00087   throw ExceptionClass(__FILE__, __LINE__, (Message), (Id) )
00088 
00092 #define MAX_SEGWAYSTATUS_QUEUE_SIZE 100
00093 
00094 namespace segwayrmp {
00095 
00099 typedef enum {
00103   can     = 0,
00107   usb     = 1,
00111   serial  = 2,
00116   ethernet  = 3,
00117   no_interface = -1
00118 } InterfaceType;
00119 
00123 typedef enum {
00127   rmp50   = 0,
00131   rmp100  = 1,
00135   rmp200  = 2,
00140   rmp400  = 3,
00144   rmpx440  = 4
00145 } SegwayRMPType;
00146 
00150 typedef enum {
00154   disabled   = 0,
00159   tractor    = 1,
00163   balanced   = 2,
00167   power_down = 3
00168 } OperationalMode;
00169 
00173 typedef enum {
00177   light = 0,
00182   tall  = 1,
00186   heavy = 2
00187 } ControllerGainSchedule;
00188 
00192 class SegwayTime
00193 {
00194 public:
00195   SegwayTime (uint32_t sec = 0, uint32_t nsec = 0) {
00196     this->sec = sec;
00197     this->nsec = nsec;
00198   }
00199   
00200   uint32_t sec; 
00201   uint32_t nsec; 
00202 };
00203 
00204 template<typename T>
00205 class FiniteConcurrentSharedQueue {
00206   std::queue<boost::shared_ptr<T> > queue_;
00207   boost::mutex mutex_;
00208   boost::condition_variable condition_variable_;
00209   size_t size_;
00210   bool canceled_;
00211 public:
00212   FiniteConcurrentSharedQueue(size_t size = 1024)
00213     : size_(size), canceled_(false) {}
00214   ~FiniteConcurrentSharedQueue() {}
00215   
00216   size_t size() {
00217     boost::mutex::scoped_lock(mutex_);
00218     return queue_.size();
00219   }
00220   
00221   bool empty() {
00222     return this->size() == 0;
00223   }
00224   
00225   bool enqueue(boost::shared_ptr<T> element) {
00226     bool dropped_element = false;
00227     {
00228       boost::lock_guard<boost::mutex> lock(mutex_);
00229       if (queue_.size() == size_) {
00230         queue_.pop();
00231         dropped_element = true;
00232       }
00233       queue_.push(element);
00234     }
00235     condition_variable_.notify_one();
00236     return dropped_element;
00237   }
00238   
00239   boost::shared_ptr<T> dequeue() {
00240     boost::unique_lock<boost::mutex> lock(mutex_);
00241     while (queue_.empty()) {
00242       condition_variable_.wait(lock);
00243       if (this->canceled_) {
00244         return boost::shared_ptr<T>();
00245       }
00246     }
00247     boost::shared_ptr<T> element = queue_.front();
00248     queue_.pop();
00249     return element;
00250   }
00251   
00252   void cancel() {
00253     {
00254       boost::lock_guard<boost::mutex> lock(mutex_);
00255       this->canceled_ = true;
00256     }
00257     condition_variable_.notify_all();
00258   }
00259   
00260   void reset() {
00261     {
00262       boost::lock_guard<boost::mutex> lock(mutex_);
00263       this->canceled_ = false;
00264     }
00265     condition_variable_.notify_all();
00266   }
00267 };
00268 
00269 // Forward declarations
00270 class RMPIO;
00271 class Packet;
00272 
00276 class SegwayStatus {
00277 public:
00278   SegwayTime timestamp; 
00279   float pitch; 
00280   float pitch_rate; 
00281   float roll; 
00282   float roll_rate; 
00283   float left_wheel_speed; 
00284   float right_wheel_speed; 
00285   float yaw_rate; 
00286   float servo_frames; 
00288   float integrated_left_wheel_position;
00290   float integrated_right_wheel_position;
00292   float integrated_forward_position;
00294   float integrated_turn_position;
00296   float left_motor_torque;
00298   float right_motor_torque;
00300   float ui_battery_voltage;
00302   float powerbase_battery_voltage;
00306   OperationalMode operational_mode;
00311   ControllerGainSchedule controller_gain_schedule;
00313   float commanded_velocity;
00315   float commanded_yaw_rate; 
00317   int motor_status;
00319   bool touched;
00320   
00321   SegwayStatus();
00322   
00323   std::string str();
00324 
00325   typedef boost::shared_ptr<SegwayStatus> Ptr;
00326 };
00327 
00328 typedef boost::function<void(SegwayStatus::Ptr)> SegwayStatusCallback;
00329 typedef boost::function<SegwayTime(void)> GetTimeCallback;
00330 typedef boost::function<void(const std::exception&)> ExceptionCallback;
00331 typedef boost::function<void(const std::string&)> LogMsgCallback;
00332 
00336 class SegwayRMP {
00337 public:
00345   SegwayRMP(InterfaceType interface_type = serial,
00346             SegwayRMPType segway_rmp_type = rmp200);
00347   ~SegwayRMP();
00348 
00357   void
00358   configureSerial(std::string port, int baudrate = 460800);
00359 
00369   void
00370   configureUSBBySerial(std::string serial_number,
00371                        int baudrate = 460800);
00372 
00382   void
00383   configureUSBByDescription(std::string description, int baudrate = 460800);
00384 
00395   void
00396   configureUSBByIndex(int device_index, int baudrate = 460800);
00397 
00403   void
00404   connect(bool reset_integrators = true);
00405 
00409   void
00410   shutdown();
00411 
00420   void
00421   moveCounts(short int linear_counts, short int angular_counts);
00422 
00432   void
00433   move(float linear_velocity, float angular_velocity);
00434 
00435   /************ Getter and Setters ************/
00436   
00442   void
00443   setOperationalMode(OperationalMode operational_mode);
00444    
00451   void
00452   setControllerGainSchedule(ControllerGainSchedule controller_gain_schedule);
00453   
00461   void
00462   setBalanceModeLocking(bool state = true);
00463   
00469   void
00470   resetAllIntegrators();
00471   
00480   void
00481   setMaxVelocityScaleFactor(double scalar = 1.0);
00482   
00491   void
00492   setMaxAccelerationScaleFactor(double scalar = 1.0);
00493   
00502   void
00503   setMaxTurnScaleFactor(double scalar = 1.0);
00504   
00513   void
00514   setCurrentLimitScaleFactor(double scalar = 1.0);
00515   
00561   void
00562   setStatusCallback(SegwayStatusCallback callback);
00563   
00613   void
00614   setLogMsgCallback(std::string log_level, LogMsgCallback callback);
00615   
00675   void
00676   setTimestampCallback(GetTimeCallback callback);
00677   
00728   void
00729   setExceptionCallback(ExceptionCallback callback);
00730 private:
00731   // Disable Copy Constructor
00732   void operator=(const SegwayRMP &);
00733   const SegwayRMP & operator=(SegwayRMP);
00734 
00735   // Interface implementation (pimpl idiom)
00736   RMPIO * rmp_io_;
00737 
00738   // Configuration Variables
00739   InterfaceType interface_type_;
00740   SegwayRMPType segway_rmp_type_;
00741 
00742   // Status Variables
00743   bool connected_;
00744   SegwayStatus::Ptr segway_status_;
00745 
00746   // Constants
00747   void SetConstantsBySegwayType_(SegwayRMPType &rmp_type);
00748   double dps_to_counts_;
00749   double mps_to_counts_;
00750   double meters_to_counts_;
00751   double rev_to_counts_;
00752   double torque_to_counts_;
00753 
00754   // Callbacks
00755   SegwayStatusCallback status_callback_;
00756   GetTimeCallback get_time_;
00757   LogMsgCallback debug_, info_, error_;
00758   ExceptionCallback handle_exception_;
00759   FiniteConcurrentSharedQueue<SegwayStatus> ss_queue_;
00760 
00761   // Continuous Read Functions and Variables
00762   void ReadContinuously_();
00763   void ExecuteCallbacks_();
00764   void StartReadingContinuously_();
00765   void StopReadingContinuously_();
00766   bool continuously_reading_;
00767   boost::thread read_thread_;
00768   boost::thread callback_execution_thread_;
00769 
00770   // Parsing Functions and Variables
00771   void ProcessPacket_(Packet &packet);
00772   bool ParsePacket_(Packet &packet, SegwayStatus::Ptr &ss_ptr);
00773 };
00774 
00775 DEFINE_EXCEPTION(NoHighPerformanceTimersException, "", "This system does not "
00776   "appear to have a High Precision Event Timer (HPET) device.");
00777 
00778 DEFINE_EXCEPTION(ConnectionFailedException, "Error connecting to the "
00779   "SegwayRMP: ", "Unspecified");
00780 
00781 DEFINE_EXCEPTION(ReadFailedException, "Error reading from the SegwayRMP: ",
00782   "Unspecified");
00783 
00784 DEFINE_EXCEPTION(WriteFailedException, "Error writing to the SegwayRMP: ",
00785   "Unspecified");
00786 
00787 DEFINE_EXCEPTION(MoveFailedException, "Error moving the SegwayRMP: ",
00788   "Unspecified");
00789 
00790 DEFINE_EXCEPTION(ConfigurationException, "Error configuring the SegwayRMP: ",
00791   "Unspecified");
00792 
00793 } // Namespace segwayrmp
00794 
00795 #endif


libsegwayrmp
Author(s): William Woodall
autogenerated on Mon Oct 6 2014 01:48:09