console.h
Go to the documentation of this file.
00001 #ifndef __THREEMXL_CONSOLE_H
00002 #define __THREEMXL_CONSOLE_H
00003 
00004 #include <ros/ros.h>
00005 #include <threemxl/platform/hardware/dynamixel/CDxlGeneric.h>
00006 
00007 class DxlROSConsole;
00008 
00010 class DxlROSCommand
00011 {
00012   public:
00013         typedef std::vector<std::string> ArgList;
00014 
00015   protected:
00016         DxlROSConsole *console_; 
00017         size_t nargs_;       
00018         std::string name_;   
00019         std::string help_;   
00020 
00021   public:
00022         DxlROSCommand(DxlROSConsole *console, std::string name, size_t nargs, std::string help) :
00023                 console_(console), nargs_(nargs), name_(name), help_(help)
00024         {
00025         }
00026         
00028         bool match(std::string cmd, ArgList args)
00029         {
00030           return cmd == name_ && args.size() == nargs_;
00031         }
00032         
00034         bool matchHelp(std::string cmd, ArgList args)
00035         {
00036           return cmd == name_;
00037         }
00038 
00040         std::string getHelp()
00041         {
00042           return help_;
00043         }
00044 
00046         bool execute(ArgList args);
00047 };
00048 
00049 class Lockable
00050 {
00051   protected:
00052     pthread_mutex_t mutex_; 
00053     pthread_cond_t condition_; 
00054 
00055   public:
00056     Lockable()
00057     {
00058       pthread_mutex_init(&mutex_, NULL);
00059       pthread_cond_init(&condition_, NULL);
00060     }
00061     
00062     virtual ~Lockable()
00063     {
00064       pthread_mutex_destroy(&mutex_);
00065       pthread_cond_destroy(&condition_);
00066     }
00067   
00069     void lock() { pthread_mutex_lock(&mutex_); }
00070     
00072     void unlock() { pthread_mutex_unlock(&mutex_); }
00073     
00075 
00080     void wait(double interval);
00081     
00083     /* \note Must be locked. */
00084     void signal() { pthread_cond_signal(&condition_); }
00085 };
00086 
00088 class DxlROSConsole : public Lockable
00089 {
00090   public:
00091     typedef std::vector<DxlROSCommand> CommandList;
00092     typedef std::vector<CDxlGeneric*>  MotorList;
00093         
00094   protected:
00095     ros::NodeHandle nh_;   
00096     CDxlGeneric *motor_;   
00097     MotorList motors_;     
00098     LxSerial serial_port_; 
00099     CommandList commands_; 
00100     char *path_;           
00101     pthread_t hb_thread_;  
00102     double hb_interval_;   
00103 
00104   public:
00106     DxlROSConsole() : nh_("~"), motor_(NULL), path_(NULL), hb_interval_(0)
00107     {
00108       // Spawn heartbeat thread
00109       pthread_create(&hb_thread_, NULL, spin_hb, (void*)this);
00110     }
00111     
00113 
00114     ~DxlROSConsole()
00115     {
00116       // Prod heartbeat thread. Does not actually work if ros::ok() is true
00117       lock();
00118       signal();
00119       unlock();
00120       
00121       // Wait for heartbeat thread to finish
00122       pthread_join(hb_thread_, NULL);
00123     
00124       for (size_t ii=0; ii < motors_.size(); ++ii)
00125         delete motors_[ii];
00126         
00127       if (serial_port_.is_port_open())
00128         serial_port_.port_close();
00129           
00130       nh_.shutdown();
00131     }
00132     
00134     CDxlGeneric *getMotor() { return motor_; }
00135     
00137     CDxlGeneric *createMotor();
00138     
00140 
00144     void setMotor(int id);
00145     
00147     MotorList &getMotors() { return motors_; }
00148     
00150 
00154     void setHeartbeatInterval(double interval);
00155 
00157 
00158     void init(char *path);
00159     
00161 
00165     void execute(std::string cmd);
00166     
00168 
00169     void spin();
00170     
00172 
00173     static void *spin_hb(void *obj);
00174 };    
00175 
00176 #endif /* __THREEMXL_CONSOLE_H */


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