controller.h
Go to the documentation of this file.
00001 
00025 #ifndef ROBOTEQ_CONTROLLER
00026 #define ROBOTEQ_CONTROLLER
00027 
00028 #include "ros/ros.h"
00029 
00030 #include <boost/thread/condition_variable.hpp>
00031 #include <boost/lexical_cast.hpp>
00032 #include <stdint.h>
00033 #include <string>
00034 
00035 namespace serial {
00036   class Serial;
00037 }
00038 
00039 namespace roboteq {
00040 
00041 class Channel;
00042 
00043 class Controller {
00044   friend class Channel;
00045 
00046 private :
00047   const char *port_;
00048   int baud_;
00049   bool connected_;
00050   std::string version_;
00051   serial::Serial *serial_;
00052   std::stringstream tx_buffer_;
00053   std::vector<Channel*> channels_;
00054 
00055   ros::NodeHandle nh_;
00056   ros::Publisher pub_status_;
00057 
00058   void read();
00059   void write(std::string);
00060   
00061   void processStatus(std::string msg);
00062   void processFeedback(std::string msg);
00063 
00064 protected:
00065   // These data members are the core of the synchronization strategy in this class.
00066   // In short, the sendWaitAck method blocks on receiving an ack, which is passed to
00067   // it from the read thread using the last_response_ string.
00068   std::string last_response_;
00069   boost::mutex last_response_mutex_;
00070   boost::condition_variable last_response_available_;
00071   bool haveLastResponse() { return !last_response_.empty(); }
00072 
00073   // These track our progress in attempting to initialize the controller.
00074   uint8_t start_script_attempts_;
00075 
00076   class EOMSend {};
00077 
00078   class MessageSender {
00079     public:
00080     MessageSender(std::string init, Controller* interface)
00081         : init_(init), interface_(interface) {}
00082 
00083     template<typename T>
00084     MessageSender& operator<<(const T val) {
00085       if (ss.tellp() == 0) {
00086         ss << init_ << val;
00087       } else {
00088         ss << ' ' << val;
00089       }
00090       return *this;
00091     }
00092  
00093     void operator<<(EOMSend) 
00094     {
00095       interface_->write(ss.str());
00096       ss.str("");
00097     }
00098    
00099     private:
00100     std::string init_;
00101     Controller* interface_;
00102     std::stringstream ss;
00103   };
00104 
00105   MessageSender command;
00106   MessageSender query;
00107   MessageSender param;
00108   EOMSend send, sendVerify;
00109  
00110 public :
00111   Controller (const char *port, int baud);
00112   ~Controller();
00113 
00114   void addChannel(Channel* channel);
00115   void connect();
00116   bool connected() { return connected_; }
00117   void spinOnce() { read(); }
00118   void flush();
00119 
00120   // Send commands to motor driver.
00121   void setEstop() { command << "EX" << send; }
00122   void resetEstop() { command << "MG" << send; }
00123   void resetDIOx(int i) { command << "D0" << i << send; }
00124   void setDIOx(int i) { command << "D1" << i << send; }
00125   void startScript() { command << "R" << send; }
00126   void stopScript() { command << "R" << 0 << send; }
00127   void setUserVariable(int var, int val) { command << "VAR" << var << val << send; }
00128   void setUserBool(int var, bool val) { command << "B" << var << (val ? 1 : 0) << send; }
00129   bool downloadScript();
00130 
00131   int setSerialEcho(bool serial_echo) {
00132     param << "ECHOF" << (serial_echo ? 0 : 1) << sendVerify; }
00133 };
00134 
00135 }
00136 
00137 #endif


roboteq_driver
Author(s):
autogenerated on Fri Feb 12 2016 00:10:33