segwayrmp.h

Go to the documentation of this file.

#ifndef SEGWAYRMP_H
#define SEGWAYRMP_H

#include <exception>
#include <sstream>
#include <queue>
#include <typeinfo>

#include <boost/function.hpp>
#include <boost/thread.hpp>

/* Setup (u)int*_t types if not UNIX. */
#if defined(_WIN32) && !defined(__MINGW32__)
  typedef unsigned int uint32_t;
#else
# include <stdint.h>
#endif

// ClassName - The name of the exception class
// Prefix    - The common prefix
// Message   - The default message, if one is not specified at Throw
// The message takes the form of:
// ClassName occurred at line # of file `<file>`: Prefix+Message
#define DEFINE_EXCEPTION(ClassName, Prefix, Message) \
  class ClassName : public std::exception { \
    void operator=(const ClassName &); \
    const ClassName & operator=( ClassName ); \
    const char * what_; \
    const int id_; \
  public: \
    ClassName(const char * file, const int ln, \
              const char * msg = Message, const int id = -1) : id_(id) { \
      std::stringstream ss; \
      ss << #ClassName " occurred at line " << ln \
         << " of `" << file << "`: " << Prefix << msg; \
      what_ = ss.str().c_str(); \
    } \
    virtual const char* what () const throw () { return what_; } \
    const int error_number() { return this->id_; } \
  };

#define RMP_THROW(ExceptionClass) \
  throw ExceptionClass(__FILE__, __LINE__)

#define RMP_THROW_MSG(ExceptionClass, Message) \
  throw ExceptionClass(__FILE__, __LINE__, (Message) )

#define RMP_THROW_MSG_AND_ID(ExceptionClass, Message, Id) \
  throw ExceptionClass(__FILE__, __LINE__, (Message), (Id) )

#define MAX_SEGWAYSTATUS_QUEUE_SIZE 100

namespace segwayrmp {

typedef enum {
  can     = 0,
  usb     = 1,
  serial  = 2,
  ethernet  = 3,
  no_interface = -1
} InterfaceType;

typedef enum {
  rmp50   = 0,
  rmp100  = 1,
  rmp200  = 2,
  rmp400  = 3,
  rmpx440  = 4
} SegwayRMPType;

typedef enum {
  disabled   = 0,
  tractor    = 1,
  balanced   = 2,
  power_down = 3
} OperationalMode;

typedef enum {
  light = 0,
  tall  = 1,
  heavy = 2
} ControllerGainSchedule;

class SegwayTime
{
public:
  SegwayTime (uint32_t sec = 0, uint32_t nsec = 0) {
    this->sec = sec;
    this->nsec = nsec;
  }
  
  uint32_t sec; 
  uint32_t nsec; 
};

template<typename T>
class FiniteConcurrentSharedQueue {
  std::queue<boost::shared_ptr<T> > queue_;
  boost::mutex mutex_;
  boost::condition_variable condition_variable_;
  size_t size_;
  bool canceled_;
public:
  FiniteConcurrentSharedQueue(size_t size = 1024)
    : size_(size), canceled_(false) {}
  ~FiniteConcurrentSharedQueue() {}
  
  size_t size() {
    boost::mutex::scoped_lock(mutex_);
    return queue_.size();
  }
  
  bool empty() {
    return this->size() == 0;
  }
  
  bool enqueue(boost::shared_ptr<T> element) {
    bool dropped_element = false;
    {
      boost::lock_guard<boost::mutex> lock(mutex_);
      if (queue_.size() == size_) {
        queue_.pop();
        dropped_element = true;
      }
      queue_.push(element);
    }
    condition_variable_.notify_one();
    return dropped_element;
  }
  
  boost::shared_ptr<T> dequeue() {
    boost::unique_lock<boost::mutex> lock(mutex_);
    while (queue_.empty()) {
      condition_variable_.wait(lock);
      if (this->canceled_) {
        return boost::shared_ptr<T>();
      }
    }
    boost::shared_ptr<T> element = queue_.front();
    queue_.pop();
    return element;
  }
  
  void cancel() {
    {
      boost::lock_guard<boost::mutex> lock(mutex_);
      this->canceled_ = true;
    }
    condition_variable_.notify_all();
  }
  
  void reset() {
    {
      boost::lock_guard<boost::mutex> lock(mutex_);
      this->canceled_ = false;
    }
    condition_variable_.notify_all();
  }
};

// Forward declarations
class RMPIO;
class Packet;

class SegwayStatus {
public:
  SegwayTime timestamp; 
  float pitch; 
  float pitch_rate; 
  float roll; 
  float roll_rate; 
  float left_wheel_speed; 
  float right_wheel_speed; 
  float yaw_rate; 
  float servo_frames; 
  float integrated_left_wheel_position;
  float integrated_right_wheel_position;
  float integrated_forward_position;
  float integrated_turn_position;
  float left_motor_torque;
  float right_motor_torque;
  float ui_battery_voltage;
  float powerbase_battery_voltage;
  OperationalMode operational_mode;
  ControllerGainSchedule controller_gain_schedule;
  float commanded_velocity;
  float commanded_yaw_rate; 
  int motor_status;
  bool touched;
  
  SegwayStatus();
  
  std::string str();

  typedef boost::shared_ptr<SegwayStatus> Ptr;
};

typedef boost::function<void(SegwayStatus::Ptr)> SegwayStatusCallback;
typedef boost::function<SegwayTime(void)> GetTimeCallback;
typedef boost::function<void(const std::exception&)> ExceptionCallback;
typedef boost::function<void(const std::string&)> LogMsgCallback;

class SegwayRMP {
public:
  SegwayRMP(InterfaceType interface_type = serial,
            SegwayRMPType segway_rmp_type = rmp200);
  ~SegwayRMP();

  void
  configureSerial(std::string port, int baudrate = 460800);

  void
  configureUSBBySerial(std::string serial_number,
                       int baudrate = 460800);

  void
  configureUSBByDescription(std::string description, int baudrate = 460800);

  void
  configureUSBByIndex(int device_index, int baudrate = 460800);

  void
  connect(bool reset_integrators = true);

  void
  shutdown();

  void
  moveCounts(short int linear_counts, short int angular_counts);

  void
  move(float linear_velocity, float angular_velocity);

  /************ Getter and Setters ************/
  
  void
  setOperationalMode(OperationalMode operational_mode);
   
  void
  setControllerGainSchedule(ControllerGainSchedule controller_gain_schedule);
  
  void
  setBalanceModeLocking(bool state = true);
  
  void
  resetAllIntegrators();
  
  void
  setMaxVelocityScaleFactor(double scalar = 1.0);
  
  void
  setMaxAccelerationScaleFactor(double scalar = 1.0);
  
  void
  setMaxTurnScaleFactor(double scalar = 1.0);
  
  void
  setCurrentLimitScaleFactor(double scalar = 1.0);
  
  void
  setStatusCallback(SegwayStatusCallback callback);
  
  void
  setLogMsgCallback(std::string log_level, LogMsgCallback callback);
  
  void
  setTimestampCallback(GetTimeCallback callback);
  
  void
  setExceptionCallback(ExceptionCallback callback);
private:
  // Disable Copy Constructor
  void operator=(const SegwayRMP &);
  const SegwayRMP & operator=(SegwayRMP);

  // Interface implementation (pimpl idiom)
  RMPIO * rmp_io_;

  // Configuration Variables
  InterfaceType interface_type_;
  SegwayRMPType segway_rmp_type_;

  // Status Variables
  bool connected_;
  SegwayStatus::Ptr segway_status_;

  // Constants
  void SetConstantsBySegwayType_(SegwayRMPType &rmp_type);
  double dps_to_counts_;
  double mps_to_counts_;
  double meters_to_counts_;
  double rev_to_counts_;
  double torque_to_counts_;

  // Callbacks
  SegwayStatusCallback status_callback_;
  GetTimeCallback get_time_;
  LogMsgCallback debug_, info_, error_;
  ExceptionCallback handle_exception_;
  FiniteConcurrentSharedQueue<SegwayStatus> ss_queue_;

  // Continuous Read Functions and Variables
  void ReadContinuously_();
  void ExecuteCallbacks_();
  void StartReadingContinuously_();
  void StopReadingContinuously_();
  bool continuously_reading_;
  boost::thread read_thread_;
  boost::thread callback_execution_thread_;

  // Parsing Functions and Variables
  void ProcessPacket_(Packet &packet);
  bool ParsePacket_(Packet &packet, SegwayStatus::Ptr &ss_ptr);
};

DEFINE_EXCEPTION(NoHighPerformanceTimersException, "", "This system does not "
  "appear to have a High Precision Event Timer (HPET) device.");

DEFINE_EXCEPTION(ConnectionFailedException, "Error connecting to the "
  "SegwayRMP: ", "Unspecified");

DEFINE_EXCEPTION(ReadFailedException, "Error reading from the SegwayRMP: ",
  "Unspecified");

DEFINE_EXCEPTION(WriteFailedException, "Error writing to the SegwayRMP: ",
  "Unspecified");

DEFINE_EXCEPTION(MoveFailedException, "Error moving the SegwayRMP: ",
  "Unspecified");

DEFINE_EXCEPTION(ConfigurationException, "Error configuring the SegwayRMP: ",
  "Unspecified");

} // Namespace segwayrmp

#endif